Integrate SampleTankDrive to all Opmodes.

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<Double> getWheelPositions();
+    List<Double> 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;
+    }
+
+
+}
+

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<Double> 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);

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<Double> 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);

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();
 

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)

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;
 

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);
 

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..
         }
+
     }
 }

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();
 

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);