diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle
index 9fa2169..8c796da 100644
--- a/FtcRobotController/build.gradle
+++ b/FtcRobotController/build.gradle
@@ -17,8 +17,8 @@ android {
     buildFeatures {
         buildConfig = true
     }
+    compileSdk 34
 
-    compileSdkVersion 30
 
     compileOptions {
         sourceCompatibility JavaVersion.VERSION_1_8
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java
new file mode 100644
index 0000000..84087b5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java
@@ -0,0 +1,478 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.h1;
+import static org.firstinspires.ftc.teamcode.constants.Poses.h2;
+import static org.firstinspires.ftc.teamcode.constants.Poses.h2_b;
+import static org.firstinspires.ftc.teamcode.constants.Poses.h3;
+import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.Poses.x1;
+import static org.firstinspires.ftc.teamcode.constants.Poses.x2;
+import static org.firstinspires.ftc.teamcode.constants.Poses.x2_b;
+import static org.firstinspires.ftc.teamcode.constants.Poses.x3;
+import static org.firstinspires.ftc.teamcode.constants.Poses.y1;
+import static org.firstinspires.ftc.teamcode.constants.Poses.y2;
+import static org.firstinspires.ftc.teamcode.constants.Poses.y2_b;
+import static org.firstinspires.ftc.teamcode.constants.Poses.y3;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.ParallelAction;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Shooter;
+import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
+import org.firstinspires.ftc.teamcode.subsystems.Transfer;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+
+@Config
+@Autonomous (preselectTeleOp = "TeleopV1")
+public class Blue extends LinearOpMode {
+
+    Robot robot;
+
+    MultipleTelemetry TELE;
+
+    MecanumDrive drive;
+
+    AprilTag aprilTag;
+
+    Shooter shooter;
+
+
+
+    public static double angle = 0.1;
+
+    Spindexer spindexer;
+
+    Transfer transfer;
+
+    public class shooterOn implements Action {
+
+        int ticker = 1;
+
+        double stamp = 0.0;
+
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+            if (ticker ==1){
+                stamp = getRuntime();
+
+            }
+
+            ticker ++;
+
+            if (getRuntime() - stamp < 0.2){
+                return true;
+            } else if (getRuntime() - stamp < 0.4) {
+
+                shooter.setManualPower(1);
+
+                shooter.update();
+
+                return true;
+            } else {
+
+                return false;
+
+            }
+        }
+    }
+
+
+
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+
+
+
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        drive = new MecanumDrive(hardwareMap, new Pose2d(
+                0,0,0
+        ));
+
+        aprilTag = new AprilTag(robot, TELE);
+
+        shooter = new Shooter(robot, TELE);
+
+        spindexer = new Spindexer(robot, TELE);
+
+        spindexer.outtake3();
+
+        shooter.setShooterMode("MANUAL");
+
+        shooter.sethoodPosition(0.53);
+
+        transfer = new Transfer(robot);
+
+        transfer.setTransferPosition(transferServo_out);
+
+        Intake intake = new Intake(robot);
+
+        robot.hood.setPosition(hoodDefault);
+
+
+
+
+
+
+
+
+        TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
+
+
+        TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .turnTo(Math.toRadians(-135))
+                .strafeToLinearHeading(new Vector2d(x2, -y2), -h2 );
+
+
+        TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, -y2, -h2))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
+
+        TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+
+                .strafeToLinearHeading(new Vector2d(x2_b, -y2_b), -h2_b )
+
+                .strafeToLinearHeading(new Vector2d(x3, -y3), -h3 );
+
+
+        TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, -y3, -h3))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
+
+
+        TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .strafeToLinearHeading(new Vector2d(x1, y1-30), h1 );
+
+        while(opModeInInit()) {
+
+            if (gamepad2.dpadUpWasPressed()){
+                hoodDefault -= 0.02;
+            }
+
+            if (gamepad2.dpadDownWasPressed()){
+                hoodDefault += 0.02;
+            }
+
+            robot.hood.setPosition(hoodDefault);
+
+            shooter.setTurretPosition(0.3);
+
+            aprilTag.initTelemetry();
+
+            aprilTag.update();
+            shooter.update();
+
+            TELE.addData("hood", hoodDefault);
+
+            TELE.update();
+
+
+        }
+
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        if (opModeIsActive()){
+
+            robot.hood.setPosition(hoodDefault);
+
+            transfer.setTransferPower(1);
+
+            transfer.update();
+
+
+
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            traj1.build(),
+                            new shooterOn()
+                    )
+            );
+
+
+            transfer.setTransferPower(1);
+
+            transfer.update();
+
+            shooter.setManualPower(1);
+
+            double stamp = getRuntime();
+
+
+             stamp = getRuntime();
+
+            while (getRuntime()-stamp<4.5) {
+
+
+
+                shooter.moveTurret(0.3);
+
+                double time = getRuntime()-stamp;
+
+
+                if (time < 0.3) {
+
+
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
+
+                    spindexer.outtake2();
+
+
+
+                } else if (time < 3.0) {
+
+
+                    transfer.transferIn();
+                } else if (time < 4.0) {
+
+                    transfer.transferOut();
+
+                    spindexer.outtake1();
+
+                } else if (time < 4.4) {
+                    transfer.transferIn();
+                } else {
+
+                    transfer.transferOut();
+                    spindexer.outtake3();
+
+                    shooter.setManualPower(1);
+
+                }
+
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
+            }
+
+            spindexer.outtake3();
+
+            robot.intake.setPower(1);
+
+            Actions.runBlocking(
+                    traj2.build()
+            );
+
+
+            Actions.runBlocking(
+                    traj3.build()
+            );
+
+            shooter.setManualPower(1);
+
+
+
+
+            robot.intake.setPower(0);
+
+            spindexer.outtake3();
+
+
+            stamp = getRuntime();
+
+            shooter.setManualPower(1);
+            while (getRuntime()-stamp<4.5) {
+
+
+
+                shooter.moveTurret(0.3);
+
+                double time = getRuntime()-stamp;
+
+
+                if (time < 0.3) {
+
+
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
+
+                    spindexer.outtake2();
+
+
+
+                } else if (time < 3.0) {
+
+
+                    transfer.transferIn();
+                } else if (time < 4.0) {
+
+                    transfer.transferOut();
+
+                    spindexer.outtake1();
+
+                } else if (time < 4.4) {
+                    transfer.transferIn();
+                } else {
+
+                    transfer.transferOut();
+                    spindexer.outtake3();
+
+                    shooter.setManualPower(1);
+
+                }
+
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
+            }
+
+            spindexer.outtake3();
+            robot.intake.setPower(1);
+
+            Actions.runBlocking(
+                    traj4.build()
+            );
+
+
+            Actions.runBlocking(
+                    traj5.build()
+            );
+
+            shooter.setManualPower(1);
+
+
+
+
+            robot.intake.setPower(0);
+
+
+            stamp = getRuntime();
+
+            shooter.setManualPower(1);
+
+            while (getRuntime()-stamp<4.5) {
+
+
+
+                shooter.moveTurret(0.3);
+
+                double time = getRuntime()-stamp;
+
+
+                if (time < 0.3) {
+
+
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
+
+                    spindexer.outtake2();
+
+
+
+                } else if (time < 3.0) {
+
+
+                    transfer.transferIn();
+                } else if (time < 4.0) {
+
+                    transfer.transferOut();
+
+                    spindexer.outtake1();
+
+                } else if (time < 4.4) {
+                    transfer.transferIn();
+                } else {
+
+                    transfer.transferOut();
+                    spindexer.outtake3();
+
+                    shooter.setManualPower(1);
+
+                }
+
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
+            }
+
+            spindexer.outtake3();
+
+            Actions.runBlocking(
+                    traj6.build()
+            );
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            TELE.addLine("finished");
+
+            TELE.update();
+
+            sleep (2000);
+
+
+        }
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java
new file mode 100644
index 0000000..5da26ae
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java
@@ -0,0 +1,431 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.ParallelAction;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Shooter;
+import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
+import org.firstinspires.ftc.teamcode.subsystems.Transfer;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+
+
+@Config
+@Autonomous
+public class Red extends LinearOpMode {
+
+    Robot robot;
+
+    MultipleTelemetry TELE;
+
+    MecanumDrive drive;
+
+    AprilTag aprilTag;
+
+    Shooter shooter;
+
+
+
+    public static double angle = 0.1;
+
+    Spindexer spindexer;
+
+    Transfer transfer;
+
+
+
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+
+
+
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        drive = new MecanumDrive(hardwareMap, new Pose2d(
+                0,0,0
+        ));
+
+        aprilTag = new AprilTag(robot, TELE);
+
+        shooter = new Shooter(robot, TELE);
+
+        spindexer = new Spindexer(robot, TELE);
+
+        spindexer.outtake3();
+
+        shooter.setShooterMode("MANUAL");
+
+        shooter.sethoodPosition(hoodDefault);
+
+        transfer = new Transfer(robot);
+
+        transfer.setTransferPosition(transferServo_out);
+
+        Intake intake = new Intake(robot);
+
+        robot.hood.setPosition(hoodDefault);
+
+
+
+
+
+
+
+
+        TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
+
+
+        TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .turnTo(Math.toRadians(135))
+                .strafeToLinearHeading(new Vector2d(x2, y2), h2 );
+
+
+        TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, y2, h2))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
+
+        TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+
+                .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b )
+
+                .strafeToLinearHeading(new Vector2d(x3, y3), h3 );
+
+
+        TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, y3, h3))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
+
+
+        TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .strafeToLinearHeading(new Vector2d(x1, y1+30), h1 );
+
+        while(opModeInInit()) {
+
+            if (gamepad2.dpadUpWasPressed()){
+                hoodDefault -= 0.02;
+            }
+
+            if (gamepad2.dpadDownWasPressed()){
+                hoodDefault += 0.02;
+            }
+
+            robot.hood.setPosition(hoodDefault);
+
+            shooter.setTurretPosition(0.33);
+
+            aprilTag.initTelemetry();
+
+            aprilTag.update();
+            shooter.update();
+
+            TELE.update();
+        }
+
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        if (opModeIsActive()){
+
+            robot.hood.setPosition(hoodDefault);
+
+            transfer.setTransferPower(1);
+
+            transfer.update();
+
+
+
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            traj1.build()
+                    )
+            );
+
+
+            transfer.setTransferPower(1);
+
+            transfer.update();
+
+            shooter.setManualPower(1);
+
+            double stamp = getRuntime();
+
+
+             stamp = getRuntime();
+
+            while (getRuntime()-stamp<4.5) {
+
+
+
+                shooter.moveTurret(0.31);
+
+                double time = getRuntime()-stamp;
+
+
+                if (time < 0.3) {
+
+
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
+
+                    spindexer.outtake2();
+
+
+
+                } else if (time < 3.0) {
+
+
+                    transfer.transferIn();
+                } else if (time < 4.0) {
+
+                    transfer.transferOut();
+
+                    spindexer.outtake1();
+
+                } else if (time < 4.4) {
+                    transfer.transferIn();
+                } else {
+
+                    transfer.transferOut();
+                    spindexer.outtake3();
+
+                    shooter.setManualPower(0);
+
+                }
+
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
+            }
+
+            spindexer.outtake3();
+
+            robot.intake.setPower(1);
+
+            Actions.runBlocking(
+                    traj2.build()
+            );
+
+
+            Actions.runBlocking(
+                    traj3.build()
+            );
+
+            shooter.setManualPower(1);
+
+
+
+
+            robot.intake.setPower(0);
+
+            spindexer.outtake3();
+
+
+            stamp = getRuntime();
+
+            shooter.setManualPower(1);
+            while (getRuntime()-stamp<4.5) {
+
+
+
+                shooter.moveTurret(0.31);
+
+                double time = getRuntime()-stamp;
+
+
+                if (time < 0.3) {
+
+
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
+
+                    spindexer.outtake2();
+
+
+
+                } else if (time < 3.0) {
+
+
+                    transfer.transferIn();
+                } else if (time < 4.0) {
+
+                    transfer.transferOut();
+
+                    spindexer.outtake1();
+
+                } else if (time < 4.4) {
+                    transfer.transferIn();
+                } else {
+
+                    transfer.transferOut();
+                    spindexer.outtake3();
+
+                    shooter.setManualPower(0);
+
+                }
+
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
+            }
+
+            spindexer.outtake3();
+            robot.intake.setPower(1);
+
+            Actions.runBlocking(
+                    traj4.build()
+            );
+
+
+            Actions.runBlocking(
+                    traj5.build()
+            );
+
+            shooter.setManualPower(1);
+
+
+
+
+            robot.intake.setPower(0);
+
+
+            stamp = getRuntime();
+
+            shooter.setManualPower(1);
+
+            while (getRuntime()-stamp<4.5) {
+
+
+
+                shooter.moveTurret(0.31);
+
+                double time = getRuntime()-stamp;
+
+
+                if (time < 0.3) {
+
+
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
+
+                    spindexer.outtake2();
+
+
+
+                } else if (time < 3.0) {
+
+
+                    transfer.transferIn();
+                } else if (time < 4.0) {
+
+                    transfer.transferOut();
+
+                    spindexer.outtake1();
+
+                } else if (time < 4.4) {
+                    transfer.transferIn();
+                } else {
+
+                    transfer.transferOut();
+                    spindexer.outtake3();
+
+                    shooter.setManualPower(0);
+
+                }
+
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
+            }
+
+            spindexer.outtake3();
+
+            Actions.runBlocking(
+                    traj6.build()
+            );
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            TELE.addLine("finished");
+
+            TELE.update();
+
+            sleep (2000);
+
+
+        }
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java
new file mode 100644
index 0000000..2c80fab
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+public class blank {
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
new file mode 100644
index 0000000..6cc3514
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
+
+@Config
+public class Poses {
+
+    public static double goalHeight = 42; //in inches
+
+    public static double turretHeight = 12;
+
+    public static double relativeGoalHeight = goalHeight - turretHeight;
+
+    public static double x1 = 50, y1 = 0, h1 = 0;
+
+    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135);
+
+    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135);
+
+
+    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135);
+
+    public static Pose2d teleStart = new Pose2d(x1,-10,0);
+
+
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
new file mode 100644
index 0000000..beb7514
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
@@ -0,0 +1,23 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class ServoPositions {
+
+    public static double spindexer_intakePos = 0.665;
+
+    public static double spindexer_outtakeBall3 = 0.845;
+
+    public static double spindexer_outtakeBall2 = 0.48;
+    public static double spindexer_outtakeBall1 = 0.1;
+
+    public static double transferServo_out = 0.13;
+
+    public static double transferServo_in = 0.4;
+
+    public static double hoodDefault = 0.35;
+
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java
new file mode 100644
index 0000000..53e7fcb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+public class blank {
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/disabled/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/disabled/blank.java
new file mode 100644
index 0000000..9258773
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/disabled/blank.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.disabled;
+
+public class blank {
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Drawing.java
new file mode 100644
index 0000000..764c4ee
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Drawing.java
@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode.libs.RR;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Vector2d;
+
+public final class Drawing {
+    private Drawing() {}
+
+
+    public static void drawRobot(Canvas c, Pose2d t) {
+        final double ROBOT_RADIUS = 9;
+
+        c.setStrokeWidth(1);
+        c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS);
+
+        Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
+        Vector2d p1 = t.position.plus(halfv);
+        Vector2d p2 = p1.plus(halfv);
+        c.strokeLine(p1.x, p1.y, p2.x, p2.y);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Localizer.java
new file mode 100644
index 0000000..970c400
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/Localizer.java
@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.libs.RR;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+
+/**
+ * Interface for localization methods.
+ */
+public interface Localizer {
+    void setPose(Pose2d pose);
+
+    /**
+     * Returns the current pose estimate.
+     * NOTE: Does not update the pose estimate;
+     * you must call update() to update the pose estimate.
+     * @return the Localizer's current pose
+     */
+    Pose2d getPose();
+
+    /**
+     * Updates the Localizer's pose estimate.
+     * @return the Localizer's current velocity estimate
+     */
+    PoseVelocity2d update();
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
new file mode 100644
index 0000000..45ad2e2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
@@ -0,0 +1,500 @@
+package org.firstinspires.ftc.teamcode.libs.RR;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.*;
+import com.acmerobotics.roadrunner.AngularVelConstraint;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.HolonomicController;
+import com.acmerobotics.roadrunner.MecanumKinematics;
+import com.acmerobotics.roadrunner.MinVelConstraint;
+import com.acmerobotics.roadrunner.MotorFeedforward;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Pose2dDual;
+import com.acmerobotics.roadrunner.ProfileAccelConstraint;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.TimeTrajectory;
+import com.acmerobotics.roadrunner.TimeTurn;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.TurnConstraints;
+import com.acmerobotics.roadrunner.VelConstraint;
+import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
+import com.acmerobotics.roadrunner.ftc.Encoder;
+import com.acmerobotics.roadrunner.ftc.FlightRecorder;
+import com.acmerobotics.roadrunner.ftc.LazyHardwareMapImu;
+import com.acmerobotics.roadrunner.ftc.LazyImu;
+import com.acmerobotics.roadrunner.ftc.LynxFirmware;
+import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+import com.acmerobotics.roadrunner.ftc.RawEncoder;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import org.firstinspires.ftc.teamcode.libs.RR.messages.DriveCommandMessage;
+import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumCommandMessage;
+import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage;
+import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage;
+
+import java.lang.Math;
+import java.util.Arrays;
+import java.util.LinkedList;
+import java.util.List;
+
+@Config
+public final class MecanumDrive {
+    public static class Params {
+        // IMU orientation
+        // TODO: fill in these values based on
+        //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
+        public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
+                RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
+        public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
+                RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD;
+
+        // drive model parameters
+        public double inPerTick = 0.001978956;
+        public double lateralInPerTick = 0.0013863732202094405;
+        public double trackWidthTicks = 6488.883015684446;
+
+        // feedforward parameters (in tick units)
+        public double kS = 1.2147826978829488;
+        public double kV = 0.00032;
+        public double kA = 0.000046;
+
+        // path profile parameters (in inches)
+        public double maxWheelVel = 120;
+        public double minProfileAccel = -30;
+        public double maxProfileAccel = 120;
+
+        // turn profile parameters (in radians)
+        public double maxAngVel = 2* Math.PI; // shared with path
+        public double maxAngAccel = 2* Math.PI;
+
+        // path controller gains
+        public double axialGain = 4;
+        public double lateralGain = 4;
+        public double headingGain = 4; // shared with turn
+
+        public double axialVelGain = 0.0;
+        public double lateralVelGain = 0.0;
+        public double headingVelGain = 0.0; // shared with turn
+    }
+
+    public static Params PARAMS = new Params();
+
+    public final MecanumKinematics kinematics = new MecanumKinematics(
+            PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
+
+    public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
+            PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
+    public final VelConstraint defaultVelConstraint =
+            new MinVelConstraint(Arrays.asList(
+                    kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
+                    new AngularVelConstraint(PARAMS.maxAngVel)
+            ));
+    public final AccelConstraint defaultAccelConstraint =
+            new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
+
+    public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
+
+    public final VoltageSensor voltageSensor;
+
+    public final LazyImu lazyImu;
+
+    public final Localizer localizer;
+    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
+
+    private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
+    private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
+    private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
+    private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
+
+    public class DriveLocalizer implements Localizer {
+        public final Encoder leftFront, leftBack, rightBack, rightFront;
+        public final IMU imu;
+
+        private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
+        private Rotation2d lastHeading;
+        private boolean initialized;
+        private Pose2d pose;
+
+        public DriveLocalizer(Pose2d pose) {
+            leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
+            leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
+            rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
+            rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
+
+            imu = lazyImu.get();
+
+            // TODO: reverse encoders if needed
+            //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+
+            this.pose = pose;
+        }
+
+        @Override
+        public void setPose(Pose2d pose) {
+            this.pose = pose;
+        }
+
+        @Override
+        public Pose2d getPose() {
+            return pose;
+        }
+
+        @Override
+        public PoseVelocity2d update() {
+            PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
+            PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity();
+            PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
+            PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
+
+            YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
+
+            FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
+                    leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
+
+            Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
+
+            if (!initialized) {
+                initialized = true;
+
+                lastLeftFrontPos = leftFrontPosVel.position;
+                lastLeftBackPos = leftBackPosVel.position;
+                lastRightBackPos = rightBackPosVel.position;
+                lastRightFrontPos = rightFrontPosVel.position;
+
+                lastHeading = heading;
+
+                return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0);
+            }
+
+            double headingDelta = heading.minus(lastHeading);
+            Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
+                    new DualNum<Time>(new double[]{
+                            (leftFrontPosVel.position - lastLeftFrontPos),
+                            leftFrontPosVel.velocity,
+                    }).times(PARAMS.inPerTick),
+                    new DualNum<Time>(new double[]{
+                            (leftBackPosVel.position - lastLeftBackPos),
+                            leftBackPosVel.velocity,
+                    }).times(PARAMS.inPerTick),
+                    new DualNum<Time>(new double[]{
+                            (rightBackPosVel.position - lastRightBackPos),
+                            rightBackPosVel.velocity,
+                    }).times(PARAMS.inPerTick),
+                    new DualNum<Time>(new double[]{
+                            (rightFrontPosVel.position - lastRightFrontPos),
+                            rightFrontPosVel.velocity,
+                    }).times(PARAMS.inPerTick)
+            ));
+
+            lastLeftFrontPos = leftFrontPosVel.position;
+            lastLeftBackPos = leftBackPosVel.position;
+            lastRightBackPos = rightBackPosVel.position;
+            lastRightFrontPos = rightFrontPosVel.position;
+
+            lastHeading = heading;
+
+            pose = pose.plus(new Twist2d(
+                    twist.line.value(),
+                    headingDelta
+            ));
+
+            return twist.velocity().value();
+        }
+    }
+
+    public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
+        LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
+
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+
+        // TODO: make sure your config has motors with these names (or change them)
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        leftFront = hardwareMap.get(DcMotorEx.class, "fl");
+        leftBack = hardwareMap.get(DcMotorEx.class, "bl");
+        rightBack = hardwareMap.get(DcMotorEx.class, "br");
+        rightFront = hardwareMap.get(DcMotorEx.class, "fr");
+
+        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        // TODO: reverse motor directions if needed
+        //
+        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
+                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
+
+        voltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
+
+        FlightRecorder.write("MECANUM_PARAMS", PARAMS);
+    }
+
+    public void setDrivePowers(PoseVelocity2d powers) {
+        MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
+                PoseVelocity2dDual.constant(powers, 1));
+
+        double maxPowerMag = 1;
+        for (DualNum<Time> power : wheelVels.all()) {
+            maxPowerMag = Math.max(maxPowerMag, power.value());
+        }
+
+        leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
+        leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
+        rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
+        rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
+    }
+
+    public final class FollowTrajectoryAction implements Action {
+        public final TimeTrajectory timeTrajectory;
+        private double beginTs = -1;
+
+        private final double[] xPoints, yPoints;
+
+        public FollowTrajectoryAction(TimeTrajectory t) {
+            timeTrajectory = t;
+
+            List<Double> disps = com.acmerobotics.roadrunner.Math.range(
+                    0, t.path.length(),
+                    Math.max(2, (int) Math.ceil(t.path.length() / 2)));
+            xPoints = new double[disps.size()];
+            yPoints = new double[disps.size()];
+            for (int i = 0; i < disps.size(); i++) {
+                Pose2d p = t.path.get(disps.get(i), 1).value();
+                xPoints[i] = p.position.x;
+                yPoints[i] = p.position.y;
+            }
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket p) {
+            double t;
+            if (beginTs < 0) {
+                beginTs = Actions.now();
+                t = 0;
+            } else {
+                t = Actions.now() - beginTs;
+            }
+
+            if (t >= timeTrajectory.duration) {
+                leftFront.setPower(0);
+                leftBack.setPower(0);
+                rightBack.setPower(0);
+                rightFront.setPower(0);
+
+                return false;
+            }
+
+            Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
+
+            PoseVelocity2d robotVelRobot = updatePoseEstimate();
+
+            PoseVelocity2dDual<Time> command = new HolonomicController(
+                    PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
+                    PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
+            )
+                    .compute(txWorldTarget, localizer.getPose(), robotVelRobot);
+            driveCommandWriter.write(new DriveCommandMessage(command));
+
+            MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
+            double voltage = voltageSensor.getVoltage();
+
+            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
+                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
+            double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
+            double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
+            double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
+            mecanumCommandWriter.write(new MecanumCommandMessage(
+                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
+            ));
+
+            leftFront.setPower(leftFrontPower);
+            leftBack.setPower(leftBackPower);
+            rightBack.setPower(rightBackPower);
+            rightFront.setPower(rightFrontPower);
+
+            p.put("x", localizer.getPose().position.x);
+            p.put("y", localizer.getPose().position.y);
+            p.put("heading (deg)", Math.toDegrees(localizer.getPose().heading.toDouble()));
+
+            Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
+            p.put("xError", error.position.x);
+            p.put("yError", error.position.y);
+            p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));
+
+            // only draw when active; only one drive action should be active at a time
+            Canvas c = p.fieldOverlay();
+            drawPoseHistory(c);
+
+            c.setStroke("#4CAF50");
+            Drawing.drawRobot(c, txWorldTarget.value());
+
+            c.setStroke("#3F51B5");
+            Drawing.drawRobot(c, localizer.getPose());
+
+            c.setStroke("#4CAF50FF");
+            c.setStrokeWidth(1);
+            c.strokePolyline(xPoints, yPoints);
+
+            return true;
+        }
+
+        @Override
+        public void preview(Canvas c) {
+            c.setStroke("#4CAF507A");
+            c.setStrokeWidth(1);
+            c.strokePolyline(xPoints, yPoints);
+        }
+    }
+
+    public final class TurnAction implements Action {
+        private final TimeTurn turn;
+
+        private double beginTs = -1;
+
+        public TurnAction(TimeTurn turn) {
+            this.turn = turn;
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket p) {
+            double t;
+            if (beginTs < 0) {
+                beginTs = Actions.now();
+                t = 0;
+            } else {
+                t = Actions.now() - beginTs;
+            }
+
+            if (t >= turn.duration) {
+                leftFront.setPower(0);
+                leftBack.setPower(0);
+                rightBack.setPower(0);
+                rightFront.setPower(0);
+
+                return false;
+            }
+
+            Pose2dDual<Time> txWorldTarget = turn.get(t);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
+
+            PoseVelocity2d robotVelRobot = updatePoseEstimate();
+
+            PoseVelocity2dDual<Time> command = new HolonomicController(
+                    PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
+                    PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
+            )
+                    .compute(txWorldTarget, localizer.getPose(), robotVelRobot);
+            driveCommandWriter.write(new DriveCommandMessage(command));
+
+            MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
+            double voltage = voltageSensor.getVoltage();
+            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
+                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
+            double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
+            double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
+            double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
+            mecanumCommandWriter.write(new MecanumCommandMessage(
+                    voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
+            ));
+
+            leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
+            leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
+            rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
+            rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
+
+            Canvas c = p.fieldOverlay();
+            drawPoseHistory(c);
+
+            c.setStroke("#4CAF50");
+            Drawing.drawRobot(c, txWorldTarget.value());
+
+            c.setStroke("#3F51B5");
+            Drawing.drawRobot(c, localizer.getPose());
+
+            c.setStroke("#7C4DFFFF");
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
+
+            return true;
+        }
+
+        @Override
+        public void preview(Canvas c) {
+            c.setStroke("#7C4DFF7A");
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
+        }
+    }
+
+    public PoseVelocity2d updatePoseEstimate() {
+        PoseVelocity2d vel = localizer.update();
+        poseHistory.add(localizer.getPose());
+        
+        while (poseHistory.size() > 100) {
+            poseHistory.removeFirst();
+        }
+
+        estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
+        
+        
+        return vel;
+    }
+
+    private void drawPoseHistory(Canvas c) {
+        double[] xPoints = new double[poseHistory.size()];
+        double[] yPoints = new double[poseHistory.size()];
+
+        int i = 0;
+        for (Pose2d t : poseHistory) {
+            xPoints[i] = t.position.x;
+            yPoints[i] = t.position.y;
+
+            i++;
+        }
+
+        c.setStrokeWidth(1);
+        c.setStroke("#3F51B5");
+        c.strokePolyline(xPoints, yPoints);
+    }
+
+    public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
+        return new TrajectoryActionBuilder(
+                TurnAction::new,
+                FollowTrajectoryAction::new,
+                new TrajectoryBuilderParams(
+                        1e-6,
+                        new ProfileParams(
+                                0.25, 0.1, 1e-2
+                        )
+                ),
+                beginPose, 0.0,
+                defaultTurnConstraints,
+                defaultVelConstraint, defaultAccelConstraint
+        );
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java
new file mode 100644
index 0000000..3345787
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java
@@ -0,0 +1,68 @@
+package org.firstinspires.ftc.teamcode.libs.RR;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.Rotation2d;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.ftc.OTOSKt;
+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;
+
+@Config
+public class OTOSLocalizer implements Localizer {
+    public static class Params {
+        public double angularScalar = 1.0;
+        public double linearScalar = 1.0;
+
+        // Note: units are in inches and radians
+        public SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0);
+    }
+
+    public static Params PARAMS = new Params();
+
+    public final SparkFunOTOS otos;
+    private Pose2d currentPose;
+
+    public OTOSLocalizer(HardwareMap hardwareMap, Pose2d initialPose) {
+        // TODO: make sure your config has an OTOS device with this name
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
+        currentPose = initialPose;
+        otos.setPosition(OTOSKt.toOTOSPose(currentPose));
+        otos.setLinearUnit(DistanceUnit.INCH);
+        otos.setAngularUnit(AngleUnit.RADIANS);
+
+        otos.calibrateImu();
+        otos.setLinearScalar(PARAMS.linearScalar);
+        otos.setAngularScalar(PARAMS.angularScalar);
+        otos.setOffset(PARAMS.offset);
+    }
+
+    @Override
+    public Pose2d getPose() {
+        return currentPose;
+    }
+
+    @Override
+    public void setPose(Pose2d pose) {
+        currentPose = pose;
+        otos.setPosition(OTOSKt.toOTOSPose(currentPose));
+    }
+
+    @Override
+    public PoseVelocity2d update() {
+        SparkFunOTOS.Pose2D otosPose = new SparkFunOTOS.Pose2D();
+        SparkFunOTOS.Pose2D otosVel = new SparkFunOTOS.Pose2D();
+        SparkFunOTOS.Pose2D otosAcc = new SparkFunOTOS.Pose2D();
+        otos.getPosVelAcc(otosPose, otosVel, otosAcc);
+
+        currentPose = OTOSKt.toRRPose(otosPose);
+        Vector2d fieldVel = new Vector2d(otosVel.x, otosVel.y);
+        Vector2d robotVel = Rotation2d.exp(otosPose.h).inverse().times(fieldVel);
+        return new PoseVelocity2d(robotVel, otosVel.h);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java
new file mode 100644
index 0000000..6464c63
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java
@@ -0,0 +1,73 @@
+package org.firstinspires.ftc.teamcode.libs.RR;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.Rotation2d;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit;
+
+import java.util.Objects;
+
+@Config
+public final class PinpointLocalizer implements Localizer {
+    public static class Params {
+        public double parYTicks = -3765.023079161767; // y position of the parallel encoder (in tick units)
+        public double perpXTicks = -1962.6377639490684; // x position of the perpendicular encoder (in tick units)
+    }
+
+    public static Params PARAMS = new Params();
+
+    public final GoBildaPinpointDriver driver;
+    public final GoBildaPinpointDriver.EncoderDirection initialParDirection, initialPerpDirection;
+
+    private Pose2d txWorldPinpoint;
+    private Pose2d txPinpointRobot = new Pose2d(0, 0, 0);
+
+    public PinpointLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) {
+        // TODO: make sure your config has a Pinpoint device with this name
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        driver = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
+
+        double mmPerTick = inPerTick * 25.4;
+        driver.setEncoderResolution(1 / mmPerTick, DistanceUnit.MM);
+        driver.setOffsets(mmPerTick * PARAMS.parYTicks, mmPerTick * PARAMS.perpXTicks, DistanceUnit.MM);
+
+        // TODO: reverse encoder directions if needed
+        initialParDirection = GoBildaPinpointDriver.EncoderDirection.REVERSED;
+        initialPerpDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
+
+        driver.setEncoderDirections(initialParDirection, initialPerpDirection);
+
+        driver.resetPosAndIMU();
+
+        txWorldPinpoint = initialPose;
+    }
+
+    @Override
+    public void setPose(Pose2d pose) {
+        txWorldPinpoint = pose.times(txPinpointRobot.inverse());
+    }
+
+    @Override
+    public Pose2d getPose() {
+        return txWorldPinpoint.times(txPinpointRobot);
+    }
+
+    @Override
+    public PoseVelocity2d update() {
+        driver.update();
+        if (Objects.requireNonNull(driver.getDeviceStatus()) == GoBildaPinpointDriver.DeviceStatus.READY) {
+            txPinpointRobot = new Pose2d(driver.getPosX(DistanceUnit.INCH), driver.getPosY(DistanceUnit.INCH), driver.getHeading(UnnormalizedAngleUnit.RADIANS));
+            Vector2d worldVelocity = new Vector2d(driver.getVelX(DistanceUnit.INCH), driver.getVelY(DistanceUnit.INCH));
+            Vector2d robotVelocity = Rotation2d.fromDouble(-txPinpointRobot.heading.log()).times(worldVelocity);
+
+            return new PoseVelocity2d(robotVelocity, driver.getHeadingVelocity(UnnormalizedAngleUnit.RADIANS));
+        }
+        return new PoseVelocity2d(new Vector2d(0, 0), 0);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java
new file mode 100644
index 0000000..c94dea2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java
@@ -0,0 +1,509 @@
+package org.firstinspires.ftc.teamcode.libs.RR;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.AccelConstraint;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.Actions;
+import com.acmerobotics.roadrunner.AngularVelConstraint;
+import com.acmerobotics.roadrunner.Arclength;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.MinVelConstraint;
+import com.acmerobotics.roadrunner.MotorFeedforward;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Pose2dDual;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.PoseVelocity2dDual;
+import com.acmerobotics.roadrunner.ProfileAccelConstraint;
+import com.acmerobotics.roadrunner.ProfileParams;
+import com.acmerobotics.roadrunner.RamseteController;
+import com.acmerobotics.roadrunner.TankKinematics;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.TimeTrajectory;
+import com.acmerobotics.roadrunner.TimeTurn;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.TrajectoryBuilderParams;
+import com.acmerobotics.roadrunner.TurnConstraints;
+import com.acmerobotics.roadrunner.Twist2dDual;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.Vector2dDual;
+import com.acmerobotics.roadrunner.VelConstraint;
+import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
+import com.acmerobotics.roadrunner.ftc.Encoder;
+import com.acmerobotics.roadrunner.ftc.FlightRecorder;
+import com.acmerobotics.roadrunner.ftc.LazyHardwareMapImu;
+import com.acmerobotics.roadrunner.ftc.LazyImu;
+import com.acmerobotics.roadrunner.ftc.LynxFirmware;
+import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+import com.acmerobotics.roadrunner.ftc.RawEncoder;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.libs.RR.messages.DriveCommandMessage;
+import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage;
+import org.firstinspires.ftc.teamcode.libs.RR.messages.TankCommandMessage;
+import org.firstinspires.ftc.teamcode.libs.RR.messages.TankLocalizerInputsMessage;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.LinkedList;
+import java.util.List;
+
+@Config
+public final class TankDrive {
+    public static class Params {
+        // IMU orientation
+        // TODO: fill in these values based on
+        //   see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
+        public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
+                RevHubOrientationOnRobot.LogoFacingDirection.UP;
+        public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
+                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
+        // drive model parameters
+        public double inPerTick = 0;
+        public double trackWidthTicks = 0;
+
+        // feedforward parameters (in tick units)
+        public double kS = 0;
+        public double kV = 0;
+        public double kA = 0;
+
+        // path profile parameters (in inches)
+        public double maxWheelVel = 50;
+        public double minProfileAccel = -30;
+        public double maxProfileAccel = 50;
+
+        // turn profile parameters (in radians)
+        public double maxAngVel = Math.PI; // shared with path
+        public double maxAngAccel = Math.PI;
+
+        // path controller gains
+        public double ramseteZeta = 0.7; // in the range (0, 1)
+        public double ramseteBBar = 2.0; // positive
+
+        // turn controller gains
+        public double turnGain = 0.0;
+        public double turnVelGain = 0.0;
+    }
+
+    public static Params PARAMS = new Params();
+
+    public final TankKinematics kinematics = new TankKinematics(PARAMS.inPerTick * PARAMS.trackWidthTicks);
+
+    public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
+            PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
+    public final VelConstraint defaultVelConstraint =
+            new MinVelConstraint(Arrays.asList(
+                    kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
+                    new AngularVelConstraint(PARAMS.maxAngVel)
+            ));
+    public final AccelConstraint defaultAccelConstraint =
+            new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
+
+    public final List<DcMotorEx> leftMotors, rightMotors;
+
+    public final LazyImu lazyImu;
+
+    public final VoltageSensor voltageSensor;
+
+    public final Localizer localizer;
+    private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
+
+    private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
+    private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
+    private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
+
+    private final DownsampledWriter tankCommandWriter = new DownsampledWriter("TANK_COMMAND", 50_000_000);
+
+    public class DriveLocalizer implements Localizer {
+        public final List<Encoder> leftEncs, rightEncs;
+        private Pose2d pose;
+
+        private double lastLeftPos, lastRightPos;
+        private boolean initialized;
+
+        public DriveLocalizer(Pose2d pose) {
+            {
+                List<Encoder> leftEncs = new ArrayList<>();
+                for (DcMotorEx m : leftMotors) {
+                    Encoder e = new OverflowEncoder(new RawEncoder(m));
+                    leftEncs.add(e);
+                }
+                this.leftEncs = Collections.unmodifiableList(leftEncs);
+            }
+
+            {
+                List<Encoder> rightEncs = new ArrayList<>();
+                for (DcMotorEx m : rightMotors) {
+                    Encoder e = new OverflowEncoder(new RawEncoder(m));
+                    rightEncs.add(e);
+                }
+                this.rightEncs = Collections.unmodifiableList(rightEncs);
+            }
+
+            // TODO: reverse encoder directions if needed
+            //   leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
+
+            this.pose = pose;
+        }
+
+        @Override
+        public void setPose(Pose2d pose) {
+            this.pose = pose;
+        }
+
+        @Override
+        public Pose2d getPose() {
+            return pose;
+        }
+
+        @Override
+        public PoseVelocity2d update() {
+            Twist2dDual<Time> delta;
+
+            List<PositionVelocityPair> leftReadings = new ArrayList<>(), rightReadings = new ArrayList<>();
+            double meanLeftPos = 0.0, meanLeftVel = 0.0;
+            for (Encoder e : leftEncs) {
+                PositionVelocityPair p = e.getPositionAndVelocity();
+                meanLeftPos += p.position;
+                meanLeftVel += p.velocity;
+                leftReadings.add(p);
+            }
+            meanLeftPos /= leftEncs.size();
+            meanLeftVel /= leftEncs.size();
+
+            double meanRightPos = 0.0, meanRightVel = 0.0;
+            for (Encoder e : rightEncs) {
+                PositionVelocityPair p = e.getPositionAndVelocity();
+                meanRightPos += p.position;
+                meanRightVel += p.velocity;
+                rightReadings.add(p);
+            }
+            meanRightPos /= rightEncs.size();
+            meanRightVel /= rightEncs.size();
+
+            FlightRecorder.write("TANK_LOCALIZER_INPUTS",
+                     new TankLocalizerInputsMessage(leftReadings, rightReadings));
+
+            if (!initialized) {
+                initialized = true;
+
+                lastLeftPos = meanLeftPos;
+                lastRightPos = meanRightPos;
+
+                return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0);
+
+            }
+
+            Twist2dDual<Time> twist = kinematics.forward(new TankKinematics.WheelIncrements<>(
+                    new DualNum<Time>(new double[]{
+                            meanLeftPos - lastLeftPos,
+                            meanLeftVel
+                    }).times(PARAMS.inPerTick),
+                    new DualNum<Time>(new double[]{
+                            meanRightPos - lastRightPos,
+                            meanRightVel,
+                    }).times(PARAMS.inPerTick)
+            ));
+
+            lastLeftPos = meanLeftPos;
+            lastRightPos = meanRightPos;
+
+            pose = pose.plus(twist.value());
+
+            return twist.velocity().value();
+        }
+    }
+
+    public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
+        LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
+
+        for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+            module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
+
+        // TODO: make sure your config has motors with these names (or change them)
+        //   add additional motors on each side if you have them
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "left"));
+        rightMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "right"));
+
+        for (DcMotorEx m : leftMotors) {
+            m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        }
+        for (DcMotorEx m : rightMotors) {
+            m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        }
+
+        // TODO: reverse motor directions if needed
+        //   leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
+
+        // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
+                PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
+
+        voltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+        localizer = new DriveLocalizer(pose);
+
+        FlightRecorder.write("TANK_PARAMS", PARAMS);
+    }
+
+    public void setDrivePowers(PoseVelocity2d powers) {
+        TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(
+                PoseVelocity2dDual.constant(powers, 1));
+
+        double maxPowerMag = 1;
+        for (DualNum<Time> power : wheelVels.all()) {
+            maxPowerMag = Math.max(maxPowerMag, power.value());
+        }
+
+        for (DcMotorEx m : leftMotors) {
+            m.setPower(wheelVels.left.get(0) / maxPowerMag);
+        }
+        for (DcMotorEx m : rightMotors) {
+            m.setPower(wheelVels.right.get(0) / maxPowerMag);
+        }
+    }
+
+    public final class FollowTrajectoryAction implements Action {
+        public final TimeTrajectory timeTrajectory;
+        private double beginTs = -1;
+
+        private final double[] xPoints, yPoints;
+
+        public FollowTrajectoryAction(TimeTrajectory t) {
+            timeTrajectory = t;
+
+            List<Double> disps = com.acmerobotics.roadrunner.Math.range(
+                    0, t.path.length(),
+                    Math.max(2, (int) Math.ceil(t.path.length() / 2)));
+            xPoints = new double[disps.size()];
+            yPoints = new double[disps.size()];
+            for (int i = 0; i < disps.size(); i++) {
+                Pose2d p = t.path.get(disps.get(i), 1).value();
+                xPoints[i] = p.position.x;
+                yPoints[i] = p.position.y;
+            }
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket p) {
+            double t;
+            if (beginTs < 0) {
+                beginTs = Actions.now();
+                t = 0;
+            } else {
+                t = Actions.now() - beginTs;
+            }
+
+            if (t >= timeTrajectory.duration) {
+                for (DcMotorEx m : leftMotors) {
+                    m.setPower(0);
+                }
+                for (DcMotorEx m : rightMotors) {
+                    m.setPower(0);
+                }
+
+                return false;
+            }
+
+            DualNum<Time> x = timeTrajectory.profile.get(t);
+
+            Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
+
+            updatePoseEstimate();
+
+            PoseVelocity2dDual<Time> command = new RamseteController(kinematics.trackWidth, PARAMS.ramseteZeta, PARAMS.ramseteBBar)
+                    .compute(x, txWorldTarget, localizer.getPose());
+            driveCommandWriter.write(new DriveCommandMessage(command));
+
+            TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
+            double voltage = voltageSensor.getVoltage();
+            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
+                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            double leftPower = feedforward.compute(wheelVels.left) / voltage;
+            double rightPower = feedforward.compute(wheelVels.right) / voltage;
+            tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));
+
+            for (DcMotorEx m : leftMotors) {
+                m.setPower(leftPower);
+            }
+            for (DcMotorEx m : rightMotors) {
+                m.setPower(rightPower);
+            }
+
+            p.put("x", localizer.getPose().position.x);
+            p.put("y", localizer.getPose().position.y);
+            p.put("heading (deg)", Math.toDegrees(localizer.getPose().heading.toDouble()));
+
+            Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
+            p.put("xError", error.position.x);
+            p.put("yError", error.position.y);
+            p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));
+
+            // only draw when active; only one drive action should be active at a time
+            Canvas c = p.fieldOverlay();
+            drawPoseHistory(c);
+
+            c.setStroke("#4CAF50");
+            Drawing.drawRobot(c, txWorldTarget.value());
+
+            c.setStroke("#3F51B5");
+            Drawing.drawRobot(c, localizer.getPose());
+
+            c.setStroke("#4CAF50FF");
+            c.setStrokeWidth(1);
+            c.strokePolyline(xPoints, yPoints);
+
+            return true;
+        }
+
+        @Override
+        public void preview(Canvas c) {
+            c.setStroke("#4CAF507A");
+            c.setStrokeWidth(1);
+            c.strokePolyline(xPoints, yPoints);
+        }
+    }
+
+    public final class TurnAction implements Action {
+        private final TimeTurn turn;
+
+        private double beginTs = -1;
+
+        public TurnAction(TimeTurn turn) {
+            this.turn = turn;
+        }
+
+        @Override
+        public boolean run(@NonNull TelemetryPacket p) {
+            double t;
+            if (beginTs < 0) {
+                beginTs = Actions.now();
+                t = 0;
+            } else {
+                t = Actions.now() - beginTs;
+            }
+
+            if (t >= turn.duration) {
+                for (DcMotorEx m : leftMotors) {
+                    m.setPower(0);
+                }
+                for (DcMotorEx m : rightMotors) {
+                    m.setPower(0);
+                }
+
+                return false;
+            }
+
+            Pose2dDual<Time> txWorldTarget = turn.get(t);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
+
+            PoseVelocity2d robotVelRobot = updatePoseEstimate();
+
+            PoseVelocity2dDual<Time> command = new PoseVelocity2dDual<>(
+                    Vector2dDual.constant(new Vector2d(0, 0), 3),
+                    txWorldTarget.heading.velocity().plus(
+                            PARAMS.turnGain * localizer.getPose().heading.minus(txWorldTarget.heading.value()) +
+                            PARAMS.turnVelGain * (robotVelRobot.angVel - txWorldTarget.heading.velocity().value())
+                    )
+            );
+            driveCommandWriter.write(new DriveCommandMessage(command));
+
+            TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
+            double voltage = voltageSensor.getVoltage();
+            final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
+                    PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
+            double leftPower = feedforward.compute(wheelVels.left) / voltage;
+            double rightPower = feedforward.compute(wheelVels.right) / voltage;
+            tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));
+
+            for (DcMotorEx m : leftMotors) {
+                m.setPower(leftPower);
+            }
+            for (DcMotorEx m : rightMotors) {
+                m.setPower(rightPower);
+            }
+
+            Canvas c = p.fieldOverlay();
+            drawPoseHistory(c);
+
+            c.setStroke("#4CAF50");
+            Drawing.drawRobot(c, txWorldTarget.value());
+
+            c.setStroke("#3F51B5");
+            Drawing.drawRobot(c, localizer.getPose());
+
+            c.setStroke("#7C4DFFFF");
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
+
+            return true;
+        }
+
+        @Override
+        public void preview(Canvas c) {
+            c.setStroke("#7C4DFF7A");
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
+        }
+    }
+
+    public PoseVelocity2d updatePoseEstimate() {
+        PoseVelocity2d vel = localizer.update();
+        poseHistory.add(localizer.getPose());
+
+        while (poseHistory.size() > 100) {
+            poseHistory.removeFirst();
+        }
+
+        estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
+
+
+        return vel;
+    }
+
+    private void drawPoseHistory(Canvas c) {
+        double[] xPoints = new double[poseHistory.size()];
+        double[] yPoints = new double[poseHistory.size()];
+
+        int i = 0;
+        for (Pose2d t : poseHistory) {
+            xPoints[i] = t.position.x;
+            yPoints[i] = t.position.y;
+
+            i++;
+        }
+
+        c.setStrokeWidth(1);
+        c.setStroke("#3F51B5");
+        c.strokePolyline(xPoints, yPoints);
+    }
+
+    public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
+        return new TrajectoryActionBuilder(
+                TurnAction::new,
+                FollowTrajectoryAction::new,
+                new TrajectoryBuilderParams(
+                        1e-6,
+                        new ProfileParams(
+                                0.25, 0.1, 1e-2
+                        )
+                ),
+                beginPose, 0.0,
+                defaultTurnConstraints,
+                defaultVelConstraint, defaultAccelConstraint
+        );
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java
new file mode 100644
index 0000000..b3a8220
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java
@@ -0,0 +1,113 @@
+package org.firstinspires.ftc.teamcode.libs.RR;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.Twist2dDual;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.Vector2dDual;
+import com.acmerobotics.roadrunner.ftc.Encoder;
+import com.acmerobotics.roadrunner.ftc.FlightRecorder;
+import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+import com.acmerobotics.roadrunner.ftc.RawEncoder;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.libs.RR.messages.ThreeDeadWheelInputsMessage;
+
+@Config
+public final class ThreeDeadWheelLocalizer implements Localizer {
+    public static class Params {
+        public double par0YTicks = 0.0; // y position of the first parallel encoder (in tick units)
+        public double par1YTicks = 1.0; // y position of the second parallel encoder (in tick units)
+        public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
+    }
+
+    public static Params PARAMS = new Params();
+
+    public final Encoder par0, par1, perp;
+
+    public final double inPerTick;
+
+    private int lastPar0Pos, lastPar1Pos, lastPerpPos;
+    private boolean initialized;
+    private Pose2d pose;
+
+    public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) {
+        // TODO: make sure your config has **motors** with these names (or change them)
+        //   the encoders should be plugged into the slot matching the named motor
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0")));
+        par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1")));
+        perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
+
+        // TODO: reverse encoder directions if needed
+        //   par0.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        this.inPerTick = inPerTick;
+
+        FlightRecorder.write("THREE_DEAD_WHEEL_PARAMS", PARAMS);
+
+        pose = initialPose;
+    }
+
+    @Override
+    public void setPose(Pose2d pose) {
+        this.pose = pose;
+    }
+
+    @Override
+    public Pose2d getPose() {
+        return pose;
+    }
+
+    @Override
+    public PoseVelocity2d update() {
+        PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
+        PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
+        PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
+
+        FlightRecorder.write("THREE_DEAD_WHEEL_INPUTS", new ThreeDeadWheelInputsMessage(par0PosVel, par1PosVel, perpPosVel));
+
+        if (!initialized) {
+            initialized = true;
+
+            lastPar0Pos = par0PosVel.position;
+            lastPar1Pos = par1PosVel.position;
+            lastPerpPos = perpPosVel.position;
+
+            return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0);
+        }
+
+        int par0PosDelta = par0PosVel.position - lastPar0Pos;
+        int par1PosDelta = par1PosVel.position - lastPar1Pos;
+        int perpPosDelta = perpPosVel.position - lastPerpPos;
+
+        Twist2dDual<Time> twist = new Twist2dDual<>(
+                new Vector2dDual<>(
+                        new DualNum<Time>(new double[] {
+                                (PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
+                                (PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
+                        }).times(inPerTick),
+                        new DualNum<Time>(new double[] {
+                                (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
+                                (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
+                        }).times(inPerTick)
+                ),
+                new DualNum<>(new double[] {
+                        (par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
+                        (par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
+                })
+        );
+
+        lastPar0Pos = par0PosVel.position;
+        lastPar1Pos = par1PosVel.position;
+        lastPerpPos = perpPosVel.position;
+
+        pose = pose.plus(twist.value());
+        return twist.velocity().value();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java
new file mode 100644
index 0000000..d634311
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java
@@ -0,0 +1,143 @@
+package org.firstinspires.ftc.teamcode.libs.RR;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.Rotation2d;
+import com.acmerobotics.roadrunner.Time;
+import com.acmerobotics.roadrunner.Twist2dDual;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.Vector2dDual;
+import com.acmerobotics.roadrunner.ftc.Encoder;
+import com.acmerobotics.roadrunner.ftc.FlightRecorder;
+import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+import com.acmerobotics.roadrunner.ftc.RawEncoder;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import org.firstinspires.ftc.teamcode.libs.RR.messages.TwoDeadWheelInputsMessage;
+
+@Config
+public final class TwoDeadWheelLocalizer implements Localizer {
+    public static class Params {
+        public double parYTicks = 0.0; // y position of the parallel encoder (in tick units)
+        public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
+    }
+
+    public static Params PARAMS = new Params();
+
+    public final Encoder par, perp;
+    public final IMU imu;
+
+    private int lastParPos, lastPerpPos;
+    private Rotation2d lastHeading;
+
+    private final double inPerTick;
+
+    private double lastRawHeadingVel, headingVelOffset;
+    private boolean initialized;
+    private Pose2d pose;
+
+    public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick, Pose2d initialPose) {
+        // TODO: make sure your config has **motors** with these names (or change them)
+        //   the encoders should be plugged into the slot matching the named motor
+        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
+        par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));
+        perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
+
+        // TODO: reverse encoder directions if needed
+        //   par.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        this.imu = imu;
+
+        this.inPerTick = inPerTick;
+
+        FlightRecorder.write("TWO_DEAD_WHEEL_PARAMS", PARAMS);
+
+        pose = initialPose;
+    }
+
+    @Override
+    public void setPose(Pose2d pose) {
+        this.pose = pose;
+    }
+
+    @Override
+    public Pose2d getPose() {
+        return pose;
+    }
+
+    @Override
+    public PoseVelocity2d update() {
+        PositionVelocityPair parPosVel = par.getPositionAndVelocity();
+        PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
+
+        YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
+        // Use degrees here to work around https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1070
+        AngularVelocity angularVelocityDegrees = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+        AngularVelocity angularVelocity = new AngularVelocity(
+                UnnormalizedAngleUnit.RADIANS,
+                (float) Math.toRadians(angularVelocityDegrees.xRotationRate),
+                (float) Math.toRadians(angularVelocityDegrees.yRotationRate),
+                (float) Math.toRadians(angularVelocityDegrees.zRotationRate),
+                angularVelocityDegrees.acquisitionTime
+        );
+
+        FlightRecorder.write("TWO_DEAD_WHEEL_INPUTS", new TwoDeadWheelInputsMessage(parPosVel, perpPosVel, angles, angularVelocity));
+
+        Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
+
+        // see https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/617
+        double rawHeadingVel = angularVelocity.zRotationRate;
+        if (Math.abs(rawHeadingVel - lastRawHeadingVel) > Math.PI) {
+            headingVelOffset -= Math.signum(rawHeadingVel) * 2 * Math.PI;
+        }
+        lastRawHeadingVel = rawHeadingVel;
+        double headingVel = headingVelOffset + rawHeadingVel;
+
+        if (!initialized) {
+            initialized = true;
+
+            lastParPos = parPosVel.position;
+            lastPerpPos = perpPosVel.position;
+            lastHeading = heading;
+
+            return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0);
+        }
+
+        int parPosDelta = parPosVel.position - lastParPos;
+        int perpPosDelta = perpPosVel.position - lastPerpPos;
+        double headingDelta = heading.minus(lastHeading);
+
+        Twist2dDual<Time> twist = new Twist2dDual<>(
+                new Vector2dDual<>(
+                        new DualNum<Time>(new double[] {
+                                parPosDelta - PARAMS.parYTicks * headingDelta,
+                                parPosVel.velocity - PARAMS.parYTicks * headingVel,
+                        }).times(inPerTick),
+                        new DualNum<Time>(new double[] {
+                                perpPosDelta - PARAMS.perpXTicks * headingDelta,
+                                perpPosVel.velocity - PARAMS.perpXTicks * headingVel,
+                        }).times(inPerTick)
+                ),
+                new DualNum<>(new double[] {
+                        headingDelta,
+                        headingVel,
+                })
+        );
+
+        lastParPos = parPosVel.position;
+        lastPerpPos = perpPosVel.position;
+        lastHeading = heading;
+
+        pose = pose.plus(twist.value());
+        return twist.velocity().value();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/DriveCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/DriveCommandMessage.java
new file mode 100644
index 0000000..c22cb3d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/DriveCommandMessage.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.libs.RR.messages;
+
+import com.acmerobotics.roadrunner.PoseVelocity2dDual;
+import com.acmerobotics.roadrunner.Time;
+
+public final class DriveCommandMessage {
+    public long timestamp;
+    public double forwardVelocity;
+    public double forwardAcceleration;
+    public double lateralVelocity;
+    public double lateralAcceleration;
+    public double angularVelocity;
+    public double angularAcceleration;
+
+    public DriveCommandMessage(PoseVelocity2dDual<Time> poseVelocity) {
+        this.timestamp = System.nanoTime();
+        this.forwardVelocity = poseVelocity.linearVel.x.get(0);
+        this.forwardAcceleration = poseVelocity.linearVel.x.get(1);
+        this.lateralVelocity = poseVelocity.linearVel.y.get(0);
+        this.lateralAcceleration = poseVelocity.linearVel.y.get(1);
+        this.angularVelocity = poseVelocity.angVel.get(0);
+        this.angularAcceleration = poseVelocity.angVel.get(1);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/MecanumCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/MecanumCommandMessage.java
new file mode 100644
index 0000000..a443c9e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/MecanumCommandMessage.java
@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.libs.RR.messages;
+
+public final class MecanumCommandMessage {
+    public long timestamp;
+    public double voltage;
+    public double leftFrontPower;
+    public double leftBackPower;
+    public double rightBackPower;
+    public double rightFrontPower;
+
+    public MecanumCommandMessage(double voltage, double leftFrontPower, double leftBackPower, double rightBackPower, double rightFrontPower) {
+        this.timestamp = System.nanoTime();
+        this.voltage = voltage;
+        this.leftFrontPower = leftFrontPower;
+        this.leftBackPower = leftBackPower;
+        this.rightBackPower = rightBackPower;
+        this.rightFrontPower = rightFrontPower;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/MecanumLocalizerInputsMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/MecanumLocalizerInputsMessage.java
new file mode 100644
index 0000000..034a9f3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/MecanumLocalizerInputsMessage.java
@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.libs.RR.messages;
+
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+public final class MecanumLocalizerInputsMessage {
+    public long timestamp;
+    public PositionVelocityPair leftFront;
+    public PositionVelocityPair leftBack;
+    public PositionVelocityPair rightBack;
+    public PositionVelocityPair rightFront;
+    public double yaw;
+    public double pitch;
+    public double roll;
+
+    public MecanumLocalizerInputsMessage(PositionVelocityPair leftFront, PositionVelocityPair leftBack, PositionVelocityPair rightBack, PositionVelocityPair rightFront, YawPitchRollAngles angles) {
+        this.timestamp = System.nanoTime();
+        this.leftFront = leftFront;
+        this.leftBack = leftBack;
+        this.rightBack = rightBack;
+        this.rightFront = rightFront;
+        {
+            this.yaw = angles.getYaw(AngleUnit.RADIANS);
+            this.pitch = angles.getPitch(AngleUnit.RADIANS);
+            this.roll = angles.getRoll(AngleUnit.RADIANS);
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/PoseMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/PoseMessage.java
new file mode 100644
index 0000000..417459f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/PoseMessage.java
@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.libs.RR.messages;
+
+import com.acmerobotics.roadrunner.Pose2d;
+
+public final class PoseMessage {
+    public long timestamp;
+    public double x;
+    public double y;
+    public double heading;
+
+    public PoseMessage(Pose2d pose) {
+        this.timestamp = System.nanoTime();
+        this.x = pose.position.x;
+        this.y = pose.position.y;
+        this.heading = pose.heading.toDouble();
+    }
+}
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TankCommandMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TankCommandMessage.java
new file mode 100644
index 0000000..3be8b5f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TankCommandMessage.java
@@ -0,0 +1,15 @@
+package org.firstinspires.ftc.teamcode.libs.RR.messages;
+
+public final class TankCommandMessage {
+    public long timestamp;
+    public double voltage;
+    public double leftPower;
+    public double rightPower;
+
+    public TankCommandMessage(double voltage, double leftPower, double rightPower) {
+        this.timestamp = System.nanoTime();
+        this.voltage = voltage;
+        this.leftPower = leftPower;
+        this.rightPower = rightPower;
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TankLocalizerInputsMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TankLocalizerInputsMessage.java
new file mode 100644
index 0000000..d5dd970
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TankLocalizerInputsMessage.java
@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.libs.RR.messages;
+
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+
+import java.util.List;
+
+public final class TankLocalizerInputsMessage {
+    public long timestamp;
+    public PositionVelocityPair[] left;
+    public PositionVelocityPair[] right;
+
+    public TankLocalizerInputsMessage(List<PositionVelocityPair> left, List<PositionVelocityPair> right) {
+        this.timestamp = System.nanoTime();
+        this.left = left.toArray(new PositionVelocityPair[0]);
+        this.right = right.toArray(new PositionVelocityPair[0]);
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/ThreeDeadWheelInputsMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/ThreeDeadWheelInputsMessage.java
new file mode 100644
index 0000000..1cf57bd
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/ThreeDeadWheelInputsMessage.java
@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.libs.RR.messages;
+
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+
+public final class ThreeDeadWheelInputsMessage {
+    public long timestamp;
+    public PositionVelocityPair par0;
+    public PositionVelocityPair par1;
+    public PositionVelocityPair perp;
+
+    public ThreeDeadWheelInputsMessage(PositionVelocityPair par0, PositionVelocityPair par1, PositionVelocityPair perp) {
+        this.timestamp = System.nanoTime();
+        this.par0 = par0;
+        this.par1 = par1;
+        this.perp = perp;
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TwoDeadWheelInputsMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TwoDeadWheelInputsMessage.java
new file mode 100644
index 0000000..79d2b6b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/messages/TwoDeadWheelInputsMessage.java
@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.libs.RR.messages;
+
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+public final class TwoDeadWheelInputsMessage {
+    public long timestamp;
+    public PositionVelocityPair par;
+    public PositionVelocityPair perp;
+    public double yaw;
+    public double pitch;
+    public double roll;
+    public double xRotationRate;
+    public double yRotationRate;
+    public double zRotationRate;
+
+    public TwoDeadWheelInputsMessage(PositionVelocityPair par, PositionVelocityPair perp, YawPitchRollAngles angles, AngularVelocity angularVelocity) {
+        this.timestamp = System.nanoTime();
+        this.par = par;
+        this.perp = perp;
+        {
+            this.yaw = angles.getYaw(AngleUnit.RADIANS);
+            this.pitch = angles.getPitch(AngleUnit.RADIANS);
+            this.roll = angles.getRoll(AngleUnit.RADIANS);
+        }
+        {
+            this.xRotationRate = angularVelocity.xRotationRate;
+            this.yRotationRate = angularVelocity.yRotationRate;
+            this.zRotationRate = angularVelocity.zRotationRate;
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/LocalizationTest.java
new file mode 100644
index 0000000..568d4d1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/LocalizationTest.java
@@ -0,0 +1,78 @@
+package org.firstinspires.ftc.teamcode.libs.RR.tuning;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.libs.RR.Drawing;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.libs.RR.TankDrive;
+
+public class LocalizationTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            waitForStart();
+
+            while (opModeIsActive()) {
+                drive.setDrivePowers(new PoseVelocity2d(
+                        new Vector2d(
+                                -gamepad1.left_stick_y,
+                                -gamepad1.left_stick_x
+                        ),
+                        -gamepad1.right_stick_x
+                ));
+
+                drive.updatePoseEstimate();
+
+                Pose2d pose = drive.localizer.getPose();
+                telemetry.addData("x", pose.position.x);
+                telemetry.addData("y", pose.position.y);
+                telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
+                telemetry.update();
+
+                TelemetryPacket packet = new TelemetryPacket();
+                packet.fieldOverlay().setStroke("#3F51B5");
+                Drawing.drawRobot(packet.fieldOverlay(), pose);
+                FtcDashboard.getInstance().sendTelemetryPacket(packet);
+            }
+        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
+            TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            waitForStart();
+
+            while (opModeIsActive()) {
+                drive.setDrivePowers(new PoseVelocity2d(
+                        new Vector2d(
+                                -gamepad1.left_stick_y,
+                                0.0
+                        ),
+                        -gamepad1.right_stick_x
+                ));
+
+                drive.updatePoseEstimate();
+
+                Pose2d pose = drive.localizer.getPose();
+                telemetry.addData("x", pose.position.x);
+                telemetry.addData("y", pose.position.y);
+                telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
+                telemetry.update();
+
+                TelemetryPacket packet = new TelemetryPacket();
+                packet.fieldOverlay().setStroke("#3F51B5");
+                Drawing.drawRobot(packet.fieldOverlay(), pose);
+                FtcDashboard.getInstance().sendTelemetryPacket(packet);
+            }
+        } else {
+            throw new RuntimeException();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/ManualFeedbackTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/ManualFeedbackTuner.java
new file mode 100644
index 0000000..59ee5ee
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/ManualFeedbackTuner.java
@@ -0,0 +1,63 @@
+package org.firstinspires.ftc.teamcode.libs.RR.tuning;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.libs.RR.TankDrive;
+import org.firstinspires.ftc.teamcode.libs.RR.ThreeDeadWheelLocalizer;
+import org.firstinspires.ftc.teamcode.libs.RR.TwoDeadWheelLocalizer;
+
+public final class ManualFeedbackTuner extends LinearOpMode {
+    public static double DISTANCE = 64;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+            
+            if (drive.localizer instanceof TwoDeadWheelLocalizer) {
+                if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) {
+                    throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
+                }
+            } else if (drive.localizer instanceof ThreeDeadWheelLocalizer) {
+                if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) {
+                    throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
+                }
+            }
+            waitForStart();
+
+            while (opModeIsActive()) {
+                Actions.runBlocking(
+                    drive.actionBuilder(new Pose2d(0, 0, 0))
+                            .lineToX(DISTANCE)
+                            .lineToX(0)
+                            .build());
+            }
+        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
+            TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            if (drive.localizer instanceof TwoDeadWheelLocalizer) {
+                if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) {
+                    throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
+                }
+            } else if (drive.localizer instanceof ThreeDeadWheelLocalizer) {
+                if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) {
+                    throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
+                }
+            }
+            waitForStart();
+
+            while (opModeIsActive()) {
+                Actions.runBlocking(
+                    drive.actionBuilder(new Pose2d(0, 0, 0))
+                            .lineToX(DISTANCE)
+                            .lineToX(0)
+                            .build());
+            }
+        } else {
+            throw new RuntimeException();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/SplineTest.java
new file mode 100644
index 0000000..6075df6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/SplineTest.java
@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.teamcode.libs.RR.tuning;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.libs.RR.TankDrive;
+
+public final class SplineTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Pose2d beginPose = new Pose2d(0, 0, 0);
+        if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            MecanumDrive drive = new MecanumDrive(hardwareMap, beginPose);
+
+            waitForStart();
+
+            Actions.runBlocking(
+                drive.actionBuilder(beginPose)
+                        .splineTo(new Vector2d(30, 30), Math.PI / 2)
+                        .splineTo(new Vector2d(0, 60), Math.PI)
+                        .build());
+        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
+            TankDrive drive = new TankDrive(hardwareMap, beginPose);
+
+            waitForStart();
+
+            Actions.runBlocking(
+                    drive.actionBuilder(beginPose)
+                            .splineTo(new Vector2d(30, 30), Math.PI / 2)
+                            .splineTo(new Vector2d(0, 60), Math.PI)
+                            .build());
+        } else {
+            throw new RuntimeException();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/TuningOpModes.java
new file mode 100644
index 0000000..effcadf
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/tuning/TuningOpModes.java
@@ -0,0 +1,321 @@
+package org.firstinspires.ftc.teamcode.libs.RR.tuning;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.reflection.ReflectionConfig;
+import com.acmerobotics.roadrunner.MotorFeedforward;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.ftc.AngularRampLogger;
+import com.acmerobotics.roadrunner.ftc.DeadWheelDirectionDebugger;
+import com.acmerobotics.roadrunner.ftc.DriveType;
+import com.acmerobotics.roadrunner.ftc.DriveView;
+import com.acmerobotics.roadrunner.ftc.DriveViewFactory;
+import com.acmerobotics.roadrunner.ftc.Encoder;
+import com.acmerobotics.roadrunner.ftc.EncoderGroup;
+import com.acmerobotics.roadrunner.ftc.EncoderRef;
+import com.acmerobotics.roadrunner.ftc.ForwardPushTest;
+import com.acmerobotics.roadrunner.ftc.ForwardRampLogger;
+import com.acmerobotics.roadrunner.ftc.LateralPushTest;
+import com.acmerobotics.roadrunner.ftc.LateralRampLogger;
+import com.acmerobotics.roadrunner.ftc.LazyImu;
+import com.acmerobotics.roadrunner.ftc.LynxQuadratureEncoderGroup;
+import com.acmerobotics.roadrunner.ftc.ManualFeedforwardTuner;
+import com.acmerobotics.roadrunner.ftc.MecanumMotorDirectionDebugger;
+import com.acmerobotics.roadrunner.ftc.OTOSAngularScalarTuner;
+import com.acmerobotics.roadrunner.ftc.OTOSEncoderGroup;
+import com.acmerobotics.roadrunner.ftc.OTOSHeadingOffsetTuner;
+import com.acmerobotics.roadrunner.ftc.OTOSIMU;
+import com.acmerobotics.roadrunner.ftc.OTOSLinearScalarTuner;
+import com.acmerobotics.roadrunner.ftc.OTOSPositionOffsetTuner;
+import com.acmerobotics.roadrunner.ftc.PinpointEncoderGroup;
+import com.acmerobotics.roadrunner.ftc.PinpointIMU;
+import com.acmerobotics.roadrunner.ftc.PinpointView;
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit;
+import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.libs.RR.OTOSLocalizer;
+import org.firstinspires.ftc.teamcode.libs.RR.PinpointLocalizer;
+import org.firstinspires.ftc.teamcode.libs.RR.TankDrive;
+import org.firstinspires.ftc.teamcode.libs.RR.ThreeDeadWheelLocalizer;
+import org.firstinspires.ftc.teamcode.libs.RR.TwoDeadWheelLocalizer;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public final class TuningOpModes {
+    // TODO: change this to TankDrive.class if you're using tank
+    public static final Class<?> DRIVE_CLASS = MecanumDrive.class;
+
+    public static final String GROUP = "quickstart";
+    public static final boolean DISABLED = false;
+
+    private TuningOpModes() {}
+
+    private static OpModeMeta metaForClass(Class<? extends OpMode> cls) {
+        return new OpModeMeta.Builder()
+                .setName(cls.getSimpleName())
+                .setGroup(GROUP)
+                .setFlavor(OpModeMeta.Flavor.TELEOP)
+                .build();
+    }
+
+    private static PinpointView makePinpointView(PinpointLocalizer pl) {
+        return new PinpointView() {
+            GoBildaPinpointDriver.EncoderDirection parDirection = pl.initialParDirection;
+            GoBildaPinpointDriver.EncoderDirection perpDirection = pl.initialPerpDirection;
+
+            @Override
+            public void update() {
+                pl.driver.update();
+            }
+
+            @Override
+            public int getParEncoderPosition() {
+                return pl.driver.getEncoderX();
+            }
+
+            @Override
+            public int getPerpEncoderPosition() {
+                return pl.driver.getEncoderY();
+            }
+
+            @Override
+            public float getHeadingVelocity(UnnormalizedAngleUnit unit) {
+                return (float) pl.driver.getHeadingVelocity(unit);
+            }
+
+            @Override
+            public void setParDirection(@NonNull DcMotorSimple.Direction direction) {
+                parDirection = direction == DcMotorSimple.Direction.FORWARD ?
+                        GoBildaPinpointDriver.EncoderDirection.FORWARD :
+                        GoBildaPinpointDriver.EncoderDirection.REVERSED;
+                pl.driver.setEncoderDirections(parDirection, perpDirection);
+            }
+
+            @Override
+            public DcMotorSimple.Direction getParDirection() {
+                return parDirection == GoBildaPinpointDriver.EncoderDirection.FORWARD ?
+                        DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE;
+            }
+
+            @Override
+            public void setPerpDirection(@NonNull DcMotorSimple.Direction direction) {
+                perpDirection = direction == DcMotorSimple.Direction.FORWARD ?
+                        GoBildaPinpointDriver.EncoderDirection.FORWARD :
+                        GoBildaPinpointDriver.EncoderDirection.REVERSED;
+                pl.driver.setEncoderDirections(parDirection, perpDirection);
+            }
+
+            @Override
+            public DcMotorSimple.Direction getPerpDirection() {
+                return perpDirection == GoBildaPinpointDriver.EncoderDirection.FORWARD ?
+                        DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE;
+            }
+        };
+    }
+
+    @OpModeRegistrar
+    public static void register(OpModeManager manager) {
+        if (DISABLED) return;
+
+        DriveViewFactory dvf;
+        if (DRIVE_CLASS.equals(MecanumDrive.class)) {
+            dvf = hardwareMap -> {
+                MecanumDrive md = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+                LazyImu lazyImu = md.lazyImu;
+
+                List<EncoderGroup> encoderGroups = new ArrayList<>();
+                List<EncoderRef> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
+                List<EncoderRef> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
+                if (md.localizer instanceof MecanumDrive.DriveLocalizer) {
+                    MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) md.localizer;
+                    encoderGroups.add(new LynxQuadratureEncoderGroup(
+                            hardwareMap.getAll(LynxModule.class),
+                            Arrays.asList(dl.leftFront, dl.leftBack, dl.rightFront, dl.rightBack)
+                    ));
+                    leftEncs.add(new EncoderRef(0, 0));
+                    leftEncs.add(new EncoderRef(0, 1));
+                    rightEncs.add(new EncoderRef(0, 2));
+                    rightEncs.add(new EncoderRef(0, 3));
+                } else if (md.localizer instanceof ThreeDeadWheelLocalizer) {
+                    ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer;
+                    encoderGroups.add(new LynxQuadratureEncoderGroup(
+                            hardwareMap.getAll(LynxModule.class),
+                            Arrays.asList(dl.par0, dl.par1, dl.perp)
+                    ));
+                    parEncs.add(new EncoderRef(0, 0));
+                    parEncs.add(new EncoderRef(0, 1));
+                    perpEncs.add(new EncoderRef(0, 2));
+                } else if (md.localizer instanceof TwoDeadWheelLocalizer) {
+                    TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) md.localizer;
+                    encoderGroups.add(new LynxQuadratureEncoderGroup(
+                            hardwareMap.getAll(LynxModule.class),
+                            Arrays.asList(dl.par, dl.perp)
+                    ));
+                    parEncs.add(new EncoderRef(0, 0));
+                    perpEncs.add(new EncoderRef(0, 1));
+                } else if (md.localizer instanceof OTOSLocalizer) {
+                    OTOSLocalizer ol = (OTOSLocalizer) md.localizer;
+                    encoderGroups.add(new OTOSEncoderGroup(ol.otos));
+                    parEncs.add(new EncoderRef(0, 0));
+                    perpEncs.add(new EncoderRef(0, 1));
+                    lazyImu = new OTOSIMU(ol.otos);
+                }  else if (md.localizer instanceof PinpointLocalizer) {
+                    PinpointView pv = makePinpointView((PinpointLocalizer) md.localizer);
+                    encoderGroups.add(new PinpointEncoderGroup(pv));
+                    parEncs.add(new EncoderRef(0, 0));
+                    perpEncs.add(new EncoderRef(0, 1));
+                    lazyImu = new PinpointIMU(pv);
+                } else {
+                    throw new RuntimeException("unknown localizer: " + md.localizer.getClass().getName());
+                }
+
+                return new DriveView(
+                    DriveType.MECANUM,
+                        MecanumDrive.PARAMS.inPerTick,
+                        MecanumDrive.PARAMS.maxWheelVel,
+                        MecanumDrive.PARAMS.minProfileAccel,
+                        MecanumDrive.PARAMS.maxProfileAccel,
+                        encoderGroups,
+                        Arrays.asList(
+                                md.leftFront,
+                                md.leftBack
+                        ),
+                        Arrays.asList(
+                                md.rightFront,
+                                md.rightBack
+                        ),
+                        leftEncs,
+                        rightEncs,
+                        parEncs,
+                        perpEncs,
+                        lazyImu,
+                        md.voltageSensor,
+                        () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
+                                MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
+                                MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick),
+                        0
+                );
+            };
+        } else if (DRIVE_CLASS.equals(TankDrive.class)) {
+            dvf = hardwareMap -> {
+                TankDrive td = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
+                LazyImu lazyImu = td.lazyImu;
+
+                List<EncoderGroup> encoderGroups = new ArrayList<>();
+                List<EncoderRef> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
+                List<EncoderRef> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
+                if (td.localizer instanceof TankDrive.DriveLocalizer) {
+                    TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) td.localizer;
+                    List<Encoder> allEncoders = new ArrayList<>();
+                    allEncoders.addAll(dl.leftEncs);
+                    allEncoders.addAll(dl.rightEncs);
+                    encoderGroups.add(new LynxQuadratureEncoderGroup(
+                            hardwareMap.getAll(LynxModule.class),
+                            allEncoders
+                    ));
+                    for (int i = 0; i < dl.leftEncs.size(); i++) {
+                        leftEncs.add(new EncoderRef(0, i));
+                    }
+                    for (int i = 0; i < dl.rightEncs.size(); i++) {
+                        rightEncs.add(new EncoderRef(0, dl.leftEncs.size() + i));
+                    }
+                } else if (td.localizer instanceof ThreeDeadWheelLocalizer) {
+                    ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) td.localizer;
+                    encoderGroups.add(new LynxQuadratureEncoderGroup(
+                            hardwareMap.getAll(LynxModule.class),
+                            Arrays.asList(dl.par0, dl.par1, dl.perp)
+                    ));
+                    parEncs.add(new EncoderRef(0, 0));
+                    parEncs.add(new EncoderRef(0, 1));
+                    perpEncs.add(new EncoderRef(0, 2));
+                } else if (td.localizer instanceof TwoDeadWheelLocalizer) {
+                    TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) td.localizer;
+                    encoderGroups.add(new LynxQuadratureEncoderGroup(
+                            hardwareMap.getAll(LynxModule.class),
+                            Arrays.asList(dl.par, dl.perp)
+                    ));
+                    parEncs.add(new EncoderRef(0, 0));
+                    perpEncs.add(new EncoderRef(0, 1));
+                }  else if (td.localizer instanceof PinpointLocalizer) {
+                    PinpointView pv = makePinpointView((PinpointLocalizer) td.localizer);
+                    encoderGroups.add(new PinpointEncoderGroup(pv));
+                    parEncs.add(new EncoderRef(0, 0));
+                    perpEncs.add(new EncoderRef(0, 1));
+                    lazyImu = new PinpointIMU(pv);
+                } else if (td.localizer instanceof OTOSLocalizer) {
+                    OTOSLocalizer ol = (OTOSLocalizer) td.localizer;
+                    encoderGroups.add(new OTOSEncoderGroup(ol.otos));
+                    parEncs.add(new EncoderRef(0, 0));
+                    perpEncs.add(new EncoderRef(0, 1));
+                    lazyImu = new OTOSIMU(ol.otos);
+                } else {
+                    throw new RuntimeException("unknown localizer: " + td.localizer.getClass().getName());
+                }
+
+                return new DriveView(
+                        DriveType.TANK,
+                        TankDrive.PARAMS.inPerTick,
+                        TankDrive.PARAMS.maxWheelVel,
+                        TankDrive.PARAMS.minProfileAccel,
+                        TankDrive.PARAMS.maxProfileAccel,
+                        encoderGroups,
+                        td.leftMotors,
+                        td.rightMotors,
+                        leftEncs,
+                        rightEncs,
+                        parEncs,
+                        perpEncs,
+                        lazyImu,
+                        td.voltageSensor,
+                        () -> new MotorFeedforward(TankDrive.PARAMS.kS,
+                                TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick,
+                                TankDrive.PARAMS.kA / TankDrive.PARAMS.inPerTick),
+                        0
+                );
+            };
+        } else {
+            throw new RuntimeException();
+        }
+
+        manager.register(metaForClass(AngularRampLogger.class), new AngularRampLogger(dvf));
+        manager.register(metaForClass(ForwardPushTest.class), new ForwardPushTest(dvf));
+        manager.register(metaForClass(ForwardRampLogger.class), new ForwardRampLogger(dvf));
+        manager.register(metaForClass(LateralPushTest.class), new LateralPushTest(dvf));
+        manager.register(metaForClass(LateralRampLogger.class), new LateralRampLogger(dvf));
+        manager.register(metaForClass(ManualFeedforwardTuner.class), new ManualFeedforwardTuner(dvf));
+        manager.register(metaForClass(MecanumMotorDirectionDebugger.class), new MecanumMotorDirectionDebugger(dvf));
+        manager.register(metaForClass(DeadWheelDirectionDebugger.class), new DeadWheelDirectionDebugger(dvf));
+
+        manager.register(metaForClass(ManualFeedbackTuner.class), ManualFeedbackTuner.class);
+        manager.register(metaForClass(SplineTest.class), SplineTest.class);
+        manager.register(metaForClass(LocalizationTest.class), LocalizationTest.class);
+
+        manager.register(metaForClass(OTOSAngularScalarTuner.class), new OTOSAngularScalarTuner(dvf));
+        manager.register(metaForClass(OTOSLinearScalarTuner.class), new OTOSLinearScalarTuner(dvf));
+        manager.register(metaForClass(OTOSHeadingOffsetTuner.class), new OTOSHeadingOffsetTuner(dvf));
+        manager.register(metaForClass(OTOSPositionOffsetTuner.class), new OTOSPositionOffsetTuner(dvf));
+
+        FtcDashboard.getInstance().withConfigRoot(configRoot -> {
+            for (Class<?> c : Arrays.asList(
+                    AngularRampLogger.class,
+                    ForwardRampLogger.class,
+                    LateralRampLogger.class,
+                    ManualFeedforwardTuner.class,
+                    MecanumMotorDirectionDebugger.class,
+                    ManualFeedbackTuner.class
+            )) {
+                configRoot.putVariable(c.getSimpleName(), ReflectionConfig.createVariableFromClass(c));
+            }
+        });
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java
new file mode 100644
index 0000000..d596ce1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java
@@ -0,0 +1,19 @@
+package org.firstinspires.ftc.teamcode.libs.pedroPathing;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.ftc.FollowerBuilder;
+import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class Constants {
+    public static FollowerConstants followerConstants = new FollowerConstants();
+
+    public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
+
+    public static Follower createFollower(HardwareMap hardwareMap) {
+        return new FollowerBuilder(followerConstants, hardwareMap)
+                .pathConstraints(pathConstraints)
+                .build();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java
new file mode 100644
index 0000000..da2b0d6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java
@@ -0,0 +1,1325 @@
+package org.firstinspires.ftc.teamcode.libs.pedroPathing;
+
+import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.changes;
+import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrent;
+import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrentAndHistory;
+import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.follower;
+import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.stopRobot;
+import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.telemetryM;
+
+
+import com.bylazar.configurables.PanelsConfigurables;
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.configurables.annotations.IgnoreConfigurable;
+import com.bylazar.field.FieldManager;
+import com.bylazar.field.PanelsField;
+import com.bylazar.field.Style;
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.*;
+import com.pedropathing.math.*;
+import com.pedropathing.paths.*;
+import com.pedropathing.telemetry.SelectableOpMode;
+import com.pedropathing.util.*;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * This is the Tuning class. It contains a selection menu for various tuning OpModes.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 6/26/2025
+ */
+@Configurable
+@TeleOp(name = "Tuning", group = "Pedro Pathing")
+public class Tuning extends SelectableOpMode {
+    public static Follower follower;
+
+    @IgnoreConfigurable
+    static PoseHistory poseHistory;
+
+    @IgnoreConfigurable
+    static TelemetryManager telemetryM;
+
+    @IgnoreConfigurable
+    static ArrayList<String> changes = new ArrayList<>();
+
+    public Tuning() {
+        super("Select a Tuning OpMode", s -> {
+            s.folder("Localization", l -> {
+                l.add("Localization Test", LocalizationTest::new);
+                l.add("Forward Tuner", ForwardTuner::new);
+                l.add("Lateral Tuner", LateralTuner::new);
+                l.add("Turn Tuner", TurnTuner::new);
+            });
+            s.folder("Automatic", a -> {
+                a.add("Forward Velocity Tuner", ForwardVelocityTuner::new);
+                a.add("Lateral Velocity Tuner", LateralVelocityTuner::new);
+                a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new);
+                a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new);
+            });
+            s.folder("Manual", p -> {
+                p.add("Translational Tuner", TranslationalTuner::new);
+                p.add("Heading Tuner", HeadingTuner::new);
+                p.add("Drive Tuner", DriveTuner::new);
+                p.add("Centripetal Tuner", CentripetalTuner::new);
+            });
+            s.folder("Tests", p -> {
+                p.add("Line", Line::new);
+                p.add("Triangle", Triangle::new);
+                p.add("Circle", Circle::new);
+            });
+        });
+    }
+
+    @Override
+    public void onSelect() {
+        if (follower == null) {
+            follower = Constants.createFollower(hardwareMap);
+            PanelsConfigurables.INSTANCE.refreshClass(this);
+        } else {
+            follower = Constants.createFollower(hardwareMap);
+        }
+
+        follower.setStartingPose(new Pose());
+
+        poseHistory = follower.getPoseHistory();
+
+        telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+    }
+
+    @Override
+    public void onLog(List<String> lines) {}
+
+    public static void drawCurrent() {
+        try {
+            Drawing.drawRobot(follower.getPose());
+            Drawing.sendPacket();
+        } catch (Exception e) {
+            throw new RuntimeException("Drawing failed " + e);
+        }
+    }
+
+    public static void drawCurrentAndHistory() {
+        Drawing.drawPoseHistory(poseHistory);
+        drawCurrent();
+    }
+
+    /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */
+    public static void stopRobot() {
+        follower.startTeleopDrive(true);
+        follower.setTeleOpDrive(0,0,0,true);
+    }
+}
+
+/**
+ * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a
+ * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot.
+ * You should use this to check the robot's localization.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 5/6/2024
+ */
+class LocalizationTest extends OpMode {
+    @Override
+    public void init() {}
+
+    /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("This will print your robot's position to telemetry while "
+                + "allowing robot control through a basic mecanum drive on gamepad 1.");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    @Override
+    public void start() {
+        follower.startTeleopDrive();
+        follower.update();
+    }
+
+    /**
+     * This updates the robot's pose estimate, the simple mecanum drive, and updates the
+     * Panels telemetry with the robot's position as well as draws the robot's position.
+     */
+    @Override
+    public void loop() {
+        follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true);
+        follower.update();
+
+        telemetryM.debug("x:" + follower.getPose().getX());
+        telemetryM.debug("y:" + follower.getPose().getY());
+        telemetryM.debug("heading:" + follower.getPose().getHeading());
+        telemetryM.debug("total heading:" + follower.getTotalHeading());
+        telemetryM.update(telemetry);
+
+        drawCurrentAndHistory();
+    }
+}
+
+/**
+ * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current distance in ticks to the specified distance in inches. So, to use this, run the
+ * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're
+ * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials
+ * and average the results. Then, input the multiplier into the forward ticks to inches in your
+ * localizer of choice.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 5/6/2024
+ */
+class ForwardTuner extends OpMode {
+    public static double DISTANCE = 48;
+
+    @Override
+    public void init() {
+        follower.update();
+        drawCurrent();
+    }
+
+    /** This initializes the PoseUpdater as well as the Panels telemetry. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry.");
+        telemetryM.update(telemetry);
+        drawCurrent();
+    }
+
+    /**
+     * This updates the robot's pose estimate, and updates the Panels telemetry with the
+     * calculated multiplier and draws the robot.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+
+        telemetryM.debug("Distance Moved: " + follower.getPose().getX());
+        telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
+        telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier())));
+        telemetryM.update(telemetry);
+
+        drawCurrentAndHistory();
+    }
+}
+
+/**
+ * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current distance in ticks to the specified distance in inches. So, to use this, run the
+ * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're
+ * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials
+ * and average the results. Then, input the multiplier into the strafe ticks to inches in your
+ * localizer of choice.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 2.0, 6/26/2025
+ */
+class LateralTuner extends OpMode {
+    public static double DISTANCE = 48;
+
+    @Override
+    public void init() {
+        follower.update();
+        drawCurrent();
+    }
+
+    /** This initializes the PoseUpdater as well as the Panels telemetry. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry.");
+        telemetryM.update(telemetry);
+        drawCurrent();
+    }
+
+    /**
+     * This updates the robot's pose estimate, and updates the Panels telemetry with the
+     * calculated multiplier and draws the robot.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+
+        telemetryM.debug("Distance Moved: " + follower.getPose().getY());
+        telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
+        telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier())));
+        telemetryM.update(telemetry);
+
+        drawCurrentAndHistory();
+    }
+}
+
+/**
+ * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current angle in ticks to the specified angle in radians. So, to use this, run the
+ * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground.
+ * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run
+ * multiple trials and average the results. Then, input the multiplier into the turning ticks to
+ * radians in your localizer of choice.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 5/6/2024
+ */
+class TurnTuner extends OpMode {
+    public static double ANGLE = 2 * Math.PI;
+
+    @Override
+    public void init() {
+        follower.update();
+        drawCurrent();
+    }
+
+    /** This initializes the PoseUpdater as well as the Panels telemetry. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry.");
+        telemetryM.update(telemetry);
+
+        drawCurrent();
+    }
+
+    /**
+     * This updates the robot's pose estimate, and updates the Panels telemetry with the
+     * calculated multiplier and draws the robot.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+
+        telemetryM.debug("Total Angle: " + follower.getTotalHeading());
+        telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians.");
+        telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier())));
+        telemetryM.update(telemetry);
+
+        drawCurrentAndHistory();
+    }
+}
+
+/**
+ * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max
+ * power until it reaches some specified distance. It records the most recent velocities, and on
+ * reaching the end of the distance, it averages them and prints out the velocity obtained. It is
+ * recommended to run this multiple times on a full battery to get the best results. What this does
+ * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that
+ * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for
+ * more accurate following.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 3/13/2024
+ */
+class ForwardVelocityTuner extends OpMode {
+    private final ArrayList<Double> velocities = new ArrayList<>();
+    public static double DISTANCE = 48;
+    public static double RECORD_NUMBER = 10;
+
+    private boolean end;
+
+    @Override
+    public void init() {}
+
+    /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
+        telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power.");
+        telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
+        telemetryM.debug("Press B on game pad 1 to stop.");
+        telemetryM.debug("pose", follower.getPose());
+        telemetryM.update(telemetry);
+
+        follower.update();
+        drawCurrent();
+    }
+
+    /** This starts the OpMode by setting the drive motors to run forward at full power. */
+    @Override
+    public void start() {
+        for (int i = 0; i < RECORD_NUMBER; i++) {
+            velocities.add(0.0);
+        }
+        follower.startTeleopDrive(true);
+        follower.update();
+        end = false;
+    }
+
+    /**
+     * This runs the OpMode. At any point during the running of the OpMode, pressing B on
+     * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent
+     * velocities, and when the robot has run forward enough, these last velocities recorded are
+     * averaged and printed.
+     */
+    @Override
+    public void loop() {
+        if (gamepad1.bWasPressed()) {
+            stopRobot();
+            requestOpModeStop();
+        }
+
+        follower.update();
+        drawCurrentAndHistory();
+
+
+        if (!end) {
+            if (Math.abs(follower.getPose().getX()) > DISTANCE) {
+                end = true;
+                stopRobot();
+            } else {
+                follower.setTeleOpDrive(1,0,0,true);
+                //double currentVelocity = Math.abs(follower.getVelocity().getXComponent());
+                double currentVelocity = Math.abs(follower.poseTracker.getLocalizer().getVelocity().getX());
+                velocities.add(currentVelocity);
+                velocities.remove(0);
+            }
+        } else {
+            stopRobot();
+            double average = 0;
+            for (double velocity : velocities) {
+                average += velocity;
+            }
+            average /= velocities.size();
+            telemetryM.debug("Forward Velocity: " + average);
+            telemetryM.debug("\n");
+            telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on).");
+
+            for (int i = 0; i < velocities.size(); i++) {
+                telemetry.addData(String.valueOf(i), velocities.get(i));
+            }
+
+            telemetryM.update(telemetry);
+            telemetry.update();
+
+            if (gamepad1.aWasPressed()) {
+                follower.setXVelocity(average);
+                String message = "XMovement: " + average;
+                changes.add(message);
+            }
+        }
+    }
+}
+
+/**
+ * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot right at max
+ * power until it reaches some specified distance. It records the most recent velocities, and on
+ * reaching the end of the distance, it averages them and prints out the velocity obtained. It is
+ * recommended to run this multiple times on a full battery to get the best results. What this does
+ * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that
+ * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for
+ * more accurate following.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 3/13/2024
+ */
+class LateralVelocityTuner extends OpMode {
+    private final ArrayList<Double> velocities = new ArrayList<>();
+
+    public static double DISTANCE = 48;
+    public static double RECORD_NUMBER = 10;
+
+    private boolean end;
+
+    @Override
+    public void init() {}
+
+    /**
+     * This initializes the drive motors as well as the cache of velocities and the Panels
+     * telemetryM.
+     */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right.");
+        telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power.");
+        telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity.");
+        telemetryM.debug("Press B on Gamepad 1 to stop.");
+        telemetryM.update(telemetry);
+
+        follower.update();
+        drawCurrent();
+    }
+
+    /** This starts the OpMode by setting the drive motors to run right at full power. */
+    @Override
+    public void start() {
+        for (int i = 0; i < RECORD_NUMBER; i++) {
+            velocities.add(0.0);
+        }
+        follower.startTeleopDrive(true);
+        follower.update();
+    }
+
+    /**
+     * This runs the OpMode. At any point during the running of the OpMode, pressing B on
+     * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent
+     * velocities, and when the robot has run sideways enough, these last velocities recorded are
+     * averaged and printed.
+     */
+    @Override
+    public void loop() {
+        if (gamepad1.bWasPressed()) {
+            stopRobot();
+            requestOpModeStop();
+        }
+
+        follower.update();
+        drawCurrentAndHistory();
+
+        if (!end) {
+            if (Math.abs(follower.getPose().getY()) > DISTANCE) {
+                end = true;
+                stopRobot();
+            } else {
+                follower.setTeleOpDrive(0,1,0,true);
+                double currentVelocity = Math.abs(follower.getVelocity().dot(new Vector(1, Math.PI / 2)));
+                velocities.add(currentVelocity);
+                velocities.remove(0);
+            }
+        } else {
+            stopRobot();
+            double average = 0;
+            for (double velocity : velocities) {
+                average += velocity;
+            }
+            average /= velocities.size();
+
+            telemetryM.debug("Strafe Velocity: " + average);
+            telemetryM.debug("\n");
+            telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on).");
+            telemetryM.update(telemetry);
+
+            if (gamepad1.aWasPressed()) {
+                follower.setYVelocity(average);
+                String message = "YMovement: " + average;
+                changes.add(message);
+            }
+        }
+    }
+}
+
+/**
+ * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot
+ * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting
+ * them to zero power. The deceleration, or negative acceleration, is then measured until the robot
+ * stops. The accelerations across the entire time the robot is slowing down is then averaged and
+ * that number is then printed. This is used to determine how the robot will decelerate in the
+ * forward direction when power is cut, making the estimations used in the calculations for the
+ * drive Vector more accurate and giving better braking at the end of Paths.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/13/2024
+ */
+class ForwardZeroPowerAccelerationTuner extends OpMode {
+    private final ArrayList<Double> accelerations = new ArrayList<>();
+    public static double VELOCITY = 30;
+
+    private double previousVelocity;
+    private long previousTimeNano;
+
+    private boolean stopping;
+    private boolean end;
+
+    @Override
+    public void init() {}
+
+    /** This initializes the drive motors as well as the Panels telemetryM. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second.");
+        telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop.");
+        telemetryM.debug("Make sure you have enough room.");
+        telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed.");
+        telemetryM.debug("Press B on Gamepad 1 to stop.");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    /** This starts the OpMode by setting the drive motors to run forward at full power. */
+    @Override
+    public void start() {
+        follower.startTeleopDrive(false);
+        follower.update();
+        follower.setTeleOpDrive(1,0,0,true);
+    }
+
+    /**
+     * This runs the OpMode. At any point during the running of the OpMode, pressing B on
+     * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will
+     * record its deceleration / negative acceleration until it stops. Then, it will average all the
+     * recorded deceleration / negative acceleration and print that value.
+     */
+    @Override
+    public void loop() {
+        if (gamepad1.bWasPressed()) {
+            stopRobot();
+            requestOpModeStop();
+        }
+
+        follower.update();
+        drawCurrentAndHistory();
+
+        Vector heading = new Vector(1.0, follower.getPose().getHeading());
+        if (!end) {
+            if (!stopping) {
+                if (follower.getVelocity().dot(heading) > VELOCITY) {
+                    previousVelocity = follower.getVelocity().dot(heading);
+                    previousTimeNano = System.nanoTime();
+                    stopping = true;
+                    follower.setTeleOpDrive(0,0,0,true);
+                }
+            } else {
+                double currentVelocity = follower.getVelocity().dot(heading);
+                accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9)));
+                previousVelocity = currentVelocity;
+                previousTimeNano = System.nanoTime();
+                if (currentVelocity < follower.getConstraints().getVelocityConstraint()) {
+                    end = true;
+                }
+            }
+        } else {
+            double average = 0;
+            for (double acceleration : accelerations) {
+                average += acceleration;
+            }
+            average /= accelerations.size();
+
+            telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average);
+            telemetryM.debug("\n");
+            telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on).");
+            telemetryM.update(telemetry);
+
+            if (gamepad1.aWasPressed()) {
+                follower.getConstants().setForwardZeroPowerAcceleration(average);
+                String message = "Forward Zero Power Acceleration: " + average;
+                changes.add(message);
+            }
+        }
+    }
+}
+
+/**
+ * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot
+ * to the right until a specified velocity is achieved. Then, the robot cuts power to the motors, setting
+ * them to zero power. The deceleration, or negative acceleration, is then measured until the robot
+ * stops. The accelerations across the entire time the robot is slowing down is then averaged and
+ * that number is then printed. This is used to determine how the robot will decelerate in the
+ * forward direction when power is cut, making the estimations used in the calculations for the
+ * drive Vector more accurate and giving better braking at the end of Paths.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 3/13/2024
+ */
+class LateralZeroPowerAccelerationTuner extends OpMode {
+    private final ArrayList<Double> accelerations = new ArrayList<>();
+    public static double VELOCITY = 30;
+    private double previousVelocity;
+    private long previousTimeNano;
+    private boolean stopping;
+    private boolean end;
+
+    @Override
+    public void init() {}
+
+    /** This initializes the drive motors as well as the Panels telemetry. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("The robot will run to the right until it reaches " + VELOCITY + " inches per second.");
+        telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop.");
+        telemetryM.debug("Make sure you have enough room.");
+        telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed.");
+        telemetryM.debug("Press B on game pad 1 to stop.");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    /** This starts the OpMode by setting the drive motors to run forward at full power. */
+    @Override
+    public void start() {
+        follower.startTeleopDrive(false);
+        follower.update();
+        follower.setTeleOpDrive(0,1,0,true);
+    }
+
+    /**
+     * This runs the OpMode. At any point during the running of the OpMode, pressing B on
+     * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will
+     * record its deceleration / negative acceleration until it stops. Then, it will average all the
+     * recorded deceleration / negative acceleration and print that value.
+     */
+    @Override
+    public void loop() {
+        if (gamepad1.bWasPressed()) {
+            stopRobot();
+            requestOpModeStop();
+        }
+
+        follower.update();
+        drawCurrentAndHistory();
+
+        Vector heading = new Vector(1.0, follower.getPose().getHeading() - Math.PI / 2);
+        if (!end) {
+            if (!stopping) {
+                if (Math.abs(follower.getVelocity().dot(heading)) > VELOCITY) {
+                    previousVelocity = Math.abs(follower.getVelocity().dot(heading));
+                    previousTimeNano = System.nanoTime();
+                    stopping = true;
+                    follower.setTeleOpDrive(0,0,0,true);
+                }
+            } else {
+                double currentVelocity = Math.abs(follower.getVelocity().dot(heading));
+                accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9)));
+                previousVelocity = currentVelocity;
+                previousTimeNano = System.nanoTime();
+                if (currentVelocity < follower.getConstraints().getVelocityConstraint()) {
+                    end = true;
+                }
+            }
+        } else {
+            double average = 0;
+            for (double acceleration : accelerations) {
+                average += acceleration;
+            }
+            average /= accelerations.size();
+
+            telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average);
+            telemetryM.debug("\n");
+            telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on).");
+            telemetryM.update(telemetry);
+
+            if (gamepad1.aWasPressed()) {
+                follower.getConstants().setLateralZeroPowerAcceleration(average);
+                String message = "Lateral Zero Power Acceleration: " + average;
+                changes.add(message);
+            }
+        }
+    }
+}
+
+/**
+ * This is the Translational PIDF Tuner OpMode. It will keep the robot in place.
+ * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @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
+ */
+class TranslationalTuner extends OpMode {
+    public static double DISTANCE = 40;
+    private boolean forward = true;
+
+    private Path forwards;
+    private Path backwards;
+
+    @Override
+    public void init() {}
+
+    /** This initializes the Follower and creates the forward and backward Paths. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("This will activate the translational PIDF(s)");
+        telemetryM.debug("The robot will try to stay in place while you push it laterally.");
+        telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s).");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    @Override
+    public void start() {
+        follower.deactivateAllPIDFs();
+        follower.activateTranslational();
+        forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0)));
+        forwards.setConstantHeadingInterpolation(0);
+        backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0)));
+        backwards.setConstantHeadingInterpolation(0);
+        follower.followPath(forwards);
+    }
+
+    /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */
+    @Override
+    public void loop() {
+        follower.update();
+        drawCurrentAndHistory();
+
+        if (!follower.isBusy()) {
+            if (forward) {
+                forward = false;
+                follower.followPath(backwards);
+            } else {
+                forward = true;
+                follower.followPath(forwards);
+            }
+        }
+
+        telemetryM.debug("Push the robot laterally to test the Translational PIDF(s).");
+        telemetryM.update(telemetry);
+    }
+}
+
+/**
+ * This is the Heading PIDF Tuner OpMode. It will keep the robot in place.
+ * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly.
+ * It will try to keep the robot at a constant heading while the user tries to turn it.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @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
+ */
+class HeadingTuner extends OpMode {
+    public static double DISTANCE = 40;
+    private boolean forward = true;
+
+    private Path forwards;
+    private Path backwards;
+
+    @Override
+    public void init() {}
+
+    /**
+     * This initializes the Follower and creates the forward and backward Paths. Additionally, this
+     * initializes the Panels telemetry.
+     */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("This will activate the heading PIDF(s).");
+        telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it.");
+        telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s).");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    @Override
+    public void start() {
+        follower.deactivateAllPIDFs();
+        follower.activateHeading();
+        forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0)));
+        forwards.setConstantHeadingInterpolation(0);
+        backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0)));
+        backwards.setConstantHeadingInterpolation(0);
+        follower.followPath(forwards);
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the Panels.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        drawCurrentAndHistory();
+
+        if (!follower.isBusy()) {
+            if (forward) {
+                forward = false;
+                follower.followPath(backwards);
+            } else {
+                forward = true;
+                follower.followPath(forwards);
+            }
+        }
+
+        telemetryM.debug("Turn the robot manually to test the Heading PIDF(s).");
+        telemetryM.update(telemetry);
+    }
+}
+
+/**
+ * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @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
+ */
+class DriveTuner extends OpMode {
+    public static double DISTANCE = 40;
+    private boolean forward = true;
+
+    private PathChain forwards;
+    private PathChain backwards;
+
+    @Override
+    public void init() {}
+
+    /**
+     * This initializes the Follower and creates the forward and backward Paths. Additionally, this
+     * initializes the Panels telemetry.
+     */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward.");
+        telemetryM.debug("The robot will go forward and backward continuously along the path.");
+        telemetryM.debug("Make sure you have enough room.");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    @Override
+    public void start() {
+        follower.deactivateAllPIDFs();
+        follower.activateDrive();
+
+        forwards = follower.pathBuilder()
+                .setGlobalDeceleration()
+                .addPath(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0)))
+                .setConstantHeadingInterpolation(0)
+                .build();
+
+        backwards = follower.pathBuilder()
+                .setGlobalDeceleration()
+                .addPath(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0)))
+                .setConstantHeadingInterpolation(0)
+                .build();
+
+        follower.followPath(forwards);
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the Panels.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        drawCurrentAndHistory();
+
+        if (!follower.isBusy()) {
+            if (forward) {
+                forward = false;
+                follower.followPath(backwards);
+            } else {
+                forward = true;
+                follower.followPath(forwards);
+            }
+        }
+
+        telemetryM.debug("Driving forward?: " + forward);
+        telemetryM.update(telemetry);
+    }
+}
+
+/**
+ * This is the Line Test Tuner OpMode. It will drive the robot forward and back
+ * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @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
+ */
+class Line extends OpMode {
+    public static double DISTANCE = 40;
+    private boolean forward = true;
+
+    private Path forwards;
+    private Path backwards;
+
+    @Override
+    public void init() {}
+
+    /** This initializes the Follower and creates the forward and backward Paths. */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("This will activate all the PIDF(s)");
+        telemetryM.debug("The robot will go forward and backward continuously along the path while correcting.");
+        telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s).");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    @Override
+    public void start() {
+        follower.activateAllPIDFs();
+        forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0)));
+        forwards.setConstantHeadingInterpolation(0);
+        backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0)));
+        backwards.setConstantHeadingInterpolation(0);
+        follower.followPath(forwards);
+    }
+
+    /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */
+    @Override
+    public void loop() {
+        follower.update();
+        drawCurrentAndHistory();
+
+        if (!follower.isBusy()) {
+            if (forward) {
+                forward = false;
+                follower.followPath(backwards);
+            } else {
+                forward = true;
+                follower.followPath(forwards);
+            }
+        }
+
+        telemetryM.debug("Driving Forward?: " + forward);
+        telemetryM.update(telemetry);
+    }
+}
+
+/**
+ * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance
+ * forward and to the left. On reaching the end of the forward Path, the robot runs the backward
+ * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety
+ * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the
+ * centripetal Vector.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @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/13/2024
+ */
+class CentripetalTuner extends OpMode {
+    public static double DISTANCE = 20;
+    private boolean forward = true;
+
+    private Path forwards;
+    private Path backwards;
+
+    @Override
+    public void init() {}
+
+    /**
+     * This initializes the Follower and creates the forward and backward Paths.
+     * Additionally, this initializes the Panels telemetry.
+     */
+    @Override
+    public void init_loop() {
+        telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward.");
+        telemetryM.debug("The robot will go continuously along the path.");
+        telemetryM.debug("Make sure you have enough room.");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    @Override
+    public void start() {
+        follower.activateAllPIDFs();
+        forwards = new Path(new BezierCurve(new Pose(), new Pose(Math.abs(DISTANCE),0), new Pose(Math.abs(DISTANCE),DISTANCE)));
+        backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE),DISTANCE), new Pose(Math.abs(DISTANCE),0), new Pose(0,0)));
+
+        backwards.setTangentHeadingInterpolation();
+        backwards.reverseHeadingInterpolation();
+
+        follower.followPath(forwards);
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the Panels.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        drawCurrentAndHistory();
+        if (!follower.isBusy()) {
+            if (forward) {
+                forward = false;
+                follower.followPath(backwards);
+            } else {
+                forward = true;
+                follower.followPath(forwards);
+            }
+        }
+
+        telemetryM.debug("Driving away from the origin along the curve?: " + forward);
+        telemetryM.update(telemetry);
+    }
+}
+
+/**
+ * This is the Triangle autonomous OpMode.
+ * It runs the robot in a triangle, with the starting point being the bottom-middle point.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge
+ * @version 1.0, 12/30/2024
+ */
+class Triangle extends OpMode {
+
+    private final Pose startPose = new Pose(0, 0, Math.toRadians(0));
+    private final Pose interPose = new Pose(24, -24, Math.toRadians(90));
+    private final Pose endPose = new Pose(24, 24, Math.toRadians(45));
+
+    private PathChain triangle;
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the Panels.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        drawCurrentAndHistory();
+
+        if (follower.atParametricEnd()) {
+            follower.followPath(triangle, true);
+        }
+    }
+
+    @Override
+    public void init() {}
+
+    @Override
+    public void init_loop() {
+        telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point.");
+        telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode.");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    /** Creates the PathChain for the "triangle".*/
+    @Override
+    public void start() {
+        follower.setStartingPose(startPose);
+
+        triangle = follower.pathBuilder()
+                .addPath(new BezierLine(startPose, interPose))
+                .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading())
+                .addPath(new BezierLine(interPose, endPose))
+                .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading())
+                .addPath(new BezierLine(endPose, startPose))
+                .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading())
+                .build();
+
+        follower.followPath(triangle);
+    }
+}
+
+/**
+ * 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
+ */
+class Circle extends OpMode {
+    public static double RADIUS = 10;
+    private PathChain circle;
+
+    public void start() {
+        circle = follower.pathBuilder()
+                .addPath(new BezierCurve(new Pose(0, 0), new Pose(RADIUS, 0), new Pose(RADIUS, RADIUS)))
+                .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS))
+                .addPath(new BezierCurve(new Pose(RADIUS, RADIUS), new Pose(RADIUS, 2 * RADIUS), new Pose(0, 2 * RADIUS)))
+                .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS))
+                .addPath(new BezierCurve(new Pose(0, 2 * RADIUS), new Pose(-RADIUS, 2 * RADIUS), new Pose(-RADIUS, RADIUS)))
+                .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS))
+                .addPath(new BezierCurve(new Pose(-RADIUS, RADIUS), new Pose(-RADIUS, 0), new Pose(0, 0)))
+                .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS))
+                .build();
+        follower.followPath(circle);
+    }
+
+    @Override
+    public void init_loop() {
+        telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. ");
+        telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode.");
+        telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction.");
+        telemetryM.update(telemetry);
+        follower.update();
+        drawCurrent();
+    }
+
+    @Override
+    public void init() {}
+
+    /**
+     * 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();
+        drawCurrentAndHistory();
+
+        if (follower.atParametricEnd()) {
+            follower.followPath(circle);
+        }
+    }
+}
+
+/**
+ * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot.
+ *
+ * @author Lazar - 19234
+ * @version 1.1, 5/19/2025
+ */
+class Drawing {
+    public static final double ROBOT_RADIUS = 9; // woah
+    private static final FieldManager panelsField = PanelsField.INSTANCE.getField();
+
+    private static final Style robotLook = new Style(
+            "", "#3F51B5", 0.0
+    );
+    private static final Style historyLook = new Style(
+            "", "#4CAF50", 0.0
+    );
+
+    /**
+     * This prepares Panels Field for using Pedro Offsets
+     */
+    public static void init() {
+        panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING());
+    }
+
+    /**
+     * This draws everything that will be used in the Follower's telemetryDebug() method. This takes
+     * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed.
+     *
+     * @param follower Pedro Follower instance.
+     */
+    public static void drawDebug(Follower follower) {
+        if (follower.getCurrentPath() != null) {
+            drawPath(follower.getCurrentPath(), robotLook);
+            Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue());
+            drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook);
+        }
+        drawPoseHistory(follower.getPoseHistory(), historyLook);
+        drawRobot(follower.getPose(), historyLook);
+
+        sendPacket();
+    }
+
+    /**
+     * This draws a robot at a specified Pose with a specified
+     * look. The heading is represented as a line.
+     *
+     * @param pose  the Pose to draw the robot at
+     * @param style the parameters used to draw the robot with
+     */
+    public static void drawRobot(Pose pose, Style style) {
+        if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) {
+            return;
+        }
+
+        panelsField.setStyle(style);
+        panelsField.moveCursor(pose.getX(), pose.getY());
+        panelsField.circle(ROBOT_RADIUS);
+
+        Vector v = pose.getHeadingAsUnitVector();
+        v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);
+        double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2;
+        double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent();
+
+        panelsField.setStyle(style);
+        panelsField.moveCursor(x1, y1);
+        panelsField.line(x2, y2);
+    }
+
+    /**
+     * This draws a robot at a specified Pose. The heading is represented as a line.
+     *
+     * @param pose the Pose to draw the robot at
+     */
+    public static void drawRobot(Pose pose) {
+        drawRobot(pose, robotLook);
+    }
+
+    /**
+     * This draws a Path with a specified look.
+     *
+     * @param path  the Path to draw
+     * @param style the parameters used to draw the Path with
+     */
+    public static void drawPath(Path path, Style style) {
+        double[][] points = path.getPanelsDrawingPoints();
+
+        for (int i = 0; i < points[0].length; i++) {
+            for (int j = 0; j < points.length; j++) {
+                if (Double.isNaN(points[j][i])) {
+                    points[j][i] = 0;
+                }
+            }
+        }
+
+        panelsField.setStyle(style);
+        panelsField.moveCursor(points[0][0], points[0][1]);
+        panelsField.line(points[1][0], points[1][1]);
+    }
+
+    /**
+     * This draws all the Paths in a PathChain with a
+     * specified look.
+     *
+     * @param pathChain the PathChain to draw
+     * @param style     the parameters used to draw the PathChain with
+     */
+    public static void drawPath(PathChain pathChain, Style style) {
+        for (int i = 0; i < pathChain.size(); i++) {
+            drawPath(pathChain.getPath(i), style);
+        }
+    }
+
+    /**
+     * This draws the pose history of the robot.
+     *
+     * @param poseTracker the PoseHistory to get the pose history from
+     * @param style       the parameters used to draw the pose history with
+     */
+    public static void drawPoseHistory(PoseHistory poseTracker, Style style) {
+        panelsField.setStyle(style);
+
+        int size = poseTracker.getXPositionsArray().length;
+        for (int i = 0; i < size - 1; i++) {
+
+            panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]);
+            panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]);
+        }
+    }
+
+    /**
+     * This draws the pose history of the robot.
+     *
+     * @param poseTracker the PoseHistory to get the pose history from
+     */
+    public static void drawPoseHistory(PoseHistory poseTracker) {
+        drawPoseHistory(poseTracker, historyLook);
+    }
+
+    /**
+     * This tries to send the current packet to FTControl Panels.
+     */
+    public static void sendPacket() {
+        panelsField.update();
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java
new file mode 100644
index 0000000..ed0d7b3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java
@@ -0,0 +1,238 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+public class AprilTag implements Subsystem {
+
+    private AprilTagProcessor aprilTag;
+    private VisionPortal visionPortal;
+
+    private MultipleTelemetry TELE;
+
+    private boolean teleOn = false;
+
+    private int detections = 0;
+
+    List<AprilTagDetection> currentDetections;
+
+    ArrayList<ArrayList<Double>> Data = new ArrayList<>();
+
+
+
+    public AprilTag(Robot robot, MultipleTelemetry tele) {
+
+
+
+        this.aprilTag = robot.aprilTagProcessor;
+
+
+        this.TELE = tele;
+
+
+
+
+    }
+
+    @Override
+    public void update() {
+
+        currentDetections = aprilTag.getDetections();
+
+        UpdateData();
+
+        if(teleOn){
+            tagTELE();
+            initTelemetry();
+        }
+
+
+
+
+    }
+
+    public void initTelemetry (){
+
+        TELE.addData("Camera Preview", "Check Driver Station for stream");
+        TELE.addData("Status", "Initialized - Press START");
+
+
+
+    }
+
+    public void tagTELE () {
+
+        TELE.addData("# AprilTags Detected", detections);
+
+        // Display info for each detected tag
+        for (ArrayList<Double> detection : Data) {
+            if (detection.get(0) ==1) {
+                // Known AprilTag with metadata
+                TELE.addLine(String.format("\n==== (ID %d) %s ====",
+                        detection.get(1).intValue(), ""));
+
+                TELE.addLine(String.format("XYZ: %6.1f %6.1f %6.1f  (inch)",
+                        detection.get(2),
+                        detection.get(3),
+                        detection.get(4)));
+
+                TELE.addData("Distance", getDistance(detection.get(1).intValue()));
+
+                TELE.addLine(String.format("PRY: %6.1f %6.1f %6.1f  (deg)",
+                        detection.get(5),
+                        detection.get(6),
+                        detection.get(7)));
+
+                TELE.addLine(String.format("RBE: %6.1f %6.1f %6.1f  (inch, deg, deg)",
+                        detection.get(8),
+                        detection.get(9),
+                        detection.get(10)));
+            }
+        }
+    }
+
+
+    public void turnTelemetryOn(boolean bool) {
+
+        teleOn = bool;
+
+    }
+
+
+
+    public void UpdateData () {
+
+        Data.clear();  // <--- THIS FIXES YOUR ISSUE
+
+
+        detections = currentDetections.size();
+
+
+        for (AprilTagDetection detection : currentDetections) {
+
+            ArrayList<Double> detectionData = new ArrayList<Double>();
+
+
+
+            if (detection.metadata != null) {
+
+                detectionData.add(1.0);
+                // Known AprilTag with metadata
+
+                detectionData.add( (double) detection.id);
+
+
+
+                detectionData.add(detection.ftcPose.x);
+                detectionData.add(detection.ftcPose.y);
+                detectionData.add(detection.ftcPose.z);
+
+
+
+                detectionData.add(detection.ftcPose.pitch);
+                detectionData.add(detection.ftcPose.roll);
+                detectionData.add(detection.ftcPose.yaw);
+
+                detectionData.add(detection.ftcPose.range);
+                detectionData.add(detection.ftcPose.bearing);
+                detectionData.add(detection.ftcPose.elevation);
+
+            } else {
+
+                detectionData.add(0, 0.0);
+
+            }
+
+            Data.add(detectionData);
+        }
+
+    }
+
+    public int getDetectionCount() {
+
+        return detections;
+
+    }
+
+    public boolean isDetected (int id){
+        return (!filterID(Data, (double) id ).isEmpty());
+    }
+
+
+
+    public double getDistance(int id) {
+        ArrayList<Double> d = filterID(Data, (double) id);
+        if (d.size() >= 5) {
+            double x = d.get(2);
+            double y = d.get(3);
+            double z = d.get(4);
+            return Math.sqrt(x*x + y*y + z*z);
+        }
+        return -1; // tag not found
+    }
+
+    // Returns the position as [x, y, z]
+    public List<Double> getPosition(int id) {
+        ArrayList<Double> d = filterID(Data, (double) id);
+        if (d.size() >= 5) {
+            List<Double> pos = new ArrayList<>();
+            pos.add(d.get(2));
+            pos.add(d.get(3));
+            pos.add(d.get(4));
+            return pos;
+        }
+        return Collections.emptyList();
+    }
+
+    // Returns orientation as [pitch, roll, yaw]
+    public List<Double> getOrientation(int id) {
+        ArrayList<Double> d = filterID(Data, (double) id);
+        if (d.size() >= 8) {
+            List<Double> ori = new ArrayList<>();
+            ori.add(d.get(5));
+            ori.add(d.get(6));
+            ori.add(d.get(7));
+            return ori;
+        }
+        return Collections.emptyList();
+    }
+
+    // Returns range, bearing, elevation as [range, bearing, elevation]
+    public List<Double> getRBE(int id) {
+        ArrayList<Double> d = filterID(Data, (double) id);
+        if (d.size() >= 11) {
+            List<Double> rbe = new ArrayList<>();
+            rbe.add(d.get(8));
+            rbe.add(d.get(9));
+            rbe.add(d.get(10));
+            return rbe;
+        }
+        return Collections.emptyList();
+    }
+
+    // Returns full raw data for debugging or custom processing
+    public ArrayList<Double> getRawData(int id) {
+        return filterID(Data, (double) id);
+    }
+
+    public static ArrayList<Double> filterID(ArrayList<ArrayList<Double>> data, double x) {
+        for (ArrayList<Double> innerList : data) {
+            // Ensure it has a second element
+            if (innerList.size() > 1 && Math.abs(innerList.get(1) - x) < 1e-9) {
+                return innerList; // Return the first match
+            }
+        }
+        // Return an empty ArrayList if no match found
+        return new ArrayList<>();
+    }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
new file mode 100644
index 0000000..e192049
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
@@ -0,0 +1,99 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
+
+import java.util.Objects;
+
+public class Drivetrain implements Subsystem {
+
+    private GamepadEx gamepad;
+
+    public MultipleTelemetry TELE;
+
+    private String Mode = "Default";
+
+    private DcMotorEx fl;
+
+    private DcMotorEx fr;
+    private DcMotorEx bl;
+    private DcMotorEx br;
+
+    private double defaultSpeed = 0.7;
+
+    private double slowSpeed = 0.3;
+    public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){
+
+        this.fl = robot.frontLeft;
+        this.fr = robot.frontRight;
+        this.br = robot.backRight;
+        this.bl = robot.backLeft;
+
+        this.gamepad = gamepad1;
+
+        this.TELE = tele;
+
+
+
+    }
+
+    public void setMode (String mode){
+        this.Mode = mode;
+    }
+
+    public void setDefaultSpeed (double speed){
+        this.defaultSpeed = speed;
+    }
+
+    public void setSlowSpeed (double speed){
+        this.slowSpeed = speed;
+    }
+
+    public void RobotCentric(double fwd, double strafe, double turn, double turbo){
+
+        double y = -fwd; // Remember, Y stick value is reversed
+        double x = strafe * 1.1; // Counteract imperfect strafing
+        double rx = turn;
+
+        // Denominator is the largest motor power (absolute value) or 1
+        // This ensures all the powers maintain the same ratio,
+        // but only if at least one is out of the range [-1, 1]
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+        double frontLeftPower = (y + x + rx) / denominator;
+        double backLeftPower = (y - x + rx) / denominator;
+        double frontRightPower = (y - x - rx) / denominator;
+        double backRightPower = (y + x - rx) / denominator;
+
+        fl.setPower(frontLeftPower*turbo);
+        bl.setPower(backLeftPower*turbo);
+        fr.setPower(frontRightPower*turbo);
+        br.setPower(backRightPower*turbo);
+
+    }
+
+
+    @Override
+    public void update() {
+
+        if (Objects.equals(Mode, "Default")) {
+
+            RobotCentric(
+                    gamepad.getRightY(),
+                    gamepad.getRightX(),
+                    gamepad.getLeftX(),
+                    (gamepad.getTrigger(
+                            GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed)
+                            - gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
+                            + defaultSpeed
+                    )
+            );
+        }
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
new file mode 100644
index 0000000..c55fb2a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
@@ -0,0 +1,77 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+public class Intake implements Subsystem {
+
+    private GamepadEx gamepad;
+
+    public MultipleTelemetry TELE;
+
+
+    private DcMotorEx intake;
+
+    private double intakePower = 1.0;
+
+    private int intakeState = 0;
+
+
+    public Intake (Robot robot){
+
+
+        this.intake = robot.intake;
+
+
+    }
+
+    public int getIntakeState() {
+        return intakeState;
+    }
+
+    public void toggle(){
+        if (intakeState !=0){
+            intakeState = 0;
+        } else {
+            intakeState = 1;
+        }
+    }
+
+    public void setIntakePower(double pow){
+        intakePower = pow;
+    }
+
+    public void intake(){
+        intakeState =1;
+    }
+
+    public void reverse(){
+        intakeState =-1;
+    }
+
+
+    public void stop(){
+        intakeState =-1;
+    }
+
+
+
+
+    @Override
+    public void update() {
+
+        if (intakeState == 1){
+            intake.setPower(intakePower);
+        } else if (intakeState == -1){
+            intake.setPower(-intakePower);
+        } else {
+            intake.setPower(0);
+        }
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
new file mode 100644
index 0000000..7bb38ee
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
@@ -0,0 +1,337 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.arcrobotics.ftclib.controller.PIDController;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.PIDCoefficients;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+import org.firstinspires.ftc.teamcode.constants.Poses;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.subsystems.Subsystem;
+
+import java.util.Objects;
+
+public class Shooter implements Subsystem {
+    private final DcMotorEx fly1;
+    private final DcMotorEx fly2;
+
+    private final DcMotorEx encoder;
+    private final Servo hoodServo;
+
+    private final Servo turret1;
+
+    private final Servo turret2;
+
+
+    private final MultipleTelemetry telemetry;
+
+    private boolean telemetryOn = false;
+
+    private double manualPower = 0.0;
+    private double hoodPos = 0.0;
+
+    private double turretPos = 0.0;
+    private double velocity = 0.0;
+    private double posPower = 0.0;
+
+    private int targetPosition = 0;
+
+    private double p = 0.0003, i = 0, d = 0.00001;
+
+    private PIDController controller;
+
+
+
+
+
+    private String shooterMode = "AUTO";
+
+    private String turretMode = "AUTO";
+
+
+    public Shooter(Robot robot, MultipleTelemetry TELE) {
+        this.fly1 = robot.shooter1;
+        this.fly2 = robot.shooter2;
+        this.telemetry = TELE;
+        this.hoodServo = robot.hood;
+
+        // Reset encoders
+        fly1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        fly2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+
+        controller = new PIDController(p, i, d);
+
+
+        controller.setPID(p, i, d);
+
+        this.turret1 = robot.turr1;
+
+        this.turret2 = robot.turr2;
+
+        this.encoder = robot.shooterEncoder;
+
+
+
+
+
+
+    }
+
+    public void telemetryUpdate() {
+
+        // Telemetry
+
+        telemetry.addData("Mode", shooterMode);
+        telemetry.addData("ManualPower", manualPower);
+        telemetry.addData("Position", getPosition());
+        telemetry.addData("TargetPosition", targetPosition);
+        telemetry.addData("Velocity", getVelocity());
+        telemetry.addData("TargetVelocity", velocity);
+        telemetry.addData("hoodPos", gethoodPosition());
+        telemetry.addData("turretPos", getTurretPosition());
+        telemetry.addData("PID Coefficients", "P: %.6f, I: %.6f, D: %.6f", p, i, d);
+        telemetry.addData("Current Fly 1", fly1.getCurrent(CurrentUnit.AMPS));
+        telemetry.addData("Current Fly 2", fly2.getCurrent(CurrentUnit.AMPS));
+
+    }
+
+    public double gethoodPosition() {
+        return (hoodServo.getPosition());
+    }
+
+    public void sethoodPosition(double pos) {hoodPos = pos;}
+
+    public double getTurretPosition() {
+        return ((turret1.getPosition()+ (1-turret2.getPosition()))/2);
+    }
+
+    public void setTurretPosition(double pos) {turretPos = pos;}
+
+    public double getVelocity() {
+        return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2));
+    }
+
+    public void setVelocity(double vel){velocity = vel;}
+
+
+
+    public void setPosPower(double power){posPower = power;}
+
+
+    public void setTargetPosition(int pos){
+        targetPosition = pos;
+    }
+
+    public void setTolerance(int tolerance){
+        controller.setTolerance(tolerance);
+    }
+
+    public void setControllerCoefficients(double kp, double ki, double kd){
+        p = kp;
+        i = ki;
+        d = kd;
+        controller.setPID(p, i, d);
+
+    }
+
+    public PIDCoefficients  getControllerCoefficients(){
+
+        return new PIDCoefficients(p, i, d);
+
+    }
+
+
+
+
+
+
+
+    public void setManualPower(double power){manualPower = power;}
+
+    public String getShooterMode(){return shooterMode;}
+
+    public String getTurretMode(){return turretMode;}
+
+
+    public double getPosition(){
+        return ((double) ((fly1.getCurrentPosition() + fly2.getCurrentPosition()) /2));
+    }
+
+    public void setShooterMode(String mode){ shooterMode = mode;}
+
+    public void setTurretMode(String mode){ turretMode = mode;}
+
+
+    public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){
+
+        fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+
+
+        Pose2d deltaPose = new Pose2d(
+                goalPose.position.x - robotPose.position.x,
+                goalPose.position.y - robotPose.position.y,
+                goalPose.heading.toDouble() - (robotPose.heading.toDouble())
+        );
+
+        double distance = Math.sqrt(
+                deltaPose.position.x * deltaPose.position.x
+                + deltaPose.position.y * deltaPose.position.y
+                + Poses.relativeGoalHeight * Poses.relativeGoalHeight
+        );
+
+        telemetry.addData("dst", distance);
+
+        double shooterPow = getPowerByDist(distance);
+
+        double hoodAngle = getAngleByDist(distance);
+
+
+//        hoodServo.setPosition(hoodAngle);
+
+        moveTurret(getTurretPosByDeltaPose(deltaPose, offset));
+
+        return distance;
+
+
+
+
+
+        //0.9974 * 355
+
+
+    }
+
+    public double getTurretPosByDeltaPose (Pose2d dPose, double offset){
+
+        double deltaAngle = Math.toDegrees(dPose.heading.toDouble());
+
+
+        double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x));
+
+        telemetry.addData("deltaAngle", deltaAngle);
+
+
+
+        if (deltaAngle > 90) {
+            deltaAngle -=360;
+        }
+
+
+
+//        deltaAngle += aTanAngle;
+
+        deltaAngle /= (335);
+
+        telemetry.addData("dAngle", deltaAngle);
+
+        telemetry.addData("AtanAngle", aTanAngle);
+
+
+        return ((0.30-deltaAngle) + offset);
+
+
+
+    }
+
+    //62, 0.44
+
+    //56.5, 0.5
+
+
+    public double getPowerByDist(double dist){
+
+        //TODO: ADD LOGIC
+        return  dist;
+    }
+
+    public double getAngleByDist(double dist){
+
+
+        double newDist = dist - 56.5;
+
+        double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46;
+
+
+
+
+        return  pos;
+    }
+
+
+
+
+    public void setTelemetryOn(boolean state){telemetryOn = state;}
+
+    public void moveTurret(double pos){
+        turret1.setPosition(pos);
+        turret2.setPosition(1-pos);
+    }
+
+    @Override
+    public void update() {
+
+        if (Objects.equals(shooterMode, "MANUAL")){
+            fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+
+            fly1.setPower(manualPower);
+            fly2.setPower(manualPower);
+        }
+
+        else if (Objects.equals(shooterMode, "VEL")){
+
+            fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+            fly1.setVelocity(velocity);
+
+            fly2.setPower(fly1.getPower());
+
+
+
+
+
+
+
+        }
+
+        else if (Objects.equals(shooterMode, "POS")){
+
+
+            double powPID = controller.calculate(getPosition(), targetPosition);
+
+
+
+            fly1.setPower(powPID);
+            fly2.setPower(powPID);
+        }
+
+        if (Objects.equals(turretMode, "MANUAL")){
+//            hoodServo.setPosition(hoodPos);
+
+            moveTurret(turretPos);
+
+
+        }
+
+
+
+        if (telemetryOn) {telemetryUpdate();}
+
+
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java
new file mode 100644
index 0000000..e91820a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java
@@ -0,0 +1,257 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+import java.util.ArrayList;
+
+public class Spindexer implements Subsystem{
+
+    private Servo s1;
+    private Servo s2;
+
+    private DigitalChannel p0;
+
+    private DigitalChannel p1;
+    private DigitalChannel p2;
+    private DigitalChannel p3;
+    private DigitalChannel p4;
+
+    private DigitalChannel p5;
+
+    private AnalogInput input;
+
+    private AnalogInput input2;
+
+
+    private MultipleTelemetry TELE;
+
+    private double position = 0.501;
+
+    private boolean telemetryOn = false;
+
+    private boolean ball0 = false;
+
+    private boolean ball1 = false;
+
+    private boolean ball2 = false;
+
+    private boolean green0 = false;
+
+    private boolean green1 = false;
+
+    private boolean green2 = false;
+
+
+
+
+    public Spindexer (Robot robot, MultipleTelemetry tele){
+
+        this.s1 = robot.spin1;
+        this.s2 = robot.spin2;
+
+        this.p0 = robot.pin0;
+        this.p1 = robot.pin1;
+        this.p2 = robot.pin2;
+        this.p3 = robot.pin3;
+        this.p4 = robot.pin4;
+        this.p5 = robot.pin5;
+
+        this.input = robot.analogInput;
+
+        this.input2 = robot.analogInput2;
+
+        this.TELE = tele;
+
+    }
+
+    public void setTelemetryOn(boolean state){
+        telemetryOn = state;
+    }
+
+    public void colorSensorTelemetry() {
+
+
+        TELE.addData("ball0", ball0);
+        TELE.addData("ball1", ball1);
+        TELE.addData("ball2", ball2);
+        TELE.addData("green0", green0);
+        TELE.addData("green1", green1);
+        TELE.addData("green2", green2);
+
+
+
+    }
+
+    public void checkForBalls() {
+
+        if (p0.getState()){
+            ball0 = true;
+            green0 = p1.getState();
+        } else {
+            ball0 = false;
+        }
+
+        if (p2.getState()){
+            ball1 = true;
+            green1 = p3.getState();
+        } else {
+            ball1 = false;
+        }
+
+        if (p4.getState()){
+            ball2 = true;
+            green2 = p5.getState();
+        } else {
+            ball2 = false;
+        }
+    }
+
+    public void setPosition (double pos) {
+        position = pos;
+    }
+
+    public void intake () {
+        position = spindexer_intakePos;
+    }
+
+    public void intakeShake(double runtime) {
+        if ((runtime % 0.25) >0.125) {
+            position = spindexer_intakePos + 0.04;
+        } else {
+            position = spindexer_intakePos - 0.04;
+
+        }
+    }
+
+    public void outtake3Shake(double runtime) {
+        if ((runtime % 0.25) >0.125) {
+            position = spindexer_outtakeBall3 + 0.04;
+        } else {
+            position = spindexer_outtakeBall3 - 0.04;
+
+        }
+    }
+
+
+    public void outtake3 () {
+        position = spindexer_outtakeBall3;
+    }
+
+    public void outtake2 () {
+        position = spindexer_outtakeBall2;
+    }
+
+    public void outtake1 () {
+        position = spindexer_outtakeBall1;
+    }
+
+
+    public int outtakeGreen(int secLast, int Last) {
+        if (green2 && (secLast!=3) && (Last!=3)) {
+            outtake3();
+            return 3;
+        } else if (green1 && (secLast!=2) && (Last!=2)){
+            outtake2();
+            return 2;
+        } else if (green0 && (secLast!=1) && (Last!=1)) {
+            outtake1();
+            return 1;
+        } else {
+
+            if (secLast!=1 && Last!= 1){
+                outtake1();
+                return 1;
+            } else if (secLast!=2 && Last!=2){
+                outtake2();
+                return 2;
+            } else {
+                outtake3();
+                return 3;
+            }
+
+        }
+    }
+
+
+
+    public void outtakeGreenFs() {
+        if (green0 && ball0) {
+            outtake1();
+        } else if (green1 && ball1){
+            outtake2();
+        } else if (green2 && ball2) {
+            outtake3();
+        }
+    }
+
+    public int greens() {
+        int num = 0;
+
+        if (green0){num++;}
+
+        if (green1){num++;}
+
+
+        if (green2){num++;}
+
+        return num;
+
+
+    }
+
+
+    public int outtakePurple(int secLast, int Last) {
+        if (!green2 && (secLast!=3) && (Last!=3)) {
+            outtake3();
+            return 3;
+        } else if (!green1 && (secLast!=2) && (Last!=2)){
+            outtake2();
+            return 2;
+        } else if (!green0 && (secLast!=1) && (Last!=1)) {
+            outtake1();
+            return 1;
+        } else {
+
+            if (secLast!=1 && Last!= 1){
+                outtake1();
+                return 1;
+            } else if (secLast!=2 && Last!=2){
+                outtake2();
+                return 2;
+            } else {
+                outtake3();
+                return 3;
+            }
+
+
+        }
+    }
+
+
+
+
+    @Override
+    public void update() {
+
+        if (position !=0.501) {
+
+            s1.setPosition(position);
+            s2.setPosition(1 - position);
+        }
+
+
+        if (telemetryOn) {
+            colorSensorTelemetry();
+        }
+
+
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java
new file mode 100644
index 0000000..34bf534
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java
@@ -0,0 +1,6 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+public interface Subsystem {
+
+    public void update();
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
new file mode 100644
index 0000000..0b6f5f9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+public class Transfer implements Subsystem{
+
+    private final Servo servo;
+
+    private final DcMotorEx transfer;
+
+    private double motorPow = 0.0;
+
+    private double servoPos = 0.501;
+
+    public Transfer (Robot robot){
+
+        this.servo = robot.transferServo;
+
+        this.transfer = robot.transfer;
+
+    }
+
+    public void setTransferPosition(double pos){
+        this.servoPos  = pos;
+    }
+
+    public void setTransferPower (double pow){
+        this.motorPow = pow;
+    }
+
+    public void transferOut(){
+        this.setTransferPosition(transferServo_out);
+    }
+
+    public void transferIn(){
+        this.setTransferPosition(transferServo_in);
+    }
+
+
+
+
+
+    @Override
+    public void update() {
+
+        if (servoPos!=0.501){
+            servo.setPosition(servoPos);
+        }
+
+        transfer.setPower(motorPow);
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/HoldTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/HoldTest.java
new file mode 100644
index 0000000..0bd4026
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/HoldTest.java
@@ -0,0 +1,80 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.h1;
+import static org.firstinspires.ftc.teamcode.constants.Poses.x1;
+import static org.firstinspires.ftc.teamcode.constants.Poses.y1;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.ProfileAccelConstraint;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.TranslationalVelConstraint;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.acmerobotics.roadrunner.ftc.PinpointIMU;
+import com.pedropathing.ftc.localization.localizers.PinpointLocalizer;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+
+@TeleOp
+@Config
+public class HoldTest extends LinearOpMode {
+
+    Robot robot;
+    MecanumDrive drive;
+
+    public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
+    public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        MultipleTelemetry TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        robot = new Robot(hardwareMap);
+
+        drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
+
+
+        TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                .strafeToLinearHeading(new Vector2d(0.01, 0.01), 0, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+
+        waitForStart();
+
+
+        while (opModeIsActive()){
+
+            drive.updatePoseEstimate();
+
+            Pose2d currentPos = drive.localizer.getPose();
+
+
+
+            TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
+                    .strafeToLinearHeading(new Vector2d(0, 0), 0, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+
+            if (Math.abs(currentPos.position.x) >0.5 || Math.abs(currentPos.position.y)>0.5) {
+                Actions.runBlocking(
+                        traj2.build()
+                );
+            }
+
+
+
+
+
+        }
+
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
new file mode 100644
index 0000000..27f3af5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
@@ -0,0 +1,901 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.arcrobotics.ftclib.gamepad.ToggleButtonReader;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Shooter;
+import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
+import org.firstinspires.ftc.teamcode.subsystems.Transfer;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+
+@Config
+@TeleOp
+
+public class TeleopV1 extends LinearOpMode {
+
+
+    Robot robot;
+
+    Drivetrain drivetrain;
+
+    Intake intake;
+
+    Spindexer spindexer;
+
+    Transfer transfer;
+
+    MultipleTelemetry TELE;
+
+    GamepadEx g1;
+
+    GamepadEx g2;
+
+    public static double defaultSpeed = 1;
+
+    public static double slowMoSpeed = 0.4;
+
+    public static double power = 0.0;
+
+    public static double pos = hoodDefault;
+
+    public boolean all = false;
+
+    public int ticker = 0;
+
+    ToggleButtonReader g1RightBumper;
+
+    ToggleButtonReader g2Circle;
+
+    ToggleButtonReader g2Square;
+
+
+    ToggleButtonReader g2Triangle;
+
+    ToggleButtonReader g2RightBumper;
+
+    ToggleButtonReader g1LeftBumper;
+
+    ToggleButtonReader g2LeftBumper;
+
+    ToggleButtonReader g2DpadUp;
+
+    ToggleButtonReader g2DpadDown;
+
+    ToggleButtonReader g2DpadRight;
+
+    ToggleButtonReader g2DpadLeft;
+
+    public boolean leftBumper = false;
+    public double g1RightBumperStamp = 0.0;
+
+    public double g1LeftBumperStamp = 0.0;
+
+
+    public double g2LeftBumperStamp = 0.0;
+
+    public static int spindexerPos = 0;
+
+    public boolean green = false;
+
+    Shooter shooter;
+
+    public boolean scoreAll = false;
+
+    MecanumDrive drive ;
+
+    public boolean autotrack = false;
+
+    public int last = 0;
+    public int second = 0;
+
+    public double offset = 0.0;
+
+    public static double rIn = 0.59;
+
+    public static double rOut = 0;
+
+    public boolean notShooting = true;
+
+    public boolean circle = false;
+
+    public boolean square = false;
+
+    public boolean tri = false;
+
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        drive  = new MecanumDrive(hardwareMap, teleStart);
+
+
+
+
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                FtcDashboard.getInstance().getTelemetry(),
+                telemetry
+        );
+
+        g1 = new GamepadEx(gamepad1);
+
+        g1RightBumper  = new ToggleButtonReader(
+            g1, GamepadKeys.Button.RIGHT_BUMPER
+        );
+
+        g2 = new GamepadEx(gamepad2);
+
+        g1LeftBumper = new ToggleButtonReader(
+                g1, GamepadKeys.Button.LEFT_BUMPER
+        );
+
+        g2Circle  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.B
+        );
+
+        g2Triangle  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.Y
+        );
+
+        g2Square  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.X
+        );
+
+        g2RightBumper  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.RIGHT_BUMPER
+        );
+
+
+        g2LeftBumper  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.LEFT_BUMPER
+        );
+
+        g2DpadUp  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_UP
+        );
+
+
+        g2DpadDown  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_DOWN
+        );
+
+        g2DpadLeft  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_LEFT
+        );
+
+
+        g2DpadRight  = new ToggleButtonReader(
+                g2, GamepadKeys.Button.DPAD_RIGHT
+        );
+
+
+
+
+        drivetrain = new Drivetrain(robot, TELE, g1);
+
+        drivetrain.setMode("Default");
+
+        drivetrain.setDefaultSpeed(defaultSpeed);
+
+        drivetrain.setSlowSpeed(slowMoSpeed);
+
+        intake = new Intake(robot);
+
+        transfer = new Transfer(robot);
+
+
+        spindexer = new Spindexer(robot, TELE);
+
+        spindexer.setTelemetryOn(true);
+
+        shooter = new Shooter(robot, TELE);
+
+        shooter.setShooterMode("MANUAL");
+
+        robot.rejecter.setPosition(rIn);
+
+
+
+
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        drive  = new MecanumDrive(hardwareMap, teleStart);
+
+
+        while(opModeIsActive()){
+
+            drive.updatePoseEstimate();
+
+            TELE.addData("pose", drive.localizer.getPose());
+
+            TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
+
+
+            TELE.addData("off", offset);
+
+
+            robot.hood.setPosition(pos);
+
+            g1LeftBumper.readValue();
+
+            if (g1LeftBumper.wasJustPressed()){
+                g2LeftBumperStamp = getRuntime();
+
+
+
+                spindexer.intakeShake(getRuntime());
+
+                leftBumper = true;
+            }
+
+            if (leftBumper){
+                double time = getRuntime() - g2LeftBumperStamp;
+
+                if (time < 1.0){
+                    robot.rejecter.setPosition(rOut);
+                } else {
+                    robot.rejecter.setPosition(rIn);
+                }
+
+            }
+
+
+
+
+            intake();
+
+            drivetrain.update();
+
+            TELE.update();
+
+            transfer.update();
+
+            g2RightBumper.readValue();
+
+            g2LeftBumper.readValue();
+
+            g2DpadDown.readValue();
+
+            g2DpadUp.readValue();
+
+            if (!scoreAll){
+                spindexer.checkForBalls();
+            }
+
+            if(g2DpadUp.wasJustPressed()){
+                pos -=0.02;
+            }
+
+            if(g2DpadDown.wasJustPressed()){
+                pos +=0.02;
+            }
+
+            g2DpadLeft.readValue();
+
+            g2DpadRight.readValue();
+
+            if(g2DpadLeft.wasJustPressed()){
+                offset -=0.02;
+            }
+
+            if(g2DpadRight.wasJustPressed()){
+                offset +=0.02;
+            }
+
+
+
+            TELE.addData("hood", pos);
+
+
+
+
+
+
+
+
+            if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) {
+
+
+
+
+                shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset);
+
+            } else {
+
+                autotrack = false;
+
+                shooter.moveTurret(0.3+offset);
+
+            }
+
+            if (gamepad2.right_stick_button){
+                autotrack = true;
+            }
+
+
+
+            if (g2RightBumper.wasJustPressed()){
+                transfer.setTransferPower(1);
+                transfer.transferIn();
+                shooter.setManualPower(1);
+
+                notShooting = false;
+
+            }
+
+            if (g2RightBumper.wasJustReleased()){
+                transfer.setTransferPower(1);
+                transfer.transferOut();
+            }
+
+            if (gamepad2.left_stick_y>0.5){
+
+                shooter.setManualPower(0);
+            } else if (gamepad2.left_stick_y<-0.5){
+                shooter.setManualPower(1);
+            }
+
+            if (g2LeftBumper.wasJustPressed()){
+                g2LeftBumperStamp = getRuntime();
+                notShooting = false;
+                scoreAll = true;
+            }
+
+            if (scoreAll) {
+                double time = getRuntime() - g2LeftBumperStamp;
+
+
+                shooter.setManualPower(1);
+
+                TELE.addData("greenImportant", green);
+
+                TELE.addData("last", last);
+                TELE.addData("2ndLast", second);
+
+                int numGreen = spindexer.greens();
+
+                if (square) {
+
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else if (tri) {
+
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else if (circle){
+
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (numGreen == 2) {
+                                last = spindexer.outtakePurple(second, last);
+                            } else {
+                                last = spindexer.outtakeGreen(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+                } else{
+
+
+                    if (time < 0.3) {
+
+                        ticker = 0;
+
+                        last = 0;
+                        second = 0;
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+                        } else {
+                            all = true;
+                        }
+
+
+                        transfer.transferOut();
+                        transfer.setTransferPower(1);
+                    } else if (time < 2) {
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake3();
+                                last = 3;
+                                second = 3;
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                                second = last;
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+                                second = last;
+
+                            }
+                        }
+
+                        second = last;
+
+                        ticker++;
+
+
+                    } else if (time < 2.5) {
+
+                        ticker = 0;
+
+                        second = last;
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+
+
+                        }
+
+                        transfer.transferIn();
+                    } else if (time < 4) {
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake2();
+
+                                last = 2;
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+                    } else if (time < 4.5) {
+
+                        ticker = 0;
+
+
+                        if (gamepad2.right_trigger > 0.5) {
+                            green = false;
+
+                            all = gamepad2.left_trigger > 0.5;
+
+                        } else if (gamepad2.left_trigger > 0.5) {
+                            green = true;
+
+                            all = false;
+                        }
+
+                        transfer.transferIn();
+                    } else if (time < 6) {
+
+
+                        transfer.transferOut();
+
+                        if (ticker == 0) {
+
+                            if (all) {
+                                spindexer.outtake1();
+                            } else if (green) {
+                                last = spindexer.outtakeGreen(second, last);
+                            } else {
+                                last = spindexer.outtakePurple(second, last);
+
+                            }
+                        }
+
+                        ticker++;
+
+                    } else if (time < 6.5) {
+                        transfer.transferIn();
+                    } else {
+
+                        ticker = 0;
+
+
+                        scoreAll = false;
+                        transfer.transferOut();
+
+                        shooter.setManualPower(0);
+
+                    }
+
+
+                }
+            }
+
+
+            shooter.update();
+
+
+
+
+
+
+
+
+
+
+
+
+        }
+
+
+
+
+    }
+
+    public void intake(){
+
+
+        g1RightBumper.readValue();
+
+        g2Circle.readValue();
+
+        g2Square.readValue();
+
+        g2Triangle.readValue();
+
+        if (g1RightBumper.wasJustPressed()){
+
+            notShooting = true;
+
+
+
+
+            if (getRuntime() - g1RightBumperStamp < 0.3){
+                intake.reverse();
+            } else {
+                intake.toggle();
+            }
+
+            if (intake.getIntakeState()==1){
+                shooter.setManualPower(0);
+            }
+
+
+
+
+            spindexer.intake();
+            
+            transfer.transferOut();
+
+
+            g1RightBumperStamp = getRuntime();
+
+        }
+
+
+        if (intake.getIntakeState()==1 && notShooting) {
+
+            spindexer.intakeShake(getRuntime());
+
+        } else {
+            if (g2Circle.wasJustPressed()){
+                circle = true;
+                tri = false;
+                square = false;
+
+
+
+            }
+
+            if (g2Triangle.wasJustPressed()){
+                circle = false;
+                tri = true;
+                square = false;
+            }
+
+            if (g2Square.wasJustPressed()){
+                circle = false;
+                tri = false;
+                square = true;
+            }
+
+            if (gamepad2.x){
+                circle = false;
+                tri = false;
+                square = false;
+            }
+
+
+
+
+        }
+
+
+        intake.update();
+
+
+
+
+        spindexer.update();
+
+
+
+
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java
new file mode 100644
index 0000000..30ae795
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java
@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.subsystems.Transfer;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+public class TransferTest extends LinearOpMode {
+
+    Robot robot;
+
+
+    Transfer transfer;
+
+    public static double pos = 0.40;
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        transfer = new Transfer(robot);
+
+        waitForStart();
+
+        while (opModeIsActive()){
+            transfer.setTransferPosition(pos);
+        }
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java
new file mode 100644
index 0000000..d2f3bc5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java
@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
+
+
+@TeleOp
+@Config
+public class WebcamTest extends LinearOpMode {
+
+    AprilTag webcam;
+
+    MultipleTelemetry TELE;
+
+    Robot robot;
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        webcam = new AprilTag(robot, TELE);
+
+        webcam.turnTelemetryOn(true);
+
+
+
+
+        while(opModeInInit()){
+
+            webcam.initTelemetry();
+
+            TELE.update();
+
+        };
+
+        if(isStopRequested()) return;
+
+
+        while (opModeIsActive()){
+
+            webcam.update();
+
+            TELE.update();
+
+        }
+
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/blank.java
new file mode 100644
index 0000000..dd32ec5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/blank.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+public class blank {
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java
new file mode 100644
index 0000000..fb1c65b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java
@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@TeleOp
+public class MotorDirectionDebugger extends LinearOpMode {
+
+    public static double flPower = 0.0;
+
+    public static double frPower = 0.0;
+
+    public static double blPower = 0.0;
+    public static double brPower = 0.0;
+
+    Robot robot;
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+
+        waitForStart();
+
+        if(isStopRequested()) return;
+
+        while(opModeIsActive()){
+
+            robot.frontLeft.setPower(flPower);
+            robot.frontRight.setPower(frPower);
+            robot.backRight.setPower(brPower);
+            robot.backLeft.setPower(blPower);
+
+
+
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java
new file mode 100644
index 0000000..6427919
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java
@@ -0,0 +1,233 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.hardware.rev.RevColorSensorV3;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+
+@Config
+@TeleOp
+public class ConfigureColorRangefinder extends LinearOpMode {
+
+
+
+    public static double lowerBound = 80;
+    public static double higherBound = 120;
+
+    public static int led = 0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color"));
+        waitForStart();
+        /* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
+            pin0 --> purple
+            pin1 --> green */
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 3,  20);
+        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, lowerBound, higherBound); // green
+        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 25); //25 mm or closer
+        crf.setLedBrightness(led);
+
+    }
+}
+
+/**
+ * Helper class for configuring the Brushland Labs Color Rangefinder.
+ * Online documentation: <a href="https://docs.brushlandlabs.com">...</a>
+ */
+class ColorRangefinder {
+    private final I2cDeviceSynchSimple i2c;
+
+    public ColorRangefinder(RevColorSensorV3 emulator) {
+        this.i2c = emulator.getDeviceClient();
+        this.i2c.enableWriteCoalescing(true);
+    }
+
+    /**
+     * Configure Pin 0 to be in digital mode, and add a threshold.
+     * Multiple thresholds can be added to the same pin by calling this function repeatedly.
+     * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
+     */
+    public void setPin0Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
+        setDigital(PinNum.PIN0, digitalMode, lowerBound, higherBound);
+    }
+
+    /**
+     * Configure Pin 1 to be in digital mode, and add a threshold.
+     * Multiple thresholds can be added to the same pin by calling this function repeatedly.
+     * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
+     */
+    public void setPin1Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
+        setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound);
+    }
+
+    /**
+     * Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger.
+     * This is most useful when we want to know if an object is both close and the correct color.
+     */
+    public void setPin0DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
+        setPin0Digital(digitalMode, mmRequirement, mmRequirement);
+    }
+
+    /**
+     * Sets the maximum distance (in millimeters) within which an object must be located for Pin 1's thresholds to trigger.
+     * This is most useful when we want to know if an object is both close and the correct color.
+     */
+    public void setPin1DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
+        setPin1Digital(digitalMode, mmRequirement, mmRequirement);
+    }
+
+    /**
+     * Invert the hue value before thresholding it, meaning that the colors become their opposite.
+     * This is useful if we want to threshold red; instead of having two thresholds we would invert
+     * the color and look for blue.
+     */
+    public void setPin0InvertHue() {
+        setPin0DigitalMaxDistance(DigitalMode.HSV, 200);
+    }
+
+    /**
+     * Invert the hue value before thresholding it, meaning that the colors become their opposite.
+     * This is useful if we want to threshold red; instead of having two thresholds we would invert
+     * the color and look for blue.
+     */
+    public void setPin1InvertHue() {
+        setPin1DigitalMaxDistance(DigitalMode.HSV, 200);
+    }
+
+    /**
+     * The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog.
+     * For the full range of that channel, leave the denominator as 65535 for colors or 100 for distance.
+     * Smaller values will clip off higher ranges of the data in exchange for higher resolution within a lower range.
+     */
+    public void setPin0Analog(AnalogMode analogMode, int denominator) {
+        byte denom0 = (byte) (denominator & 0xFF);
+        byte denom1 = (byte) ((denominator & 0xFF00) >> 8);
+        i2c.write(PinNum.PIN0.modeAddress, new byte[]{analogMode.value, denom0, denom1});
+    }
+
+    /**
+     * Configure Pin 0 as analog output of one of the six data channels.
+     * To read analog, make sure the physical switch on the sensor is flipped away from the
+     * connector side.
+     */
+    public void setPin0Analog(AnalogMode analogMode) {
+        setPin0Analog(analogMode, analogMode == AnalogMode.DISTANCE ? 100 : 0xFFFF);
+    }
+
+    public float[] getCalibration() {
+        java.nio.ByteBuffer bytes =
+                java.nio.ByteBuffer.wrap(i2c.read(CALIB_A_VAL_0, 16)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
+        return new float[]{bytes.getFloat(), bytes.getFloat(), bytes.getFloat(), bytes.getFloat()};
+    }
+
+    /**
+     * Save a brightness value of the LED to the sensor.
+     *
+     * @param value brightness between 0-255
+     */
+    public void setLedBrightness(int value) {
+        i2c.write8(LED_BRIGHTNESS, value);
+    }
+
+    /**
+     * Change the I2C address at which the sensor will be found. The address can be reset to the
+     * default of 0x52 by holding the reset button.
+     *
+     * @param value new I2C address from 1 to 127
+     */
+    public void setI2cAddress(int value) {
+        i2c.write8(I2C_ADDRESS_REG, value << 1);
+    }
+
+    /**
+     * Read distance via I2C
+     * @return distance in millimeters
+     */
+    public double readDistance() {
+        java.nio.ByteBuffer bytes =
+                java.nio.ByteBuffer.wrap(i2c.read(PS_DISTANCE_0, 4)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
+        return bytes.getFloat();
+    }
+
+    private void setDigital(
+            PinNum pinNum,
+            DigitalMode digitalMode,
+            double lowerBound,
+            double higherBound
+    ) {
+        int lo, hi;
+        if (lowerBound == higherBound) {
+            lo = (int) lowerBound;
+            hi = (int) higherBound;
+        } else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255
+            lo = (int) Math.round(lowerBound / 255.0 * 65535);
+            hi = (int) Math.round(higherBound / 255.0 * 65535);
+        } else { // distance in mm
+            float[] calib = getCalibration();
+            if (lowerBound < .5) hi = 2048;
+            else hi = rawFromDistance(calib[0], calib[1], calib[2], calib[3], lowerBound);
+            lo = rawFromDistance(calib[0], calib[1], calib[2], calib[3], higherBound);
+        }
+
+        byte lo0 = (byte) (lo & 0xFF);
+        byte lo1 = (byte) ((lo & 0xFF00) >> 8);
+        byte hi0 = (byte) (hi & 0xFF);
+        byte hi1 = (byte) ((hi & 0xFF00) >> 8);
+        i2c.write(pinNum.modeAddress, new byte[]{digitalMode.value, lo0, lo1, hi0, hi1});
+        try {
+            Thread.sleep(25);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+    }
+
+    private double root(double n, double v) {
+        double val = Math.pow(v, 1.0 / Math.abs(n));
+        if (n < 0) val = 1.0 / val;
+        return val;
+    }
+
+    private int rawFromDistance(float a, float b, float c, float x0, double mm) {
+        return (int) (root(b, (mm - c) / a) + x0);
+    }
+
+    private enum PinNum {
+        PIN0(0x28), PIN1(0x2D);
+
+        private final byte modeAddress;
+
+        PinNum(int modeAddress) {
+            this.modeAddress = (byte) modeAddress;
+        }
+    }
+
+    // other writeable registers
+    private static final byte CALIB_A_VAL_0 = 0x32;
+    private static final byte PS_DISTANCE_0 = 0x42;
+    private static final byte LED_BRIGHTNESS = 0x46;
+    private static final byte I2C_ADDRESS_REG = 0x47;
+
+    public static int invertHue(int hue360) {
+        return ((hue360 - 180) % 360);
+    }
+
+    public enum DigitalMode {
+        RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6);
+        public final byte value;
+
+        DigitalMode(int value) {
+            this.value = (byte) value;
+        }
+    }
+
+    public enum AnalogMode {
+        RED(13), BLUE(14), GREEN(15), ALPHA(16), HSV(17), DISTANCE(18);
+        public final byte value;
+
+        AnalogMode(int value) {
+            this.value = (byte) value;
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
new file mode 100644
index 0000000..cbae6f0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
@@ -0,0 +1,154 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorImplEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+import org.openftc.easyopencv.OpenCvWebcam;
+
+public class Robot {
+
+    //Initialize Public Components
+
+    public DcMotorEx frontLeft;
+    public DcMotorEx frontRight;
+
+    public DcMotorEx backLeft;
+
+    public DcMotorEx backRight;
+
+    public DcMotorEx intake;
+
+    public DcMotorEx transfer;
+
+
+
+    public DcMotorEx shooter1;
+    public DcMotorEx shooter2;
+    public Servo hood;
+
+    public Servo transferServo;
+
+    public Servo rejecter;
+
+    public Servo turr1;
+
+    public Servo turr2;
+
+    public Servo spin1;
+
+    public Servo spin2;
+
+    public DigitalChannel pin0;
+
+    public DigitalChannel pin1;
+    public DigitalChannel pin2;
+    public DigitalChannel pin3;
+    public DigitalChannel pin4;
+
+    public DigitalChannel pin5;
+
+    public AnalogInput analogInput;
+
+    public AnalogInput analogInput2;
+
+    public AprilTagProcessor aprilTagProcessor;
+
+
+    public WebcamName webcam;
+
+    public DcMotorEx shooterEncoder;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    public Robot (HardwareMap hardwareMap) {
+
+        //Define components w/ hardware map
+
+        frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
+        frontRight = hardwareMap.get(DcMotorEx.class, "fr");
+        backLeft = hardwareMap.get(DcMotorEx.class, "bl");
+        backRight = hardwareMap.get(DcMotorEx.class, "br");
+        frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+        intake = hardwareMap.get(DcMotorEx.class, "intake");
+        rejecter = hardwareMap.get(Servo.class, "rejecter");
+
+        shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
+
+        shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
+
+        shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        hood = hardwareMap.get(Servo.class, "hood");
+
+        turr1 = hardwareMap.get(Servo.class, "t1");
+
+        turr2 = hardwareMap.get(Servo.class, "t2");
+
+        spin1 = hardwareMap.get(Servo.class, "spin1");
+
+        spin2 = hardwareMap.get(Servo.class, "spin2");
+
+        pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
+
+        pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
+
+        pin2 = hardwareMap.get(DigitalChannel.class, "pin2");
+
+        pin3 = hardwareMap.get(DigitalChannel.class, "pin3");
+
+        pin4 = hardwareMap.get(DigitalChannel.class, "pin4");
+
+        pin5 = hardwareMap.get(DigitalChannel.class, "pin5");
+
+
+
+        analogInput = hardwareMap.get(AnalogInput.class, "analog");
+
+
+        analogInput2 = hardwareMap.get(AnalogInput.class, "analog2");
+
+        transfer = hardwareMap.get(DcMotorEx.class, "transfer");
+
+        transferServo = hardwareMap.get(Servo.class, "transferServo");
+
+        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
+
+
+        webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+
+
+
+
+
+    }
+}
diff --git a/build.common.gradle b/build.common.gradle
index a1d2fa4..2d040b3 100644
--- a/build.common.gradle
+++ b/build.common.gradle
@@ -21,7 +21,7 @@ apply plugin: 'com.android.application'
 
 android {
 
-    compileSdkVersion 30
+    compileSdkVersion 34
 
     signingConfigs {
         release {
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
index 1c07e8c..770fdae 100644
--- a/build.dependencies.gradle
+++ b/build.dependencies.gradle
@@ -1,6 +1,11 @@
 repositories {
     mavenCentral()
     google() // Needed for androidx
+    maven { url = 'https://maven.pedropathing.com' } //Pedro
+    maven { url = "https://mymaven.bylazar.com/releases" } //Panels
+    maven { url = 'https://maven.brott.dev/' } //RR
+    maven { url = "https://maven.rowanmcalpin.com/" } //Next FTC
+    maven { url = "https://repo.dairy.foundation/releases" } //AS
 }
 
 dependencies {
@@ -13,5 +18,33 @@ dependencies {
     implementation 'org.firstinspires.ftc:FtcCommon:11.0.0'
     implementation 'org.firstinspires.ftc:Vision:11.0.0'
     implementation 'androidx.appcompat:appcompat:1.2.0'
+
+
+
+    implementation 'com.pedropathing:ftc:2.0.1' //PedroCore
+    implementation 'com.pedropathing:telemetry:0.0.6' //PedroTele
+
+    implementation 'com.bylazar:fullpanels:1.0.2' //Panels
+
+    implementation "com.acmerobotics.roadrunner:ftc:0.1.25" //RR
+    implementation "com.acmerobotics.roadrunner:core:1.0.1" //RR
+    implementation "com.acmerobotics.roadrunner:actions:1.0.1" //RR
+    implementation "com.acmerobotics.dashboard:dashboard:0.4.17" //FTC Dash
+
+    implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB
+
+    implementation 'com.rowanmcalpin.nextftc:core:0.6.2' //NEXT FTC
+    implementation 'com.rowanmcalpin.nextftc:ftc:0.6.2'//NEXT FTC
+    implementation 'com.rowanmcalpin.nextftc:pedro:0.6.2'//NEXT FTC
+
+    implementation("page.j5155.AdvantageScope:lite:v26.0.0") //AS
+
+
+
+
+
+
+
+
 }