From b779a61a7cb0a2d46fa63ce2acb1346541a88224 Mon Sep 17 00:00:00 2001 From: Carlos Date: Sun, 30 Jun 2024 09:57:10 -0700 Subject: [PATCH] Updating to use Roadrunner --- .../ftc/teamcode/CometBotsLinearOpMode.java | 10 +- .../teamcode/CometBotsMecanumDriveMode.java | 132 ++++++++++++++++++ .../ftc/teamcode/MecanumDrive.java | 47 ++++--- .../ftc/teamcode/cometbots/Constants.java | 5 + 4 files changed, 173 insertions(+), 21 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsMecanumDriveMode.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java index f609a65d6..0d8054ebf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java @@ -29,6 +29,8 @@ package org.firstinspires.ftc.teamcode; +import static org.firstinspires.ftc.teamcode.cometbots.Constants.*; + import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -85,10 +87,10 @@ public class CometBotsLinearOpMode extends LinearOpMode { // Initialize the hardware variables. Note that the strings used here must correspond // to the names assigned during the robot configuration step on the DS or RC devices. - rhw = hardwareMap.get(DcMotor.class, "right hand wheel"); - rlw = hardwareMap.get(DcMotor.class, "right leg wheel"); - lhw = hardwareMap.get(DcMotor.class, "left hand wheel"); - llw = hardwareMap.get(DcMotor.class, "left leg wheel"); + rhw = hardwareMap.get(DcMotor.class, RT_FRONT); + rlw = hardwareMap.get(DcMotor.class, RT_BACK); + lhw = hardwareMap.get(DcMotor.class, LT_FRONT); + llw = hardwareMap.get(DcMotor.class, LT_BACK); /* Put all motors in an array for easier handling diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsMecanumDriveMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsMecanumDriveMode.java new file mode 100644 index 000000000..69a3c81eb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsMecanumDriveMode.java @@ -0,0 +1,132 @@ +/* Copyright (c) 2021 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import static org.firstinspires.ftc.teamcode.cometbots.Constants.LT_BACK; +import static org.firstinspires.ftc.teamcode.cometbots.Constants.LT_FRONT; +import static org.firstinspires.ftc.teamcode.cometbots.Constants.RT_BACK; +import static org.firstinspires.ftc.teamcode.cometbots.Constants.RT_FRONT; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="CometBots Mecanum Drive Demo", group = "[14493] CometBots") +public class CometBotsMecanumDriveMode extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor rhw = null; + private DcMotor rlw = null; + private DcMotor lhw = null; + private DcMotor llw = null; + + @Override + public void runOpMode() { + + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,Math.toRadians(90))); + + // Button A counter to cycle speeds + int cntBtnA = 1; + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + rhw = hardwareMap.get(DcMotor.class, RT_FRONT); + rlw = hardwareMap.get(DcMotor.class, RT_BACK); + lhw = hardwareMap.get(DcMotor.class, LT_FRONT); + llw = hardwareMap.get(DcMotor.class, LT_BACK); + + /* + Put all motors in an array for easier handling + + NOTE: Placement of motor in array is important especially when dealing with functions + that set direction + */ + DcMotor[] motors = {rhw, rlw, lhw, llw}; + + /* + Initialize all wheels forward using motors a + */ + MovementLibrary.setDirectionForward(motors); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + drive.setDrivePowers( + new PoseVelocity2d( + new Vector2d( + gamepad1.right_stick_x, + gamepad1.right_stick_y + ), + -gamepad1.right_stick_x + ) + ); + drive.updatePoseEstimate(); + } + }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 914e2d6b2..f642e75ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode; +import static org.firstinspires.ftc.teamcode.cometbots.Constants.*; + import androidx.annotation.NonNull; import com.acmerobotics.dashboard.canvas.Canvas; @@ -41,6 +43,7 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.cometbots.MovementLibrary; import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage; import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage; import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage; @@ -58,19 +61,19 @@ public final class MecanumDrive { // TODO: fill in these values based on // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = - RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.LogoFacingDirection.FORWARD; public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + RevHubOrientationOnRobot.UsbFacingDirection.UP; // drive model parameters - public double inPerTick = 1; - public double lateralInPerTick = inPerTick; - public double trackWidthTicks = 0; + public double inPerTick = 119/5924.5; + public double lateralInPerTick = 124/6930.5; + public double trackWidthTicks = 1131; // feedforward parameters (in tick units) - public double kS = 0; - public double kV = 0; - public double kA = 0; + public double kS = 0.7476; + public double kV = 0.0043736; + public double kA = 00055; // path profile parameters (in inches) public double maxWheelVel = 50; @@ -82,9 +85,9 @@ public final class MecanumDrive { public double maxAngAccel = Math.PI; // path controller gains - public double axialGain = 0.0; - public double lateralGain = 0.0; - public double headingGain = 0.0; // shared with turn + public double axialGain = 4.125; + public double lateralGain = 4.125; + public double headingGain = 4.25; // shared with turn public double axialVelGain = 0.0; public double lateralVelGain = 0.0; @@ -217,18 +220,28 @@ public final class MecanumDrive { // TODO: make sure your config has motors with these names (or change them) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); - rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, LT_FRONT); + leftBack = hardwareMap.get(DcMotorEx.class, LT_BACK); + rightBack = hardwareMap.get(DcMotorEx.class, RT_BACK); + rightFront = hardwareMap.get(DcMotorEx.class, RT_FRONT); leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - // TODO: reverse motor directions if needed - // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + /* + Put all motors in an array for easier handling + + NOTE: Placement of motor in array is important especially when dealing with functions + that set direction + */ + DcMotor[] motors = {rightFront, rightBack, leftFront, leftBack}; + + /* + Initialize all wheels forward using motors a + */ + MovementLibrary.setDirectionForward(motors); // TODO: make sure your config has an IMU with this name (can be BNO or BHI) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/Constants.java index a832d0a65..c57202437 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/Constants.java @@ -4,4 +4,9 @@ public class Constants { public static double CB_BASE_SPEED = 0.2; + public static String RT_FRONT = "right hand wheel"; + public static String LT_FRONT = "left hand wheel"; + public static String RT_BACK = "right leg wheel"; + public static String LT_BACK = "left leg wheel"; + }