Merge branch 'master-backup'

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

FtcRobotController/build.gradle

@@ -17,8 +17,8 @@ android {
     buildFeatures {
         buildConfig = true
     }
+    compileSdk 34
 
-    compileSdkVersion 30
 
     compileOptions {
         sourceCompatibility JavaVersion.VERSION_1_8

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

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

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java

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

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

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

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java

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

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/disabled/blank.java

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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]);
+    }
+}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/blank.java

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

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

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

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");
+
+
+
+
+
+    }
+}

build.common.gradle

@@ -21,7 +21,7 @@ apply plugin: 'com.android.application'
 
 android {
 
-    compileSdkVersion 30
+    compileSdkVersion 34
 
     signingConfigs {
         release {

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