diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/Drive.java new file mode 100644 index 00000000000..4a2cc29715e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/Drive.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.drive; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.Localizer; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; + +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; + +import java.util.List; + +public interface Drive { + Pose2d getPoseEstimate(); + void updatePoseEstimate(); + void setPoseEstimate(Pose2d value); + + TrajectoryBuilder trajectoryBuilder(Pose2d startPose); + TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed); + TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading); + + TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose); + + void turnAsync(double angle); + void turn(double angle); + + void followTrajectoryAsync(Trajectory trajectory); + void followTrajectory(Trajectory trajectory); + void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence); + void followTrajectorySequence(TrajectorySequence trajectorySequence); + Pose2d getLastError(); + + void update(); + + void waitForIdle(); + + boolean isBusy(); + + void setMode(DcMotor.RunMode runMode); + + void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior); + void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients); + void setWeightedDrivePower(Pose2d drivePower); + + List getWheelPositions(); + List getWheelVelocities(); + Pose2d getPoseVelocity(); + + Localizer getLocalizer(); + + void setDrivePower(Pose2d drivePower ); + void setMotorPowers(double v, double v1); + void setMotorPowers(double v, double v1, double v2, double v3); + double getRawExternalHeading(); + Double getExternalHeadingVelocity(); + + static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVelocity, double trackWidth) { + return null; + } + static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { + return null; + } + + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java index fcd18aed3cc..00123912eac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java @@ -49,11 +49,12 @@ import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; + /* * Simple mecanum drive hardware implementation for REV hardware. */ @Config -public class SampleMecanumDrive extends MecanumDrive { +public class SampleMecanumDrive extends MecanumDrive implements Drive { public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); @@ -277,9 +278,15 @@ public List getWheelVelocities() { lastEncVels.add(vel); wheelVelocities.add(encoderTicksToInches(vel)); } + return wheelVelocities; } + @Override + public void setMotorPowers(double v, double v1) { + + } + @Override public void setMotorPowers(double v, double v1, double v2, double v3) { leftFront.setPower(v); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java index 55aeb3a5216..6aee4b99298 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java @@ -53,7 +53,7 @@ * Simple tank drive hardware implementation for REV hardware. */ @Config -public class SampleTankDrive extends TankDrive { +public class SampleTankDrive extends TankDrive implements Drive { public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0); public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0); public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); @@ -272,6 +272,8 @@ public List getWheelVelocities() { return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); } + + @Override public void setMotorPowers(double v, double v1) { for (DcMotorEx leftMotor : leftMotors) { @@ -282,6 +284,10 @@ public void setMotorPowers(double v, double v1) { } } + @Override + public void setMotorPowers(double v, double v1, double v2, double v3) { + } + @Override public double getRawExternalHeading() { return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java index 8e0c62140fa..7949dc18577 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java @@ -15,7 +15,9 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import org.firstinspires.ftc.teamcode.util.LoggingUtil; import org.firstinspires.ftc.teamcode.util.RegressionUtil; @@ -38,6 +40,8 @@ public class AutomaticFeedforwardTuner extends LinearOpMode { public static double MAX_POWER = 0.7; public static double DISTANCE = 100; // in + private Drive drive; + @Override public void runOpMode() throws InterruptedException { if (RUN_USING_ENCODER) { @@ -47,7 +51,11 @@ public void runOpMode() throws InterruptedException { Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } NanoClock clock = NanoClock.system(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java index a78ad3796bd..6df8a2648d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java @@ -6,7 +6,9 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; /* * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base @@ -29,10 +31,15 @@ public class BackAndForth extends LinearOpMode { public static double DISTANCE = 50; + private Drive drive; @Override public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d()) .forward(DISTANCE) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java index 931b742083a..abf6c7bb49a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java @@ -20,7 +20,9 @@ import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import java.util.List; @@ -64,6 +66,8 @@ private static MotionProfile generateProfile(boolean movingForward) { return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); } + private Drive drive; + @Override public void runOpMode() { if (!RUN_USING_ENCODER) { @@ -73,7 +77,11 @@ public void runOpMode() { Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } Mode mode = Mode.TUNING_MODE; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java index 63f577e1698..16f00668e2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java @@ -5,7 +5,9 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; /* @@ -26,9 +28,14 @@ public class FollowerPIDTuner extends LinearOpMode { public static double DISTANCE = 48; // in + private Drive drive; @Override public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java index 8411792bda9..849122f0ef9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java @@ -5,7 +5,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; /** * This is a simple teleop routine for testing localization. Drive the robot around like a normal @@ -16,30 +18,39 @@ */ @TeleOp(group = "drive") public class LocalizationTest extends LinearOpMode { + private Drive drive; + @Override public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - waitForStart(); - - while (!isStopRequested()) { - drive.setWeightedDrivePower( - new Pose2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x, - -gamepad1.right_stick_x - ) - ); - - drive.update(); - - Pose2d poseEstimate = drive.getPoseEstimate(); - telemetry.addData("x", poseEstimate.getX()); - telemetry.addData("y", poseEstimate.getY()); - telemetry.addData("heading", poseEstimate.getHeading()); - telemetry.update(); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + waitForStart(); + + while (!isStopRequested()) { + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + + drive.update(); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("x", poseEstimate.getX()); + telemetry.addData("y", poseEstimate.getY()); + telemetry.addData("heading", poseEstimate.getHeading()); + telemetry.update(); + } + + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + + // code for tank drive teleop here.. } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java index 0d01bcb9615..a4acf409327 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java @@ -23,8 +23,10 @@ import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import java.util.Objects; @@ -50,7 +52,7 @@ public class ManualFeedforwardTuner extends LinearOpMode { private FtcDashboard dashboard = FtcDashboard.getInstance(); - private SampleMecanumDrive drive; + private Drive drive; enum Mode { DRIVER_MODE, @@ -74,7 +76,11 @@ public void runOpMode() { Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry()); - drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } final VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java index 05d82657803..0897952b350 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java @@ -10,7 +10,9 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import java.util.Objects; @@ -30,9 +32,15 @@ public class MaxAngularVeloTuner extends LinearOpMode { private ElapsedTime timer; private double maxAngVelocity = 0.0; + private Drive drive; + @Override public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java index ddca6cd1a4c..166f3b9945a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java @@ -11,8 +11,10 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import java.util.Objects; @@ -33,11 +35,15 @@ public class MaxVelocityTuner extends LinearOpMode { private double maxVelocity = 0.0; private VoltageSensor batteryVoltageSensor; + private Drive drive; @Override public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java index 023cfccc382..67c9a666df4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java @@ -8,7 +8,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; /** * This is a simple teleop routine for debugging your motor configuration. @@ -43,12 +45,17 @@ @TeleOp(group = "drive") public class MotorDirectionDebugger extends LinearOpMode { public static double MOTOR_POWER = 0.7; + private Drive drive; @Override public void runOpMode() throws InterruptedException { Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } telemetry.addLine("Press play to begin the debugging opmode"); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java index d7316766a18..95c29c96adc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java @@ -6,16 +6,25 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; /* * This is an example of a more complex path to really test the tuning. */ @Autonomous(group = "drive") public class SplineTest extends LinearOpMode { + + private Drive drive; + @Override public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } waitForStart(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java index 992d03aea1d..c61d5b71ff2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java @@ -9,7 +9,9 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; /* * This is a simple routine to test translational drive capabilities. @@ -18,12 +20,17 @@ @Autonomous(group = "drive") public class StrafeTest extends LinearOpMode { public static double DISTANCE = 60; // in + private Drive drive; @Override public void runOpMode() throws InterruptedException { Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) .strafeRight(DISTANCE) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java index e1b27e1c9a6..08e0e10c972 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java @@ -7,9 +7,12 @@ import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; /* * This is a simple routine to test translational drive capabilities. @@ -19,11 +22,19 @@ public class StraightTest extends LinearOpMode { public static double DISTANCE = 60; // in + private Drive drive; + @Override public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) .forward(DISTANCE) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java index ffce0233c67..1162df843f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java @@ -11,8 +11,10 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; /* * This routine determines the effective track width. The procedure works by executing a point turn @@ -29,12 +31,17 @@ public class TrackWidthTuner extends LinearOpMode { public static double ANGLE = 180; // deg public static int NUM_TRIALS = 5; public static int DELAY = 1000; // ms + private Drive drive; @Override public void runOpMode() throws InterruptedException { Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } // TODO: if you haven't already, set the localizer to something that doesn't depend on // drive encoders for computing the heading diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java index ebe97f28b0d..ea6e5c4c506 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java @@ -12,7 +12,9 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; /** @@ -40,12 +42,17 @@ public class TrackingWheelForwardOffsetTuner extends LinearOpMode { public static double ANGLE = 180; // deg public static int NUM_TRIALS = 5; public static int DELAY = 1000; // ms + private Drive drive; @Override public void runOpMode() throws InterruptedException { Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java index a11059c4598..1875e2655d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -7,7 +7,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.RobotLog; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; /** @@ -65,10 +67,15 @@ @TeleOp(group = "drive") public class TrackingWheelLateralDistanceTuner extends LinearOpMode { public static int NUM_TURNS = 10; + private Drive drive; @Override public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TuningOpModes.java new file mode 100644 index 00000000000..a53018c7bee --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TuningOpModes.java @@ -0,0 +1,15 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.roadrunner.drive.Drive; +import com.acmerobotics.roadrunner.drive.TankDrive; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; + +public class TuningOpModes { + + // TODO: change this to TankDrive.class if you're using tank + public static final Class DRIVE_CLASS = SampleMecanumDrive.class; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java index 0637d19efea..0dda0b28910 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java @@ -4,7 +4,9 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.drive.Drive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; /* * This is a simple routine to test turning capabilities. @@ -13,10 +15,15 @@ @Autonomous(group = "drive") public class TurnTest extends LinearOpMode { public static double ANGLE = 90; // deg + private Drive drive; @Override public void runOpMode() throws InterruptedException { - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + if (TuningOpModes.DRIVE_CLASS == SampleMecanumDrive.class ) { + drive = new SampleMecanumDrive(hardwareMap); + }else if (TuningOpModes.DRIVE_CLASS == SampleTankDrive.class) { + drive = new SampleTankDrive(hardwareMap); + } waitForStart();