WIP bobito tunings

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExample.java

@@ -0,0 +1,122 @@
+package org.firstinspires.ftc.teamcode;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "AutoExample", group = "Autonomous Pathing Tuning")
+public class AutoExample extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(12.0, 11, 90);
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.6);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(12.804, 11.223, Point.CARTESIAN),
+                                new Point(12.804, 42.362, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation()
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(12.804, 42.362, Point.CARTESIAN),
+                                new Point(11.381, 57.379, Point.CARTESIAN),
+                                new Point(31.614, 56.588, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation()
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(31.614, 56.588, Point.CARTESIAN),
+                                new Point(51.214, 56.746, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation()
+                .addPath(
+                        // Line 4
+                        new BezierCurve(
+                                new Point(51.214, 56.746, Point.CARTESIAN),
+                                new Point(64.334, 58.643, Point.CARTESIAN),
+                                new Point(61.172, 45.524, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation()
+                .addPath(
+                        // Line 5
+                        new BezierCurve(
+                                new Point(61.172, 45.524, Point.CARTESIAN),
+                                new Point(36.198, 26.239, Point.CARTESIAN),
+                                new Point(19.759, 11.065, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation()
+                .build();
+
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path);
+        }
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleSeason2025V1.java

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
@@ -8,26 +9,41 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 
-@Autonomous(name = "Pre Loaded Blue Basket Auto", group = "Competition")
-public class PreLoadedBlueBasketAuto extends OpMode {
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning")
+public class AutoExampleSeason2025V1 extends OpMode {
     private Telemetry telemetryA;
 
     private Follower follower;
 
     private PathChain path;
 
-    private final Pose startPose = new Pose(7.875, 89.357);
+    private final Pose startPose = new Pose(15.0, 35, 90);
 
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
     @Override
     public void init() {
         follower = new Follower(hardwareMap);
 
-        follower.setMaxPower(.45);
+        follower.setMaxPower(.375);
 
         follower.setStartingPose(startPose);
 
@@ -35,88 +51,92 @@ public class PreLoadedBlueBasketAuto extends OpMode {
                 .addPath(
                         // Line 1
                         new BezierLine(
-                                new Point(8.036, 89.196, Point.CARTESIAN),
-                                new Point(10.125, 126.804, Point.CARTESIAN)
+                                new Point(15.000, 35.000, Point.CARTESIAN),
+                                new Point(60.000, 35.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(90))
                 .addPath(
                         // Line 2
-                        new BezierCurve(
-                                new Point(10.125, 126.804, Point.CARTESIAN),
-                                new Point(37.607, 90.000, Point.CARTESIAN),
-                                new Point(62.357, 119.893, Point.CARTESIAN)
+                        new BezierLine(
+                                new Point(60.000, 35.000, Point.CARTESIAN),
+                                new Point(60.000, 25.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(90))
                 .addPath(
                         // Line 3
-                        new BezierCurve(
-                                new Point(62.357, 119.893, Point.CARTESIAN),
-                                new Point(33.750, 112.500, Point.CARTESIAN),
-                                new Point(15.107, 130.661, Point.CARTESIAN)
+                        new BezierLine(
+                                new Point(60.000, 25.000, Point.CARTESIAN),
+                                new Point(15.000, 25.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(90))
                 .addPath(
                         // Line 4
-                        new BezierCurve(
-                                new Point(15.107, 130.661, Point.CARTESIAN),
-                                new Point(58.821, 103.018, Point.CARTESIAN),
-                                new Point(59.625, 126.964, Point.CARTESIAN)
+                        new BezierLine(
+                                new Point(15.000, 25.000, Point.CARTESIAN),
+                                new Point(60.000, 25.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(90))
                 .addPath(
                         // Line 5
                         new BezierLine(
-                                new Point(59.625, 126.964, Point.CARTESIAN),
-                                new Point(15.107, 130.339, Point.CARTESIAN)
+                                new Point(60.000, 25.000, Point.CARTESIAN),
+                                new Point(60.000, 15.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(90))
                 .addPath(
                         // Line 6
                         new BezierLine(
-                                new Point(15.107, 130.339, Point.CARTESIAN),
-                                new Point(59.625, 126.964, Point.CARTESIAN)
+                                new Point(60.000, 15.000, Point.CARTESIAN),
+                                new Point(15.000, 15.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(90))
                 .addPath(
                         // Line 7
                         new BezierLine(
-                                new Point(59.625, 126.964, Point.CARTESIAN),
-                                new Point(57.857, 133.071, Point.CARTESIAN)
+                                new Point(15.000, 15.000, Point.CARTESIAN),
+                                new Point(60.000, 15.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(90))
                 .addPath(
                         // Line 8
                         new BezierLine(
-                                new Point(57.857, 133.071, Point.CARTESIAN),
-                                new Point(18.964, 134.679, Point.CARTESIAN)
+                                new Point(60.000, 15.000, Point.CARTESIAN),
+                                new Point(60.000, 8.000, Point.CARTESIAN)
                         )
                 )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .setConstantHeadingInterpolation(Math.toRadians(90))
                 .addPath(
                         // Line 9
-                        new BezierCurve(
-                                new Point(18.964, 134.679, Point.CARTESIAN),
-                                new Point(84.536, 131.786, Point.CARTESIAN),
-                                new Point(80.036, 96.429, Point.CARTESIAN)
+                        new BezierLine(
+                                new Point(60.000, 8.000, Point.CARTESIAN),
+                                new Point(15.000, 8.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
+
         follower.followPath(path);
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
         telemetryA.update();
     }
 
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
     @Override
     public void loop() {
         follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path);
+        }
         follower.telemetryDebug(telemetryA);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java

@@ -0,0 +1,80 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "AutoExampleThree", group = "Autonomous Pathing Tuning")
+public class AutoExampleThree extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(10.0, 40, 90);
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.4);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                new Point(10.000, 20.000, Point.CARTESIAN),
+                                new Point(29.089, 61.232, Point.CARTESIAN),
+                                new Point(48.054, 19.607, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
+
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path);
+        }
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleTwo.java

@@ -0,0 +1,95 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "AutoExampleTwo", group = "Autonomous Pathing Tuning")
+public class AutoExampleTwo extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(10.0, 40, 90);
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.4);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(10.000, 40.000, Point.CARTESIAN),
+                                new Point(60.000, 40.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(60.000, 40.000, Point.CARTESIAN),
+                                new Point(60.000, 25.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(60.000, 25.000, Point.CARTESIAN),
+                                new Point(10.000, 25.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
+
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path);
+        }
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java

@@ -27,10 +27,8 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-package org.firstinspires.ftc.teamcode.cometbots.projects;
+package org.firstinspires.ftc.teamcode;
 
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
@@ -39,10 +37,6 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
-import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
 
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -85,7 +79,10 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
 
     // Declare OpMode members for each of the 4 motors.
     private final ElapsedTime runtime = new ElapsedTime();
-
+    private Encoder leftFront;
+    private Encoder rightFront;
+    private Encoder leftRear;
+    private Encoder rightRear;
     @Override
     public void runOpMode() {
 
@@ -96,18 +93,12 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
         DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
         DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
 
+        leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR));
+        leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR));
+        rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR));
+        rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR));
 
 
-        // TODO: replace these with your encoder ports
-        Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
-        Encoder rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
-        Encoder strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
-
-        // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
-        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
-        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
-
         // ########################################################################################
         // !!!            IMPORTANT Drive Information. Test your motor directions.            !!!!!
         // ########################################################################################
@@ -123,11 +114,20 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
         rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
         rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
 
+        leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        leftBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        rightBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+
+
         // Wait for the game to start (driver presses START)
         telemetry.addData("Status", "Initialized");
-        telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
-        telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
-        telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
         telemetry.update();
 
         waitForStart();
@@ -186,12 +186,14 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
             rightBackDrive.setPower(rightBackPower);
 
             // Show the elapsed game time and wheel power.
-            telemetry.addData("Status", "Run Time: " + runtime.toString());
+            telemetry.addData("Status", "Run Time: " + runtime);
             telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
             telemetry.addData("Back  left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
-            telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
-            telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
-            telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
+            telemetry.addData("Encoder Front Left", leftFrontDrive.getDirection() + " : " + leftFrontDrive.getCurrentPosition());
+            telemetry.addData("Encoder Front Right", rightFrontDrive.getCurrentPosition());
+            telemetry.addData("Encoder Back Left", leftBackDrive.getCurrentPosition());
+            telemetry.addData("Encoder Back Right", rightBackDrive.getCurrentPosition());
             telemetry.update();
         }
-    }}
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BluebAutoV1.java

@@ -0,0 +1,247 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "BluebAutoV1", group = "Autonomous Pathing Tuning")
+public class BluebAutoV1 extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(7.5, 72, 90);
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.4);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(7.5, 72, Point.CARTESIAN),
+                                new Point(29.893, 38.250, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(29.893, 38.250, Point.CARTESIAN),
+                                new Point(65.250, 32.143, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(65.250, 32.143, Point.CARTESIAN),
+                                new Point(61.714, 24.429, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(61.714, 24.429, Point.CARTESIAN),
+                                new Point(13.821, 22.821, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(13.821, 22.821, Point.CARTESIAN),
+                                new Point(61.714, 24.429, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(61.714, 24.429, Point.CARTESIAN),
+                                new Point(60.750, 12.696, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 7
+                        new BezierLine(
+                                new Point(60.750, 12.696, Point.CARTESIAN),
+                                new Point(12.375, 13.179, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(12.375, 13.179, Point.CARTESIAN),
+                                new Point(60.750, 12.536, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(60.750, 12.536, Point.CARTESIAN),
+                                new Point(60.589, 9.321, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 10
+                        new BezierLine(
+                                new Point(60.589, 9.321, Point.CARTESIAN),
+                                new Point(12.536, 8.357, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 11
+                        new BezierLine(
+                                new Point(12.536, 8.357, Point.CARTESIAN),
+                                new Point(26.679, 8.679, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 12
+                        new BezierLine(
+                                new Point(26.679, 8.679, Point.CARTESIAN),
+                                new Point(22.821, 109.446, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 13
+                        new BezierLine(
+                                new Point(22.821, 109.446, Point.CARTESIAN),
+                                new Point(70.714, 109.446, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 14
+                        new BezierLine(
+                                new Point(70.714, 109.446, Point.CARTESIAN),
+                                new Point(71.036, 120.214, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 15
+                        new BezierLine(
+                                new Point(71.036, 120.214, Point.CARTESIAN),
+                                new Point(22.179, 120.214, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 16
+                        new BezierLine(
+                                new Point(22.179, 120.214, Point.CARTESIAN),
+                                new Point(11.089, 130.821, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 17
+                        new BezierLine(
+                                new Point(11.089, 130.821, Point.CARTESIAN),
+                                new Point(70.714, 112.018, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 18
+                        new BezierLine(
+                                new Point(70.714, 112.018, Point.CARTESIAN),
+                                new Point(70.714, 128.250, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 19
+                        new BezierLine(
+                                new Point(70.714, 128.250, Point.CARTESIAN),
+                                new Point(9.964, 130.018, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 20
+                        new BezierLine(
+                                new Point(9.964, 130.018, Point.CARTESIAN),
+                                new Point(70.554, 130.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 21
+                        new BezierLine(
+                                new Point(70.554, 130.500, Point.CARTESIAN),
+                                new Point(70.393, 135.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 22
+                        new BezierLine(
+                                new Point(70.393, 135.000, Point.CARTESIAN),
+                                new Point(13.821, 134.839, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
+
+        follower.followPath(path);
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path);
+        }
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BluenbAutov1.java

@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode;
+
+public class BluenbAutov1 {
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java

@@ -1,24 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
-
-@TeleOp(name = "CometBot Auto v2", group = "Development")
-public class CometBotDevAuto extends OpMode {
-
-    public CometBotAutoDevelopment runMode;
-
-    @Override
-    public void init() {
-        runMode = new CometBotAutoDevelopment(hardwareMap, gamepad1, gamepad2);
-        runMode.init();
-    }
-
-    @Override
-    public void loop() {
-        runMode.update();
-        telemetry.update();
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java

@@ -12,73 +12,46 @@ public class PedroConstants {
     */
 
     // Robot motor configurations
-    public static final String FRONT_LEFT_MOTOR = "front-left";
-    public static final String BACK_LEFT_MOTOR = "back-left";
-    public static final String FRONT_RIGHT_MOTOR = "front-right";
-    public static final String BACK_RIGHT_MOTOR = "back-right";
+    public static final String FRONT_LEFT_MOTOR = "Drive front lt";
+    public static final String BACK_LEFT_MOTOR  = "Drive back lt";
+    public static final String FRONT_RIGHT_MOTOR  = "Drive front rt";
+    public static final String BACK_RIGHT_MOTOR  = "Drive back rt";
 
     // Robot motor direction
     public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
     public static final Direction BACK_LEFT_MOTOR_DIRECTION = Direction.REVERSE;
-    public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
-    public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD;
-
-    /*
-        Motor Max Power
-    */
-    public static final double MAX_POWER = .75;
-
-    // Robot IMU configuration
-    public static final String IMU = "imu";
-
-    // Robot IMU placement
-    public static final RevHubOrientationOnRobot.LogoFacingDirection IMU_LOGO_FACING_DIRECTION
-            = RevHubOrientationOnRobot.LogoFacingDirection.LEFT;
-    public static final RevHubOrientationOnRobot.UsbFacingDirection IMU_USB_FACING_DIRECTION
-            = RevHubOrientationOnRobot.UsbFacingDirection.UP;
-
-    // Robot encoders
-    // NOTE:  Encoders are plugged into the same ports as motors hence the weird names
-    public static final String RIGHT_ENCODER = "front-left"; //2
-    public static final String BACK_ENCODER = "front-right"; //1
-    public static final String LEFT_ENCODER = "back-right"; //0
+    public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.REVERSE;
+    public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.REVERSE;
 
     // Robot encoder direction
-    public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
-    public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE;
-    public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
-
-    // Arm config
-    public static final String LIFT_SLIDE_LEFT_MOTOR = "lift-slide-left";
-    public static final String LIFT_SLIDE_RIGHT_MOTOR = "lift-slide-right";
-    public static final String CLAW_SERVO = "claw-servo";
-    public static final String WRIST_SERVO = "wrist-servo";
-    public static final String ARM_SERVO = "arm-servo";
-    public static final String THUMB_SERVO = "thumb-servo";
+    public static final double FRONT_LEFT_MOTOR_ENCODER = Encoder.FORWARD;
+    public static final double BACK_LEFT_MOTOR_ENCODER = Encoder.FORWARD;
+    public static final double FRONT_RIGHT_MOTOR_ENCODER = Encoder.FORWARD;
+    public static final double BACK_RIGHT_MOTOR_ENCODER = Encoder.FORWARD;
 
     /*
         Pedro's parameters
     */
 
     // The weight of the robot in Kilograms
-    public static final double ROBOT_WEIGHT_IN_KG = 9;
+    public static final double ROBOT_WEIGHT_IN_KG = 5.15;
 
     // Maximum velocity of the robot going forward
-    public static final double ROBOT_SPEED_FORWARD = 53.223;
+    public static final double ROBOT_SPEED_FORWARD = 66.6117;
 
     // Maximum velocity of the robot going right
-    public static final double ROBOT_SPEED_LATERAL = 41.4081;
+    public static final double ROBOT_SPEED_LATERAL = 60.0671;
 
     // Rate of deceleration when power is cut-off when the robot is moving forward
-    public static final double FORWARD_ZERO_POWER_ACCEL = -76.8421;
+    public static final double FORWARD_ZERO_POWER_ACCEL = -71.154;
 
     // Rate of deceleration when power is cut-off when the robot is moving to the right
-    public static final double LATERAL_ZERO_POWER_ACCEL = -93.4183;
+    public static final double LATERAL_ZERO_POWER_ACCEL = -109.5358;
 
     // Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop
     public static final double ZERO_POWER_ACCEL_MULT = 4.0;
 
     /* Centripetal force correction - increase if robot is correcting into the path
                                     - decrease if robot is correcting away from the path */
-    public static final double CENTRIPETAL_SCALING = 0.0004;
+    public static final double CENTRIPETAL_SCALING = 0.0005;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java

@@ -1,86 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots;
-
-import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.SequentialAction;
-import com.acmerobotics.roadrunner.SleepAction;
-import com.acmerobotics.roadrunner.ftc.Actions;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.states.FieldStates;
-import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
-
-public class CometBotAutoDevelopment {
-
-    /*
-    Subsystems
-     */
-    private DualMotorSliderSubsystem dualSlides;
-
-    /*
-    Controllers
-     */
-    public Gamepad gamepad1;
-    public Gamepad gamepad2;
-    public Gamepad currentGamepad1;
-    public Gamepad currentGamepad2;
-    public Gamepad previousGamepad1;
-    public Gamepad previousGamepad2;
-
-    private Follower follower;
-
-    public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
-        dualSlides = new DualMotorSliderSubsystem(hardwareMap);
-        this.gamepad1 = gamepad1;
-        this.gamepad2 = gamepad2;
-        currentGamepad1 = new Gamepad();
-        currentGamepad2 = new Gamepad();
-        previousGamepad1 = new Gamepad();
-        previousGamepad2 = new Gamepad();
-        follower = new Follower(hardwareMap);
-    }
-    
-    public void init() {
-        dualSlides.init();
-        follower.setMaxPower(MAX_POWER);
-        follower.startTeleopDrive();
-    }
-
-    public void update() {
-        previousGamepad1.copy(currentGamepad1);
-        currentGamepad1.copy(gamepad1);
-        previousGamepad2.copy(currentGamepad2);
-        currentGamepad2.copy(gamepad2);
-
-        /*
-        Check if dpad_up/down is being pressed for slides
-        */
-        dualSlides.update();
-        dualSlidesToLowBucketPosition();
-        dualSlidesToHighBucketPosition();
-
-        follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
-        follower.update();
-    }
-
-    private void dualSlidesToHighBucketPosition() {
-        if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
-            dualSlides.toHighBucketPosition();
-        }
-    }
-
-    private void dualSlidesToLowBucketPosition() {
-        if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
-            dualSlides.toLowBucketPosition();
-        }
-    }
-
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/DualMotorSliderTest.java

@@ -1,72 +0,0 @@
-package org.firstinspires.ftc.teamcode.cometbots.tests;
-
-import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-
-@Config
-@Autonomous(name = "Lift Motor Subsystem - PID Test")
-public class DualMotorSliderTest extends LinearOpMode {
-
-    private DcMotorEx liftSlideLeft;
-    private DcMotorEx liftSlideRight;
-    public static double kp = 0.0015, ki = 0, kd = 0;
-    private double lastError = 0;
-    private double integralSum = 0;
-    public static int targetPosition = 0;
-    private final FtcDashboard dashboard = FtcDashboard.getInstance();
-    private ElapsedTime timer = new ElapsedTime();
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-        TelemetryPacket packet = new TelemetryPacket();
-        dashboard.setTelemetryTransmissionInterval(25);
-
-        liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
-        liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
-
-        liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        waitForStart();
-
-        while(opModeIsActive()) {
-            double power = calculatePower(targetPosition, liftSlideLeft.getCurrentPosition());
-
-            packet.put("Power", power);
-            packet.put("Position", liftSlideLeft.getCurrentPosition());
-            packet.put("Error", lastError);
-            packet.put("Seconds", timer.seconds());
-
-            liftSlideLeft.setPower(power);
-            liftSlideRight.setPower(power);
-
-            dashboard.sendTelemetryPacket(packet);
-        }
-    }
-
-    private double calculatePower(int targetPosition, int currentPosition) {
-        // reference is targetPosition, state is currentPosition
-        double error = targetPosition - currentPosition;
-        integralSum += error * timer.seconds();
-        double derivative = (error - lastError) / timer.seconds();
-        lastError = error;
-        timer.reset();
-        return (error * kp) + (derivative * kd) + (integralSum * ki);
-    }
-
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java

@@ -5,7 +5,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.DriveEncoderLocalizer;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 
@@ -67,7 +67,7 @@ public class PoseUpdater {
      */
     public PoseUpdater(HardwareMap hardwareMap) {
         // TODO: replace the second argument with your preferred localizer
-        this(hardwareMap, new ThreeWheelLocalizer(hardwareMap));
+        this(hardwareMap, new DriveEncoderLocalizer(hardwareMap));
     }
 
     /**

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java

@@ -1,8 +1,20 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
 
-import static org.firstinspires.ftc.teamcode.PedroConstants.*;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_ENCODER;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_ENCODER;
 
 import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
@@ -15,95 +27,78 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
 
 /**
- * This is the ThreeWheelLocalizer class. This class extends the Localizer superclass and is a
- * localizer that uses the three wheel odometry set up. The diagram below, which is modified from
- * Road Runner, shows a typical set up.
- *
- * The view is from the top of the robot looking downwards.
- *
- * left on robot is the y positive direction
- *
- * forward on robot is the x positive direction
- *
- *    /--------------\
- *    |     ____     |
- *    |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
- *    \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
+ * This is the DriveEncoderLocalizer class. This class extends the Localizer superclass and is a
+ * localizer that uses the drive encoder set up.
  *
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 4/2/2024
  */
 @Config
-public class ThreeWheelLocalizer extends Localizer {
-    private HardwareMap hardwareMap;
+public class DriveEncoderLocalizer extends Localizer {
+    private final HardwareMap hardwareMap;
     private Pose startPose;
     private Pose displacementPose;
     private Pose currentVelocity;
     private Matrix prevRotationMatrix;
-    private NanoTimer timer;
+    private final NanoTimer timer;
     private long deltaTimeNano;
-    private Encoder leftEncoder;
-    private Encoder rightEncoder;
-    private Encoder strafeEncoder;
-    private Pose leftEncoderPose;
-    private Pose rightEncoderPose;
-    private Pose strafeEncoderPose;
+    private final Encoder leftFront;
+    private final Encoder rightFront;
+    private final Encoder leftRear;
+    private final Encoder rightRear;
     private double totalHeading;
-    public static double FORWARD_TICKS_TO_INCHES = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
-    public static double STRAFE_TICKS_TO_INCHES = -0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
-    public static double TURN_TICKS_TO_RADIANS = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5;
+    public static double FORWARD_TICKS_TO_INCHES = -0.0058;
+    public static double STRAFE_TICKS_TO_INCHES = -0.0054;
+    public static double TURN_TICKS_TO_RADIANS = -0.0009;
+    public static double ROBOT_WIDTH = 1;
+    public static double ROBOT_LENGTH = 1;
 
     /**
-     * This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
+     * This creates a new DriveEncoderLocalizer from a HardwareMap, with a starting Pose at (0,0)
      * facing 0 heading.
      *
      * @param map the HardwareMap
      */
-    public ThreeWheelLocalizer(HardwareMap map) {
+    public DriveEncoderLocalizer(HardwareMap map) {
         this(map, new Pose());
     }
 
     /**
-     * This creates a new ThreeWheelLocalizer from a HardwareMap and a Pose, with the Pose
+     * This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose
      * specifying the starting pose of the localizer.
      *
      * @param map          the HardwareMap
      * @param setStartPose the Pose to start from
      */
-    public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) {
-        // TODO: replace these with your encoder positions
-        leftEncoderPose = new Pose(0.25, 6.25, 0);
-        rightEncoderPose = new Pose(0.25, -6.25, 0);
-        strafeEncoderPose = new Pose(-7, 0.25, Math.toRadians(90));
-
+    public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) {
         hardwareMap = map;
 
-        // TODO: replace these with your encoder ports
-        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
-        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER));
-        strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER));
+        leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR));
+        leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR));
+        rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR));
+        rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR));
+
+        DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
+        DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
+        DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
+        DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
+
+        leftFrontDrive.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
+        leftBackDrive.setDirection(BACK_LEFT_MOTOR_DIRECTION);
+        rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
+        rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
 
         // TODO: reverse any encoders necessary
-        leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
-        rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
-        strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
+        leftFront.setDirection(FRONT_LEFT_MOTOR_ENCODER);
+        rightFront.setDirection(FRONT_RIGHT_MOTOR_ENCODER);
+        leftRear.setDirection(BACK_LEFT_MOTOR_ENCODER);
+        rightRear.setDirection(BACK_RIGHT_MOTOR_ENCODER);
 
         setStartPose(setStartPose);
         timer = new NanoTimer();
         deltaTimeNano = 1;
         displacementPose = new Pose();
         currentVelocity = new Pose();
-        totalHeading = 0;
-
-        resetEncoders();
     }
 
     /**
@@ -215,18 +210,20 @@ public class ThreeWheelLocalizer extends Localizer {
      * This updates the Encoders.
      */
     public void updateEncoders() {
-        leftEncoder.update();
-        rightEncoder.update();
-        strafeEncoder.update();
+        leftFront.update();
+        rightFront.update();
+        leftRear.update();
+        rightRear.update();
     }
 
     /**
      * This resets the Encoders.
      */
     public void resetEncoders() {
-        leftEncoder.reset();
-        rightEncoder.reset();
-        strafeEncoder.reset();
+        leftFront.reset();
+        rightFront.reset();
+        leftRear.reset();
+        rightRear.reset();
     }
 
     /**
@@ -238,11 +235,11 @@ public class ThreeWheelLocalizer extends Localizer {
     public Matrix getRobotDeltas() {
         Matrix returnMatrix = new Matrix(3, 1);
         // x/forward movement
-        returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
+        returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition()));
         //y/strafe movement
-        returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))));
+        returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition()));
         // theta/turning
-        returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())));
+        returnMatrix.set(2, 0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH)));
         return returnMatrix;
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java

@@ -0,0 +1,218 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
+
+import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+
+/**
+ * This is the OTOSLocalizer class. This class extends the Localizer superclass and is a
+ * localizer that uses the SparkFun OTOS. The diagram below, which is modified from
+ * Road Runner, shows a typical set up.
+ *
+ * The view is from the top of the robot looking downwards.
+ *
+ * left on robot is the y positive direction
+ *
+ * forward on robot is the x positive direction
+ *
+ *    /--------------\
+ *    |     ____     |
+ *    |     ----     |
+ *    | ||        || |
+ *    | ||        || |  ----> left (y positive)
+ *    |              |
+ *    |              |
+ *    \--------------/
+ *           |
+ *           |
+ *           V
+ *    forward (x positive)
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 7/20/2024
+ */
+public class OTOSLocalizer extends Localizer {
+    private HardwareMap hardwareMap;
+    private Pose startPose;
+    private SparkFunOTOS otos;
+    private double previousHeading;
+    private double totalHeading;
+
+    /**
+     * This creates a new OTOSLocalizer from a HardwareMap, with a starting Pose at (0,0)
+     * facing 0 heading.
+     *
+     * @param map the HardwareMap
+     */
+    public OTOSLocalizer(HardwareMap map) {
+        this(map, new Pose());
+    }
+
+    /**
+     * This creates a new OTOSLocalizer from a HardwareMap and a Pose, with the Pose
+     * specifying the starting pose of the localizer.
+     *
+     * @param map the HardwareMap
+     * @param setStartPose the Pose to start from
+     */
+    public OTOSLocalizer(HardwareMap map, Pose setStartPose) {
+        hardwareMap = map;
+
+        // TODO: replace this with your OTOS port
+        /*
+         TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the
+          'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a
+          "SparkFunOTOS Corrected" in your robot confg
+         */
+         SparkFunOTOS
+        otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
+
+        otos.setLinearUnit(DistanceUnit.INCH);
+        otos.setAngularUnit(AngleUnit.RADIANS);
+
+        // TODO: replace this with your OTOS offset from the center of the robot
+        // For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being
+        // positive y and forward being positive x. PI/2 radians is facing forward, and clockwise
+        // rotation is negative rotation.
+        otos.setOffset(new SparkFunOTOS.Pose2D(0,0,Math.PI / 2));
+
+        // TODO: replace these with your tuned multipliers
+        otos.setLinearScalar(1.0);
+        otos.setAngularScalar(1.0);
+
+        otos.calibrateImu();
+        otos.resetTracking();
+
+        setStartPose(setStartPose);
+        totalHeading = 0;
+        previousHeading = startPose.getHeading();
+
+        resetOTOS();
+    }
+
+    /**
+     * This returns the current pose estimate.
+     *
+     * @return returns the current pose estimate as a Pose
+     */
+    @Override
+    public Pose getPose() {
+        SparkFunOTOS.Pose2D pose = otos.getPosition();
+        return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h));
+    }
+
+    /**
+     * This returns the current velocity estimate.
+     *
+     * @return returns the current velocity estimate as a Pose
+     */
+    @Override
+    public Pose getVelocity() {
+        SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity();
+        return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h);
+    }
+
+    /**
+     * This returns the current velocity estimate.
+     *
+     * @return returns the current velocity estimate as a Vector
+     */
+    @Override
+    public Vector getVelocityVector() {
+        return getVelocity().getVector();
+    }
+
+    /**
+     * This sets the start pose. Changing the start pose should move the robot as if all its
+     * previous movements were displacing it from its new start pose.
+     *
+     * @param setStart the new start pose
+     */
+    @Override
+    public void setStartPose(Pose setStart) {
+        startPose = setStart;
+    }
+
+    /**
+     * This sets the current pose estimate. Changing this should just change the robot's current
+     * pose estimate, not anything to do with the start pose.
+     *
+     * @param setPose the new current pose estimate
+     */
+    @Override
+    public void setPose(Pose setPose) {
+        resetOTOS();
+        Pose setOTOSPose = MathFunctions.subtractPoses(setPose, startPose);
+        otos.setPosition(new SparkFunOTOS.Pose2D(setOTOSPose.getX(), setOTOSPose.getY(), setOTOSPose.getHeading()));
+    }
+
+    /**
+     * This updates the total heading of the robot. The OTOS handles all other updates itself.
+     */
+    @Override
+    public void update() {
+        totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading);
+        previousHeading = otos.getPosition().h;
+    }
+
+    /**
+     * This resets the OTOS.
+     */
+    public void resetOTOS() {
+        otos.resetTracking();
+    }
+
+    /**
+     * This returns how far the robot has turned in radians, in a number not clamped between 0 and
+     * 2 * pi radians. This is used for some tuning things and nothing actually within the following.
+     *
+     * @return returns how far the robot has turned in total, in radians.
+     */
+    public double getTotalHeading() {
+        return totalHeading;
+    }
+
+    /**
+     * This returns the multiplier applied to forward movement measurement to convert from OTOS
+     * ticks to inches. For the OTOS, this value is the same as the lateral multiplier.
+     * This is found empirically through a tuner.
+     *
+     * @return returns the forward ticks to inches multiplier
+     */
+    public double getForwardMultiplier() {
+        return otos.getLinearScalar();
+    }
+
+    /**
+     * This returns the multiplier applied to lateral/strafe movement measurement to convert from
+     * OTOS ticks to inches. For the OTOS, this value is the same as the forward multiplier.
+     * This is found empirically through a tuner.
+     *
+     * @return returns the lateral/strafe ticks to inches multiplier
+     */
+    public double getLateralMultiplier() {
+        return otos.getLinearScalar();
+    }
+
+    /**
+     * This returns the multiplier applied to turning movement measurement to convert from OTOS ticks
+     * to radians. This is found empirically through a tuner.
+     *
+     * @return returns the turning ticks to radians multiplier
+     */
+    public double getTurningMultiplier() {
+        return otos.getAngularScalar();
+    }
+
+    /**
+     * This does nothing since this localizer does not use the IMU.
+     */
+    public void resetIMU() {
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java

@@ -0,0 +1,159 @@
+//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.qualcomm.robotcore.hardware.HardwareMap;
+//
+//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
+//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+//
+//import java.util.ArrayList;
+//import java.util.List;
+//
+///**
+// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and
+// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing
+// * localizer system.
+// *
+// * @author Anyi Lin - 10158 Scott's Bots
+// * @version 1.0, 5/9/2024
+// */
+//public class RRToPedroThreeWheelLocalizer extends Localizer {
+//    private RoadRunnerThreeWheelLocalizer localizer;
+//    private double totalHeading;
+//    private Pose startPose;
+//    private Pose previousPose;
+//
+//    /**
+//     * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously
+//     * used Road Runner localization system to the new Pedro Pathing localization system.
+//     *
+//     * @param hardwareMap the HardwareMap
+//     */
+//    public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) {
+//        List<Integer> lastTrackingEncPositions = new ArrayList<>();
+//        List<Integer> lastTrackingEncVels = new ArrayList<>();
+//
+//        localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels);
+//
+//        startPose = new Pose();
+//        previousPose = new Pose();
+//    }
+//
+//    /**
+//     * This returns the current pose estimate as a Pose.
+//     *
+//     * @return returns the current pose estimate
+//     */
+//    @Override
+//    public Pose getPose() {
+//        Pose2d pose = localizer.getPoseEstimate();
+//        return new Pose(pose.getX(), pose.getY(), pose.getHeading());
+//    }
+//
+//    /**
+//     * This returns the current velocity estimate as a Pose.
+//     *
+//     * @return returns the current velocity estimate
+//     */
+//    @Override
+//    public Pose getVelocity() {
+//        Pose2d pose = localizer.getPoseVelocity();
+//        return new Pose(pose.getX(), pose.getY(), pose.getHeading());
+//    }
+//
+//    /**
+//     * This returns the current velocity estimate as a Vector.
+//     *
+//     * @return returns the current velocity estimate
+//     */
+//    @Override
+//    public Vector getVelocityVector() {
+//        Pose2d pose = localizer.getPoseVelocity();
+//        Vector returnVector = new Vector();
+//        returnVector.setOrthogonalComponents(pose.getX(), pose.getY());
+//        return returnVector;
+//    }
+//
+//    /**
+//     * This sets the start pose. Any movement of the robot is treated as a displacement from the
+//     * start pose, so moving the start pose will move the current pose estimate the same amount.
+//     *
+//     * @param setStart the new start pose
+//     */
+//    @Override
+//    public void setStartPose(Pose setStart) {
+//        Pose oldStart = startPose;
+//        startPose = setStart;
+//        Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart);
+//        localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading()));
+//    }
+//
+//    /**
+//     * This sets the current pose estimate. This has no effect on the start pose.
+//     *
+//     * @param setPose the new current pose estimate
+//     */
+//    @Override
+//    public void setPose(Pose setPose) {
+//        localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading()));
+//    }
+//
+//    /**
+//     * This updates the total heading and previous pose estimate. Everything else is handled by the
+//     * Road Runner localizer on its own, but updating this tells you how far the robot has really
+//     * turned.
+//     */
+//    @Override
+//    public void update() {
+//        totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading());
+//        previousPose = getPose();
+//    }
+//
+//    /**
+//     * This returns how far the robot has actually turned.
+//     *
+//     * @return returns the total angle turned, in degrees.
+//     */
+//    @Override
+//    public double getTotalHeading() {
+//        return totalHeading;
+//    }
+//
+//    /**
+//     * This returns the forward multiplier of the Road Runner localizer, which converts from ticks
+//     * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything
+//     * multiplied together should be. If you do use that, then do be aware that the value returned is
+//     * the product of the Road Runner ticks to inches and the x multiplier.
+//     *
+//     * @return returns the forward multiplier
+//     */
+//    @Override
+//    public double getForwardMultiplier() {
+//        return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER;
+//    }
+//
+//    /**
+//     * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks
+//     * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything
+//     * multiplied together should be. If you do use that, then do be aware that the value returned is
+//     * the product of the Road Runner ticks to inches and the y multiplier.
+//     *
+//     * @return returns the lateral multiplier
+//     */
+//    @Override
+//    public double getLateralMultiplier() {
+//        return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER;
+//    }
+//
+//    /**
+//     * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist.
+//     * There really isn't a point in tuning the turning for the Road Runner localizer. This will
+//     * actually just return the average of the two other multipliers.
+//     *
+//     * @return returns the turning multiplier
+//     */
+//    @Override
+//    public double getTurningMultiplier() {
+//        return (getForwardMultiplier() + getLateralMultiplier()) / 2;
+//    }
+//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java

@@ -0,0 +1,132 @@
+//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
+//
+//import com.acmerobotics.roadrunner.util.NanoClock;
+//import com.qualcomm.robotcore.hardware.DcMotorEx;
+//import com.qualcomm.robotcore.hardware.DcMotorSimple;
+//
+///**
+// * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a
+// * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected
+// * velocity counts and allow reversing independently of the corresponding slot's motor direction.
+// *
+// * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have
+// * import statements, so I'm not crediting myself as an author for this.
+// *
+// * @author Road Runner dev team
+// * @version 1.0, 5/9/2024
+// */
+//public class RoadRunnerEncoder {
+//    private final static int CPS_STEP = 0x10000;
+//
+//    private static double inverseOverflow(double input, double estimate) {
+//        // convert to uint16
+//        int real = (int) input & 0xffff;
+//        // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
+//        // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
+//        real += ((real % 20) / 4) * CPS_STEP;
+//        // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
+//        real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
+//        return real;
+//    }
+//
+//    public enum Direction {
+//        FORWARD(1),
+//        REVERSE(-1);
+//
+//        private int multiplier;
+//
+//        Direction(int multiplier) {
+//            this.multiplier = multiplier;
+//        }
+//
+//        public int getMultiplier() {
+//            return multiplier;
+//        }
+//    }
+//
+//    private DcMotorEx motor;
+//    private NanoClock clock;
+//
+//    private Direction direction;
+//
+//    private int lastPosition;
+//    private int velocityEstimateIdx;
+//    private double[] velocityEstimates;
+//    private double lastUpdateTime;
+//
+//    public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) {
+//        this.motor = motor;
+//        this.clock = clock;
+//
+//        this.direction = Direction.FORWARD;
+//
+//        this.lastPosition = 0;
+//        this.velocityEstimates = new double[3];
+//        this.lastUpdateTime = clock.seconds();
+//    }
+//
+//    public RoadRunnerEncoder(DcMotorEx motor) {
+//        this(motor, NanoClock.system());
+//    }
+//
+//    public Direction getDirection() {
+//        return direction;
+//    }
+//
+//    private int getMultiplier() {
+//        return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
+//    }
+//
+//    /**
+//     * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
+//     * @param direction either reverse or forward depending on if encoder counts should be negated
+//     */
+//    public void setDirection(Direction direction) {
+//        this.direction = direction;
+//    }
+//
+//    /**
+//     * Gets the position from the underlying motor and adjusts for the set direction.
+//     * Additionally, this method updates the velocity estimates used for compensated velocity
+//     *
+//     * @return encoder position
+//     */
+//    public int getCurrentPosition() {
+//        int multiplier = getMultiplier();
+//        int currentPosition = motor.getCurrentPosition() * multiplier;
+//        if (currentPosition != lastPosition) {
+//            double currentTime = clock.seconds();
+//            double dt = currentTime - lastUpdateTime;
+//            velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
+//            velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
+//            lastPosition = currentPosition;
+//            lastUpdateTime = currentTime;
+//        }
+//        return currentPosition;
+//    }
+//
+//    /**
+//     * Gets the velocity directly from the underlying motor and compensates for the direction
+//     * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
+//     *
+//     * @return raw velocity
+//     */
+//    public double getRawVelocity() {
+//        int multiplier = getMultiplier();
+//        return motor.getVelocity() * multiplier;
+//    }
+//
+//    /**
+//     * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
+//     * that are lost in overflow due to velocity being transmitted as 16 bits.
+//     * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
+//     *
+//     * @return corrected velocity
+//     */
+//    public double getCorrectedVelocity() {
+//        double median = velocityEstimates[0] > velocityEstimates[1]
+//                ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
+//                : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
+//        return inverseOverflow(getRawVelocity(), median);
+//    }
+//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java

@@ -0,0 +1,123 @@
+//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
+//
+//import androidx.annotation.NonNull;
+//
+//import com.acmerobotics.dashboard.config.Config;
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
+//import com.qualcomm.robotcore.hardware.DcMotorEx;
+//import com.qualcomm.robotcore.hardware.HardwareMap;
+//
+//import java.util.Arrays;
+//import java.util.List;
+//
+///*
+// * Sample tracking wheel localizer implementation assuming the standard configuration:
+// *
+// * left on robot is y pos
+// *
+// * front of robot is x pos
+// *
+// *    /--------------\
+// *    |     ____     |
+// *    |     ----     |
+// *    | ||        || |
+// *    | ||        || |
+// *    |              |
+// *    |              |
+// *    \--------------/
+// *
+// */
+//
+///**
+// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will
+// * be replaced with a custom localizer. I made some minor changes, so I'm crediting myself as an
+// * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to
+// * Pedro Pathing to avoid having imports.
+// *
+// * @author Road Runner dev team
+// * @author Anyi Lin - 10158 Scott's Bots
+// * @version 1.0, 5/9/2024
+// */
+//@Config
+//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer {
+//    public static double TICKS_PER_REV = 8192;
+//    public static double WHEEL_RADIUS = 1.37795; // in
+//    public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
+//
+//    public static double X_MULTIPLIER = 0.5008239963;
+//    public static double Y_MULTIPLIER = 0.5018874659;
+//
+//    public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23;
+//
+//    private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder;
+//
+//    private List<Integer> lastEncPositions, lastEncVels;
+//
+//    public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
+//        super(Arrays.asList(
+//                new Pose2d(leftX, leftY, 0), // left
+//                new Pose2d(rightX, rightY, 0), // right
+//                new Pose2d(strafeX, strafeY,  Math.toRadians(90)) // strafe
+//        ));
+//
+//        lastEncPositions = lastTrackingEncPositions;
+//        lastEncVels = lastTrackingEncVels;
+//
+//        // TODO: redo the configs here
+//        leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear"));
+//        rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront"));
+//        strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder"));
+//
+//        // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+//        leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
+//        rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE);
+//        strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD);
+//    }
+//
+//    public void resetHeading(double heading) {
+//        setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading));
+//    }
+//
+//    public static double encoderTicksToInches(double ticks) {
+//        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+//    }
+//
+//    @NonNull
+//    @Override
+//    public List<Double> getWheelPositions() {
+//        int leftPos = leftEncoder.getCurrentPosition();
+//        int rightPos = rightEncoder.getCurrentPosition();
+//        int frontPos = strafeEncoder.getCurrentPosition();
+//
+//        lastEncPositions.clear();
+//        lastEncPositions.add(leftPos);
+//        lastEncPositions.add(rightPos);
+//        lastEncPositions.add(frontPos);
+//
+//        return Arrays.asList(
+//                encoderTicksToInches(leftPos) * X_MULTIPLIER,
+//                encoderTicksToInches(rightPos) * X_MULTIPLIER,
+//                encoderTicksToInches(frontPos) * Y_MULTIPLIER
+//        );
+//    }
+//
+//    @NonNull
+//    @Override
+//    public List<Double> getWheelVelocities() {
+//        int leftVel = (int) leftEncoder.getCorrectedVelocity();
+//        int rightVel = (int) rightEncoder.getCorrectedVelocity();
+//        int frontVel = (int) strafeEncoder.getCorrectedVelocity();
+//
+//        lastEncVels.clear();
+//        lastEncVels.add(leftVel);
+//        lastEncVels.add(rightVel);
+//        lastEncVels.add(frontVel);
+//
+//        return Arrays.asList(
+//                encoderTicksToInches(leftVel) * X_MULTIPLIER,
+//                encoderTicksToInches(rightVel) * X_MULTIPLIER,
+//                encoderTicksToInches(frontVel) * Y_MULTIPLIER
+//        );
+//    }
+//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java

@@ -63,6 +63,7 @@ public class LateralTuner extends OpMode {
         telemetryA.addData("distance moved", poseUpdater.getPose().getY());
         telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
         telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier()));
+
         telemetryA.update();
 
         Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50");

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java

@@ -1,9 +1,5 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning;
 
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
@@ -62,11 +58,6 @@ public class LocalizationTest extends OpMode {
         rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName);
         rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName);
 
-        leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
-        leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
-        rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
-        rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
-
         motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);
 
         for (DcMotorEx motor : motors) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java

@@ -40,9 +40,9 @@ public class FollowerConstants {
 
     // Translational PIDF coefficients (don't use integral)
     public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients(
-            .25,
+            0.1,
+            0,
             0,
-            0.0375,
             0);
 
     // Translational Integral
@@ -53,14 +53,14 @@ public class FollowerConstants {
             0);
 
     // Feed forward constant added on to the translational PIDF
-    public static double translationalPIDFFeedForward = 0.00;
+    public static double translationalPIDFFeedForward = 0.015;
 
 
     // Heading error PIDF coefficients
     public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
-            2,
+            1,
+            0,
             0,
-            0.0375,
             0);
 
     // Feed forward constant added on to the heading PIDF
@@ -69,10 +69,10 @@ public class FollowerConstants {
 
     // Drive PIDF coefficients
     public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
-            0.00375,
+            0.025,
             0,
-            0.00003,
-            0.8,
+            0.00001,
+            0.6,
             0);
 
     // Feed forward constant added on to the drive PIDF
@@ -81,7 +81,7 @@ public class FollowerConstants {
     // Kalman filter parameters for the drive error Kalman filter
     public static KalmanFilterParameters driveKalmanFilterParameters = new KalmanFilterParameters(
             6,
-            3);
+            1);
 
 
     // Mass of robot in kilograms
@@ -202,9 +202,9 @@ public class FollowerConstants {
 
     // Secondary drive PIDF coefficients
     public static CustomFilteredPIDFCoefficients secondaryDrivePIDFCoefficients = new CustomFilteredPIDFCoefficients(
-            0.02,
+            0.00315,
             0,
-            0.000005,
+            0.0001,
             0.6,
             0);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/ForwardVelocityTuner.java

@@ -93,13 +93,11 @@ public class ForwardVelocityTuner extends OpMode {
         }
 
         telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
-//        telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
-//        telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
-//        telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
-//        telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
-//
-//
-//        telemetryA.update();
+        telemetryA.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
+        telemetryA.addLine("Make sure you have enough room, since the robot has inertia after cutting power.");
+        telemetryA.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
+        telemetryA.addLine("Press CROSS or A on game pad 1 to stop.");
+        telemetryA.update();
 
     }
 
@@ -140,13 +138,6 @@ public class ForwardVelocityTuner extends OpMode {
                 velocities.add(currentVelocity);
                 velocities.remove(0);
             }
-            telemetryA.addData("x", poseUpdater.getPose().getX());
-            telemetryA.addData("y", poseUpdater.getPose().getY());
-            telemetryA.addData("heading", poseUpdater.getPose().getHeading());
-            telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
-            telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
-            telemetryA.update();
-
         } else {
             double average = 0;
             for (Double velocity : velocities) {
@@ -155,12 +146,7 @@ public class ForwardVelocityTuner extends OpMode {
             average /= (double) velocities.size();
 
             telemetryA.addData("forward velocity:", average);
-//            telemetryA.addData("x", poseUpdater.getPose().getX());
-//            telemetryA.addData("y", poseUpdater.getPose().getY());
-//            telemetryA.addData("heading", poseUpdater.getPose().getHeading());
-//            telemetryA.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
-//            telemetryA.addData("velo ", poseUpdater.getVelocity().getTheta());
-//            telemetryA.update();
+            telemetryA.update();
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java

@@ -119,6 +119,13 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
      */
     @Override
     public void loop() {
+
+        telemetry.addData("x",poseUpdater.getPose().getX());
+        telemetry.addData("y",poseUpdater.getPose().getY());
+        telemetry.addData("heading",poseUpdater.getPose().getHeading());
+        telemetry.addData("velo mag", poseUpdater.getVelocity().getMagnitude());
+        telemetry.addData("velo theta", poseUpdater.getVelocity().getTheta());
+
         if (gamepad1.cross || gamepad1.a) {
             requestOpModeStop();
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java

@@ -1,18 +0,0 @@
-package org.firstinspires.ftc.teamcode.states;
-
-public class FieldStates {
-
-    public enum FieldLocation {
-        BUCKET, SUBMARINE, FLOATING, TRAVELING
-    }
-
-    private FieldLocation fieldLocation;
-
-    public FieldLocation getFieldLocation() {
-        return fieldLocation;
-    }
-
-    public void setFieldLocation(FieldLocation fieldLocation) {
-        this.fieldLocation = fieldLocation;
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java

@@ -1,131 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystem;
-
-import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
-import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
-
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-public class DualMotorSliderSubsystem {
-
-    /*
-    liftSlideLeft - Left Motor for Dual Linear Slide as a DcMotorEx object
-    Currently, the value of (liftSlideLeft) is null because we haven't assigned a value (object) to it yet.
-    It expects an object of type "DcMotorEx".
-     */
-    private DcMotorEx liftSlideLeft;
-
-    /*
-    liftSlideRight - Right Motor for Dual Linear Slide as a DcMotorEx object
-    Currently, the value of (liftSlideRight) is null because we haven't assigned a value (object) to it yet.
-    It expects an object of type "DcMotorEx".
-     */
-    private DcMotorEx liftSlideRight;
-
-    /*
-    targetPosition - Variable that holds target position of slides.
-    */
-    private int targetPosition = 0;
-
-    /*
-    getTargetPosition/setTargetPosition - Best practice to "hide" (private) targetPosition and,
-                                          instead, use a "setter" to set the target position value or
-                                          a "getter" to get the target position value.
-    */
-    public void setTargetPosition(int value) {
-        targetPosition = value;
-    }
-    private int getTargetPosition() { return targetPosition; }
-
-    /*
-    PID - Proportional/Integral/Derivative Values
-
-    For a dual motor linear slide, we only tune the P - Proportion.
-    The Proportion variable (kp) answers the question "how fast do we want to get to our destination?"
-
-    It's the only value we set because the variable ki and kd deal with how to handle when we're off the path.
-    Since we're going straight, we don't need to worry about.
-    */
-    public final static double kp = 0.0015, ki = 0, kd = 0;
-
-    /*
-    lastError/integralSum/timer - These 3 variables are placeholders in determining how much
-                                  power to send to both motors.
-    */
-    private double lastError = 0;
-    private double integralSum = 0;
-    private ElapsedTime timer = new ElapsedTime();
-
-    public DualMotorSliderSubsystem(HardwareMap hardwareMap) {
-        /*
-        liftSlideLeft/liftSlideRight - Now, we are assigning a value of DcMotorEx to each variable
-
-        We "assign" the object DcMotorEx to liftSlideLeft and liftSlideRight and "link them" to the
-        driver hub configuration name that matches the motor on the slide.
-         */
-        liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
-        liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
-    }
-
-    public void init() {
-        /*
-        Initialize the motors with the following settings (assuming slide is at the very bottom position):
-
-        - Reset the encoders to be zero
-        - When the motor stops moving (zero power), brake.  This means we can't move the motors, not even gravity.
-        - Sets the motor to run without the encoder.  This doesn't mean we won't use the encoder values (because we will).
-          It just means to not FULLY depend on them, we will just prefer to use motor power instead.
-         */
-        liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        liftSlideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-    }
-
-    /*
-    To calculate power, we send the targetPosition value (how high we want the slider motors to
-    be based on motor 'ticks') to calculatePower function.
-
-    For every loop that occurs, it will constantly calculate power.  So long as we're far away from
-    our "target" position, we will get as much power as possible.  The closer we get, the lower the
-    power we will receive.
-    */
-    public void update() {
-        double power = calculatePower();
-        liftSlideLeft.setPower(power);
-        liftSlideRight.setPower(power);
-    }
-
-    /*
-    Calculating power - To calculate the power, we determine the proportion, derivative and
-    integral of our closed loop system.
-
-    For more information, please visit:
-    - Introduction to Closed Loop System:
-      - https://www.ctrlaltftc.com/introduction-to-closed-loop-control
-    - The PID controller:
-      - https://www.ctrlaltftc.com/the-pid-controller
-    */
-    private double calculatePower() {
-        double error = getTargetPosition() - liftSlideLeft.getCurrentPosition();
-        integralSum += error * timer.seconds();
-        double derivative = (error - lastError) / timer.seconds();
-        lastError = error;
-        timer.reset();
-        return (error * kp) + (derivative * kd) + (integralSum * ki);
-    }
-
-    public void toLowBucketPosition() {
-        setTargetPosition(1500);
-    }
-
-    public void toHighBucketPosition() {
-        setTargetPosition(3000);
-    }
-
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java

@@ -1,105 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystem;
-
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
-import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
-import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
-import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
-import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-public class MotorsSubsystem {
-
-    public HardwareMap hardwareMap;
-    public Telemetry telemetry;
-
-    public DcMotor frontLeftMotor;
-    public DcMotor backLeftMotor;
-    public DcMotor frontRightMotor;
-    public DcMotor backRightMotor;
-
-    public enum TravelState {
-        STOPPED, MOVING
-    }
-
-    public TravelState travelState;
-
-    public double power;
-
-    public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
-        this.hardwareMap = hardwareMap;
-        this.telemetry = telemetry;
-        this.power = MAX_POWER;
-    }
-
-    public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
-        this.hardwareMap = hardwareMap;
-        this.telemetry = telemetry;
-        this.power = power;
-    }
-
-    public void init() {
-        frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
-        backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
-        frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
-        backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
-
-        frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
-        backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION);
-        frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
-        backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
-
-        frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
-        this.setState(TravelState.STOPPED);
-    }
-
-    public void setFrontLeftMotorPower(double power) {
-        frontLeftMotor.setPower(power);
-    }
-
-    public void setBackLeftMotorPower(double power) {
-        backLeftMotor.setPower(power);
-    }
-
-    public void setFrontRightMotorPower(double power) {
-        frontRightMotor.setPower(power);
-    }
-
-    public void setBackRightMotorPower(double power) {
-        backRightMotor.setPower(power);
-    }
-
-
-    public void setState(TravelState travelState) {
-        this.travelState = travelState;
-    }
-
-    public TravelState getState() {
-        return this.travelState;
-    }
-
-    public void setPower(DcMotor motor, double power) {
-        motor.setPower(power);
-        if (power < 0.05) {
-            this.setState(TravelState.MOVING);
-        } else {
-            this.setState(TravelState.STOPPED);
-        }
-    }
-}

build.dependencies.gradle

@@ -16,9 +16,6 @@ dependencies {
     implementation 'org.firstinspires.ftc:Vision:10.1.0'
     implementation 'androidx.appcompat:appcompat:1.2.0'
 
-    implementation "com.acmerobotics.roadrunner:ftc:0.1.14"
-    implementation "com.acmerobotics.roadrunner:core:1.0.0"
-    implementation "com.acmerobotics.roadrunner:actions:1.0.0"
-    implementation "com.acmerobotics.dashboard:dashboard:0.4.16"
+    implementation 'com.acmerobotics.dashboard:dashboard:0.4.5'
 }