From a810199c520c16826ec2282634511a6441d61007 Mon Sep 17 00:00:00 2001
From: gatorgrad96 <jacobmm11901@gmail.com>
Date: Wed, 28 Jan 2026 10:07:47 -0500
Subject: [PATCH] Red Auto Front

Added the red auto (untested)
Removed the unused autos
---
 .../firstinspires/ftc/teamcode/AutoMain.java  |  20 +-
 .../ftc/teamcode/LimeLightAutoBlue.java       |   7 +-
 .../ftc/teamcode/LimeLightAutoBlueTEST.java   |   2 +-
 .../ftc/teamcode/LimeLightAutoRed.java        | 317 +++++++++++++++++
 .../ftc/teamcode/MecanumDrive.java            | 321 +++++++++---------
 .../ftc/teamcode/OTOSLocalizer.java           |  20 +-
 .../ftc/teamcode/PinpointLocalizer.java       |  23 +-
 .../ftc/teamcode/QuickAutoBlue.java           |   2 +-
 .../ftc/teamcode/QuickAutoForward.java        |   2 +-
 .../ftc/teamcode/QuickAutoRed.java            |   2 +-
 .../firstinspires/ftc/teamcode/TankDrive.java | 303 ++++++++---------
 .../ftc/teamcode/TeleOpMain.java              |   8 +-
 .../ftc/teamcode/ThreeDeadWheelLocalizer.java |  54 ++-
 .../ftc/teamcode/TwoDeadWheelLocalizer.java   |  65 ++--
 .../ftc/teamcode/tuning/LocalizationTest.java |  23 +-
 .../teamcode/tuning/ManualFeedbackTuner.java  |  16 +-
 .../ftc/teamcode/tuning/SplineTest.java       |  16 +-
 .../ftc/teamcode/tuning/TuningOpModes.java    | 160 ++++-----
 18 files changed, 834 insertions(+), 527 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoRed.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java
index 7bc1c93..3dd1698 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java
@@ -26,22 +26,14 @@
 import org.firstinspires.ftc.teamcode.Utils_13233.CommonAutoMethods;
 import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor;
 
-@Autonomous(name = "AutoMain", group = "Auto")
+//@Autonomous(name = "AutoMain", group = "Auto")
 public class AutoMain extends LinearOpMode {
+    // Global Variables to store Game Specific Information
+    AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected
     private AutoDrive drive;
     private AutoTurn turn;
     private CommonAutoMethods autoMethods;
 
-    // Possible Autonomous Modes
-    public enum AutoMode {
-        AUTO_MODE_NOT_SELECTED,
-        AUTO_MODE_DEFAULT,
-    }
-
-    // Global Variables to store Game Specific Information
-    AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected
-
-
     // OpMode for autonomous code
     @Override
     public void runOpMode() throws InterruptedException {
@@ -119,4 +111,10 @@ public void runOpMode() throws InterruptedException {
     private void defaultAuto() {
         drive.driveForward(10.0, AutoConstants.quarterPower);
     }
+
+    // Possible Autonomous Modes
+    public enum AutoMode {
+        AUTO_MODE_NOT_SELECTED,
+        AUTO_MODE_DEFAULT,
+    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java
index aa66945..327032e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java
@@ -18,7 +18,7 @@
 
 import java.util.List;
 
-@Autonomous(name = "LimeLightAutoBlue", group = "Auto")
+//@Autonomous(name = "LimeLightAutoBlue", group = "Auto")
 
 public class LimeLightAutoBlue extends LinearOpMode {
 
@@ -207,7 +207,8 @@ public void runOpMode() {
                     break;
 
             }
-        }switch (limelightCont.getTagID()) {
+        }
+        switch (limelightCont.getTagID()) {
             case 23:
                 //runs PPG
                 telemetry.addLine("AUTO: Purple Purple Green");
@@ -254,6 +255,6 @@ public void runOpMode() {
                 sleep(1000);
                 drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line
                 sleep(1000);
+        }
     }
 }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java
index 0b48764..ae0e13c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java
@@ -17,7 +17,7 @@
 import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor;
 import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls;
 
-@Autonomous(name = "LimeLightBlueTEST ", group = "Auto")
+@Autonomous(name = "LimeLightBlue", group = "Auto")
 public class LimeLightAutoBlueTEST extends LinearOpMode {
 
     // ================= HARDWARE =================
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoRed.java
new file mode 100644
index 0000000..c688960
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoRed.java
@@ -0,0 +1,317 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.Servo;
+
+
+import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
+import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
+import org.firstinspires.ftc.teamcode.Utils_13233.LimelightControls;
+import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor;
+import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls;
+
+@Autonomous(name = "LimeLightRed", group = "Auto")
+public class LimeLightAutoRed extends LinearOpMode {
+
+    // ================= HARDWARE =================
+
+    private DriveControls drive;
+    private LaunchControls launch;
+    private SorterControls sorter;
+    private MotorConstructor motors;
+    private LimelightControls limelightCont;
+
+    @Override
+    public void runOpMode() {
+
+        //INIT
+        drive = new DriveControls(hardwareMap);
+        launch = new LaunchControls(hardwareMap);
+        motors = new MotorConstructor(hardwareMap);
+        limelightCont = new LimelightControls(hardwareMap);
+        sorter = new SorterControls(motors);
+        telemetry.addLine("Initialized. Waiting for start...");
+        telemetry.update();
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        // - PHASE 1: MOVE TO SEE TAG
+//        preMove();
+        drive.setDrivePower(0, 1.0f, 0, 0);//strafes over to launch
+        sleep(1400);
+
+        int tagID = scanTag();
+
+        drive.setDrivePower(0.0f, 0.0f, 1.0f, 0.0f);
+        sleep(550);
+
+        drive.setDrivePower(0.0f);
+
+        // - PHASE 2: DETECT TAG
+
+        telemetry.addData("ID", limelightCont.getTagID());
+        telemetry.update();
+        sleep(500);
+
+        //  PHASE 3: RUN AUTO
+        runAuto(tagID);
+
+        //  END
+        requestOpModeStop();
+    }
+
+    // ==========================================================
+    // ===================== PHASE METHODS ======================
+    // ==========================================================
+
+//    private void preMove() {
+//    }
+
+    private int scanTag() {
+        int tagID = -1;
+        long startTime = System.currentTimeMillis();
+
+        while (opModeIsActive()
+            && tagID == -1
+            && System.currentTimeMillis() - startTime < 2000) {
+
+            tagID = limelightCont.getTagID();
+
+            telemetry.addData("Scanning Tag", tagID);
+            telemetry.update();
+        }
+
+        return tagID;
+    }
+
+    private void runAuto(int tagID) {
+
+        switch (tagID) {
+
+            case 21:
+                telemetry.addLine("AUTO: Green Purple Purple");
+                telemetry.update();
+                auto21();
+                break;
+
+            case 22:
+                telemetry.addLine("AUTO: Purple Green Purple");
+                telemetry.update();
+                auto22();
+                break;
+
+            case 23:
+                telemetry.addLine("AUTO: Purple Purple Green");
+                telemetry.update();
+                auto23();
+                break;
+
+            default:
+                telemetry.addLine("AUTO: DEFAULT (NO TAG)");
+                telemetry.update();
+                auto24();
+                break;
+        }
+    }
+
+    // ==========================================================
+    // ===================== AUTO PATHS =========================
+    // ==========================================================
+
+    private void auto21() {
+        //runs GPP
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);
+        //sets position to ensure the ball is correctly lined up
+        launch.setLaunchPower(true, 0.8f);
+
+        sleep(2500);//wait state to wait for launcher to spin up
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);//runs flipper servo into motor
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);
+        motors.Flipper.setPosition(0.15);
+        sleep(2000);
+
+        //2nd ball code
+        sleep(750);//waits to spin turntable
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2);
+        sleep(1500);//waits while turntable spins
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);
+        motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision
+        sleep(2000);
+
+        //3rd ball code
+        sleep(1000);
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3);
+        sleep(1000);//turns off ramp
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);// sets servo back to init pos
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball
+        motors.Flipper.setPosition(0.15);
+        sleep(1000);
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init
+        launch.setLaunchPower(false);//turns off launcher and ramp
+
+        //strafing section
+        drive.setDrivePower(0, -1.0f, 0, 0);
+        sleep(1000);
+        drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line
+        sleep(1000);
+        //id 21
+    }
+
+    private void auto22() {
+        //runs PGP
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2);
+        //sets position to ensure the ball is correctly lined up
+        launch.setLaunchPower(true, 0.9f);
+
+        sleep(2500);//wait state to wait for launcher to spin up
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);//runs flipper servo into motor
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);
+        motors.Flipper.setPosition(0.15);
+        sleep(2000);
+
+        //2nd ball code
+        sleep(750);//waits to spin turntable
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);
+        sleep(1500);//waits while turntable spins
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);
+        motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision
+        sleep(2000);
+
+        //3rd ball code
+        sleep(1000);
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3);
+        sleep(1000);//turns off ramp
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);// sets servo back to init pos
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball
+        motors.Flipper.setPosition(0.15);
+        sleep(1000);
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init
+        launch.setLaunchPower(false);//turns off launcher and ramp
+
+        //strafing section
+        drive.setDrivePower(0, -1.0f, 0, 0);
+        sleep(1000);
+        drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line
+        sleep(1000);
+    }
+
+    private void auto23() {
+        //runs PPG
+
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2);
+        //sets position to ensure the ball is correctly lined up
+        launch.setLaunchPower(true, 0.9f);
+
+        sleep(2500);//wait state to wait for launcher to spin up
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);//runs flipper servo into motor
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);
+        motors.Flipper.setPosition(0.15);
+        sleep(2000);
+
+        //2nd ball code
+        sleep(750);//waits to spin turntable
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3);
+        sleep(1500);//waits while turntable spins
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);
+        motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision
+        sleep(2000);
+
+        //3rd ball code
+        sleep(1000);
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);
+        sleep(1000);//turns off ramp
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);// sets servo back to init pos
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball
+        motors.Flipper.setPosition(0.15);
+        sleep(1000);
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init
+        launch.setLaunchPower(false);//turns off launcher and ramp
+
+        //strafing section
+        drive.setDrivePower(0, -1.0f, 0, 0);
+        sleep(1000);
+        drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line
+        sleep(1000);
+
+    }
+
+
+    private void auto24() {
+        //runs default (PPG)
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2);
+        //sets position to ensure the ball is correctly lined up
+        launch.setLaunchPower(true, 0.9f);
+
+        sleep(2500);//wait state to wait for launcher to spin up
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);//runs flipper servo into motor
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);
+        motors.Flipper.setPosition(0.15);
+        sleep(2000);
+
+        //2nd ball code
+        sleep(750);//waits to spin turntable
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3);
+        sleep(1500);//waits while turntable spins
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);
+        motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision
+        sleep(2000);
+
+        //3rd ball code
+        sleep(1000);
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);
+        sleep(1000);//turns off ramp
+        motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball
+        motors.Flipper.setPosition(0.0);
+        sleep(1000);// sets servo back to init pos
+        motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball
+        motors.Flipper.setPosition(0.15);
+        sleep(1000);
+        sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init
+        launch.setLaunchPower(false);//turns off launcher and ramp
+
+        //strafing section
+        drive.setDrivePower(0, -1.0f, 0, 0);
+        sleep(1000);
+        drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line
+        sleep(1000);
+    }
+
+
+}
+
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
index e1f5556..c57b0f0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -32,6 +32,7 @@
 import com.acmerobotics.roadrunner.ftc.RawEncoder;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -50,16 +51,134 @@
 import java.util.LinkedList;
 import java.util.List;
 
+@Disabled
 @Config
 public final class MecanumDrive {
+    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 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, "leftFront");
+        leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
+        rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
+        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+
+        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);
+
+        // 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("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 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
+        );
+    }
+
     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;
+            RevHubOrientationOnRobot.LogoFacingDirection.UP;
         public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
-                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+            RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
 
         // drive model parameters
         public double inPerTick = 1;
@@ -90,35 +209,6 @@ public static class Params {
         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;
@@ -143,13 +233,13 @@ public DriveLocalizer(Pose2d pose) {
         }
 
         @Override
-        public void setPose(Pose2d pose) {
-            this.pose = pose;
+        public Pose2d getPose() {
+            return pose;
         }
 
         @Override
-        public Pose2d getPose() {
-            return pose;
+        public void setPose(Pose2d pose) {
+            this.pose = pose;
         }
 
         @Override
@@ -162,7 +252,7 @@ public PoseVelocity2d update() {
             YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
 
             FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
-                    leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
+                leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
 
             Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
 
@@ -181,22 +271,22 @@ public PoseVelocity2d update() {
 
             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)
+                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;
@@ -207,75 +297,25 @@ public PoseVelocity2d update() {
             lastHeading = heading;
 
             pose = pose.plus(new Twist2d(
-                    twist.line.value(),
-                    headingDelta
+                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, "leftFront");
-        leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
-        rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
-        rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
-
-        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);
-
-        // 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("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;
+        private double beginTs = -1;
 
         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)));
+                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++) {
@@ -310,23 +350,23 @@ public boolean run(@NonNull TelemetryPacket p) {
             PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
             PoseVelocity2dDual<Time> command = new HolonomicController(
-                    PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
-                    PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
+                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
+                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
             )
-                    .compute(txWorldTarget, localizer.getPose(), robotVelRobot);
+                .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);
+                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
+                voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
             ));
 
             leftFront.setPower(leftFrontPower);
@@ -402,22 +442,22 @@ public boolean run(@NonNull TelemetryPacket p) {
             PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
             PoseVelocity2dDual<Time> command = new HolonomicController(
-                    PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
-                    PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
+                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
+                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
             )
-                    .compute(txWorldTarget, localizer.getPose(), robotVelRobot);
+                .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);
+                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
+                voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
             ));
 
             leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
@@ -446,51 +486,4 @@ public void preview(Canvas c) {
             c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
         }
     }
-
-    public PoseVelocity2d updatePoseEstimate() {
-        PoseVelocity2d vel = localizer.update();
-        poseHistory.add(localizer.getPose());
-        
-        while (poseHistory.size() > 100) {
-            poseHistory.removeFirst();
-        }
-
-        estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
-        
-        
-        return vel;
-    }
-
-    private void drawPoseHistory(Canvas c) {
-        double[] xPoints = new double[poseHistory.size()];
-        double[] yPoints = new double[poseHistory.size()];
-
-        int i = 0;
-        for (Pose2d t : poseHistory) {
-            xPoints[i] = t.position.x;
-            yPoints[i] = t.position.y;
-
-            i++;
-        }
-
-        c.setStrokeWidth(1);
-        c.setStroke("#3F51B5");
-        c.strokePolyline(xPoints, yPoints);
-    }
-
-    public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
-        return new TrajectoryActionBuilder(
-                TurnAction::new,
-                FollowTrajectoryAction::new,
-                new TrajectoryBuilderParams(
-                        1e-6,
-                        new ProfileParams(
-                                0.25, 0.1, 1e-2
-                        )
-                ),
-                beginPose, 0.0,
-                defaultTurnConstraints,
-                defaultVelConstraint, defaultAccelConstraint
-        );
-    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSLocalizer.java
index b35eb5a..ca5ce84 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSLocalizer.java
@@ -7,26 +7,18 @@
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.OTOSKt;
 import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 
+@Disabled
 @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
@@ -65,4 +57,12 @@ public PoseVelocity2d update() {
         Vector2d robotVel = Rotation2d.exp(otosPose.h).inverse().times(fieldVel);
         return new PoseVelocity2d(robotVel, otosVel.h);
     }
+
+    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);
+    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointLocalizer.java
index b7191af..bc33028 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointLocalizer.java
@@ -6,6 +6,7 @@
 import com.acmerobotics.roadrunner.Rotation2d;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
@@ -13,21 +14,14 @@
 
 import java.util.Objects;
 
+@Disabled
 @Config
 public final class PinpointLocalizer 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 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
@@ -49,13 +43,13 @@ public PinpointLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initi
     }
 
     @Override
-    public void setPose(Pose2d pose) {
-        txWorldPinpoint = pose.times(txPinpointRobot.inverse());
+    public Pose2d getPose() {
+        return txWorldPinpoint.times(txPinpointRobot);
     }
 
     @Override
-    public Pose2d getPose() {
-        return txWorldPinpoint.times(txPinpointRobot);
+    public void setPose(Pose2d pose) {
+        txWorldPinpoint = pose.times(txPinpointRobot.inverse());
     }
 
     @Override
@@ -70,4 +64,9 @@ public PoseVelocity2d update() {
         }
         return new PoseVelocity2d(new Vector2d(0, 0), 0);
     }
+
+    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)
+    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java
index afed258..9f40074 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java
@@ -11,7 +11,7 @@
 import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor;
 import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls;
 
-@Autonomous(name = "QuickAutoBlue", group = "Autonomous")
+//@Autonomous(name = "QuickAutoBlue", group = "Autonomous")
 public class QuickAutoBlue extends LinearOpMode {
     // Constructors for the utils classes
     private DriveControls drive;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java
index 97519c9..a3962a8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java
@@ -5,7 +5,7 @@
 
 import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
 
-@Autonomous(name = "QuickAuto")
+@Autonomous(name = "ForwardAuto")
 public class QuickAutoForward extends LinearOpMode {
     private final DriveControls drive;
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java
index f1965d6..dc7342e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java
@@ -11,7 +11,7 @@
 import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor;
 import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls;
 
-@Autonomous(name = "QuickAutoRed", group = "Autonomous")
+//@Autonomous(name = "QuickAutoRed", group = "Autonomous")
 public class QuickAutoRed extends LinearOpMode {
 
     // Constructors for the utils classes
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
index aa6c2aa..4505476 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -42,6 +42,7 @@
 import com.acmerobotics.roadrunner.ftc.RawEncoder;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -58,16 +59,137 @@
 import java.util.LinkedList;
 import java.util.List;
 
+@Disabled
 @Config
 public final class TankDrive {
+    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 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 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
+        );
+    }
+
     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;
+            RevHubOrientationOnRobot.LogoFacingDirection.UP;
         public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
-                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+            RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
 
         // drive model parameters
         public double inPerTick = 0;
@@ -96,35 +218,6 @@ public static class Params {
         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;
@@ -158,13 +251,13 @@ public DriveLocalizer(Pose2d pose) {
         }
 
         @Override
-        public void setPose(Pose2d pose) {
-            this.pose = pose;
+        public Pose2d getPose() {
+            return pose;
         }
 
         @Override
-        public Pose2d getPose() {
-            return pose;
+        public void setPose(Pose2d pose) {
+            this.pose = pose;
         }
 
         @Override
@@ -193,7 +286,7 @@ public PoseVelocity2d update() {
             meanRightVel /= rightEncs.size();
 
             FlightRecorder.write("TANK_LOCALIZER_INPUTS",
-                     new TankLocalizerInputsMessage(leftReadings, rightReadings));
+                new TankLocalizerInputsMessage(leftReadings, rightReadings));
 
             if (!initialized) {
                 initialized = true;
@@ -206,14 +299,14 @@ public PoseVelocity2d update() {
             }
 
             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)
+                new DualNum<Time>(new double[]{
+                    meanLeftPos - lastLeftPos,
+                    meanLeftVel
+                }).times(PARAMS.inPerTick),
+                new DualNum<Time>(new double[]{
+                    meanRightPos - lastRightPos,
+                    meanRightVel,
+                }).times(PARAMS.inPerTick)
             ));
 
             lastLeftPos = meanLeftPos;
@@ -225,70 +318,17 @@ public PoseVelocity2d update() {
         }
     }
 
-    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;
+        private double beginTs = -1;
 
         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)));
+                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++) {
@@ -327,13 +367,13 @@ public boolean run(@NonNull TelemetryPacket p) {
             updatePoseEstimate();
 
             PoseVelocity2dDual<Time> command = new RamseteController(kinematics.trackWidth, PARAMS.ramseteZeta, PARAMS.ramseteBBar)
-                    .compute(x, txWorldTarget, localizer.getPose());
+                .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);
+                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));
@@ -415,18 +455,18 @@ public boolean run(@NonNull TelemetryPacket p) {
             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())
-                    )
+                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);
+                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));
@@ -459,51 +499,4 @@ public void preview(Canvas c) {
             c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
         }
     }
-
-    public PoseVelocity2d updatePoseEstimate() {
-        PoseVelocity2d vel = localizer.update();
-        poseHistory.add(localizer.getPose());
-
-        while (poseHistory.size() > 100) {
-            poseHistory.removeFirst();
-        }
-
-        estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
-
-
-        return vel;
-    }
-
-    private void drawPoseHistory(Canvas c) {
-        double[] xPoints = new double[poseHistory.size()];
-        double[] yPoints = new double[poseHistory.size()];
-
-        int i = 0;
-        for (Pose2d t : poseHistory) {
-            xPoints[i] = t.position.x;
-            yPoints[i] = t.position.y;
-
-            i++;
-        }
-
-        c.setStrokeWidth(1);
-        c.setStroke("#3F51B5");
-        c.strokePolyline(xPoints, yPoints);
-    }
-
-    public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
-        return new TrajectoryActionBuilder(
-                TurnAction::new,
-                FollowTrajectoryAction::new,
-                new TrajectoryBuilderParams(
-                        1e-6,
-                        new ProfileParams(
-                                0.25, 0.1, 1e-2
-                        )
-                ),
-                beginPose, 0.0,
-                defaultTurnConstraints,
-                defaultVelConstraint, defaultAccelConstraint
-        );
-    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
index 91b35ad..3202d0d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java
@@ -86,12 +86,14 @@ public void runOpMode() {
             // Set the power to the launch motors based while the x button is being pressed
             // and rumble to let the driver know that the launch motors are being controlled
             if (gamepad1.right_bumper) {
-                launch.setLaunchPower(gamepad1.right_bumper, 0.7f);
+                launch.setLaunchPower(true, 0.7f);
             } else if (gamepad1.left_bumper) {
-                launch.setLaunchPower(gamepad1.left_bumper, 1.0f);
+                launch.setLaunchPower(true);
+            } else {
+                launch.setLaunchPower(false);
             }
+
             sleep(50);
-            launch.setLaunchPower(gamepad1.left_bumper, 1.0f);
 
             // Allows for driver Control of the sorter
             // Intake positions for the sorter
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
index cffe555..feb776a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
@@ -13,29 +13,21 @@
 import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
 import com.acmerobotics.roadrunner.ftc.RawEncoder;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.teamcode.messages.ThreeDeadWheelInputsMessage;
 
+@Disabled
 @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
@@ -55,13 +47,13 @@ public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d
     }
 
     @Override
-    public void setPose(Pose2d pose) {
-        this.pose = pose;
+    public Pose2d getPose() {
+        return pose;
     }
 
     @Override
-    public Pose2d getPose() {
-        return pose;
+    public void setPose(Pose2d pose) {
+        this.pose = pose;
     }
 
     @Override
@@ -87,20 +79,20 @@ public PoseVelocity2d update() {
         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),
-                })
+            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;
@@ -110,4 +102,10 @@ public PoseVelocity2d update() {
         pose = pose.plus(twist.value());
         return twist.velocity().value();
     }
+
+    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)
+    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
index a697692..c6549b6 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
@@ -14,6 +14,7 @@
 import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
 import com.acmerobotics.roadrunner.ftc.RawEncoder;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
@@ -24,27 +25,18 @@
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
 import org.firstinspires.ftc.teamcode.messages.TwoDeadWheelInputsMessage;
 
+@Disabled
 @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 final double inPerTick;
     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
@@ -65,13 +57,13 @@ public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick,
     }
 
     @Override
-    public void setPose(Pose2d pose) {
-        this.pose = pose;
+    public Pose2d getPose() {
+        return pose;
     }
 
     @Override
-    public Pose2d getPose() {
-        return pose;
+    public void setPose(Pose2d pose) {
+        this.pose = pose;
     }
 
     @Override
@@ -83,11 +75,11 @@ public PoseVelocity2d update() {
         // 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
+            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));
@@ -117,20 +109,20 @@ public PoseVelocity2d update() {
         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,
-                })
+            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;
@@ -140,4 +132,9 @@ public PoseVelocity2d update() {
         pose = pose.plus(twist.value());
         return twist.velocity().value();
     }
+
+    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)
+    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
index 151f7ac..354b96e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
@@ -6,13 +6,16 @@
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.PoseVelocity2d;
 import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.Drawing;
 import org.firstinspires.ftc.teamcode.MecanumDrive;
 import org.firstinspires.ftc.teamcode.TankDrive;
 
+@Disabled
 public class LocalizationTest extends LinearOpMode {
+
     @Override
     public void runOpMode() throws InterruptedException {
         telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -24,11 +27,11 @@ public void runOpMode() throws InterruptedException {
 
             while (opModeIsActive()) {
                 drive.setDrivePowers(new PoseVelocity2d(
-                        new Vector2d(
-                                -gamepad1.left_stick_y,
-                                -gamepad1.left_stick_x
-                        ),
-                        -gamepad1.right_stick_x
+                    new Vector2d(
+                        -gamepad1.left_stick_y,
+                        -gamepad1.left_stick_x
+                    ),
+                    -gamepad1.right_stick_x
                 ));
 
                 drive.updatePoseEstimate();
@@ -51,11 +54,11 @@ public void runOpMode() throws InterruptedException {
 
             while (opModeIsActive()) {
                 drive.setDrivePowers(new PoseVelocity2d(
-                        new Vector2d(
-                                -gamepad1.left_stick_y,
-                                0.0
-                        ),
-                        -gamepad1.right_stick_x
+                    new Vector2d(
+                        -gamepad1.left_stick_y,
+                        0.0
+                    ),
+                    -gamepad1.right_stick_x
                 ));
 
                 drive.updatePoseEstimate();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java
index 0b6f193..faea68b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java
@@ -2,6 +2,7 @@
 
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.MecanumDrive;
@@ -9,6 +10,7 @@
 import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
 import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer;
 
+@Disabled
 public final class ManualFeedbackTuner extends LinearOpMode {
     public static double DISTANCE = 64;
 
@@ -16,7 +18,7 @@ public final class ManualFeedbackTuner extends LinearOpMode {
     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.");
@@ -31,9 +33,9 @@ public void runOpMode() throws InterruptedException {
             while (opModeIsActive()) {
                 Actions.runBlocking(
                     drive.actionBuilder(new Pose2d(0, 0, 0))
-                            .lineToX(DISTANCE)
-                            .lineToX(0)
-                            .build());
+                        .lineToX(DISTANCE)
+                        .lineToX(0)
+                        .build());
             }
         } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
             TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
@@ -52,9 +54,9 @@ public void runOpMode() throws InterruptedException {
             while (opModeIsActive()) {
                 Actions.runBlocking(
                     drive.actionBuilder(new Pose2d(0, 0, 0))
-                            .lineToX(DISTANCE)
-                            .lineToX(0)
-                            .build());
+                        .lineToX(DISTANCE)
+                        .lineToX(0)
+                        .build());
             }
         } else {
             throw new RuntimeException();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
index d70825e..fce82b7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
@@ -3,11 +3,13 @@
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.MecanumDrive;
 import org.firstinspires.ftc.teamcode.TankDrive;
 
+@Disabled
 public final class SplineTest extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
@@ -19,19 +21,19 @@ public void runOpMode() throws InterruptedException {
 
             Actions.runBlocking(
                 drive.actionBuilder(beginPose)
-                        .splineTo(new Vector2d(30, 30), Math.PI / 2)
-                        .splineTo(new Vector2d(0, 60), Math.PI)
-                        .build());
+                    .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());
+                drive.actionBuilder(beginPose)
+                    .splineTo(new Vector2d(30, 30), Math.PI / 2)
+                    .splineTo(new Vector2d(0, 60), Math.PI)
+                    .build());
         } else {
             throw new RuntimeException();
         }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
index ca13bb3..8072b7f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
@@ -33,6 +33,7 @@
 import com.acmerobotics.roadrunner.ftc.PinpointView;
 import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
 import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
 import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar;
@@ -51,6 +52,7 @@
 import java.util.Arrays;
 import java.util.List;
 
+@Disabled
 public final class TuningOpModes {
     // TODO: change this to TankDrive.class if you're using tank
     public static final Class<?> DRIVE_CLASS = MecanumDrive.class;
@@ -58,14 +60,15 @@ public final class TuningOpModes {
     public static final String GROUP = "quickstart";
     public static final boolean DISABLED = false;
 
-    private TuningOpModes() {}
+    private TuningOpModes() {
+    }
 
     private static OpModeMeta metaForClass(Class<? extends OpMode> cls) {
         return new OpModeMeta.Builder()
-                .setName(cls.getSimpleName())
-                .setGroup(GROUP)
-                .setFlavor(OpModeMeta.Flavor.TELEOP)
-                .build();
+            .setName(cls.getSimpleName())
+            .setGroup(GROUP)
+            .setFlavor(OpModeMeta.Flavor.TELEOP)
+            .build();
     }
 
     private static PinpointView makePinpointView(PinpointLocalizer pl) {
@@ -93,34 +96,33 @@ public float getHeadingVelocity(UnnormalizedAngleUnit unit) {
                 return (float) pl.driver.getHeadingVelocity(unit);
             }
 
+            @Override
+            public DcMotorSimple.Direction getParDirection() {
+                return parDirection == GoBildaPinpointDriver.EncoderDirection.FORWARD ?
+                    DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE;
+            }
 
             @Override
             public void setParDirection(@NonNull DcMotorSimple.Direction direction) {
                 parDirection = direction == DcMotorSimple.Direction.FORWARD ?
-                        GoBildaPinpointDriver.EncoderDirection.FORWARD :
-                        GoBildaPinpointDriver.EncoderDirection.REVERSED;
+                    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;
+            public DcMotorSimple.Direction getPerpDirection() {
+                return perpDirection == 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;
+                    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;
-            }
         };
     }
 
@@ -140,8 +142,8 @@ public static void register(OpModeManager manager) {
                 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)
+                        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));
@@ -150,8 +152,8 @@ public static void register(OpModeManager manager) {
                 } 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)
+                        hardwareMap.getAll(LynxModule.class),
+                        Arrays.asList(dl.par0, dl.par1, dl.perp)
                     ));
                     parEncs.add(new EncoderRef(0, 0));
                     parEncs.add(new EncoderRef(0, 1));
@@ -159,8 +161,8 @@ public static void register(OpModeManager manager) {
                 } 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)
+                        hardwareMap.getAll(LynxModule.class),
+                        Arrays.asList(dl.par, dl.perp)
                     ));
                     parEncs.add(new EncoderRef(0, 0));
                     perpEncs.add(new EncoderRef(0, 1));
@@ -170,7 +172,7 @@ public static void register(OpModeManager manager) {
                     parEncs.add(new EncoderRef(0, 0));
                     perpEncs.add(new EncoderRef(0, 1));
                     lazyImu = new OTOSIMU(ol.otos);
-                }  else if (md.localizer instanceof PinpointLocalizer) {
+                } else if (md.localizer instanceof PinpointLocalizer) {
                     PinpointView pv = makePinpointView((PinpointLocalizer) md.localizer);
                     encoderGroups.add(new PinpointEncoderGroup(pv));
                     parEncs.add(new EncoderRef(0, 0));
@@ -182,29 +184,29 @@ public static void register(OpModeManager manager) {
 
                 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
+                    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)) {
@@ -221,8 +223,8 @@ public static void register(OpModeManager manager) {
                     allEncoders.addAll(dl.leftEncs);
                     allEncoders.addAll(dl.rightEncs);
                     encoderGroups.add(new LynxQuadratureEncoderGroup(
-                            hardwareMap.getAll(LynxModule.class),
-                            allEncoders
+                        hardwareMap.getAll(LynxModule.class),
+                        allEncoders
                     ));
                     for (int i = 0; i < dl.leftEncs.size(); i++) {
                         leftEncs.add(new EncoderRef(0, i));
@@ -233,8 +235,8 @@ public static void register(OpModeManager manager) {
                 } 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)
+                        hardwareMap.getAll(LynxModule.class),
+                        Arrays.asList(dl.par0, dl.par1, dl.perp)
                     ));
                     parEncs.add(new EncoderRef(0, 0));
                     parEncs.add(new EncoderRef(0, 1));
@@ -242,12 +244,12 @@ public static void register(OpModeManager manager) {
                 } 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)
+                        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) {
+                } else if (td.localizer instanceof PinpointLocalizer) {
                     PinpointView pv = makePinpointView((PinpointLocalizer) td.localizer);
                     encoderGroups.add(new PinpointEncoderGroup(pv));
                     parEncs.add(new EncoderRef(0, 0));
@@ -264,24 +266,24 @@ public static void register(OpModeManager manager) {
                 }
 
                 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
+                    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 {
@@ -308,12 +310,12 @@ public static void register(OpModeManager manager) {
 
         FtcDashboard.getInstance().withConfigRoot(configRoot -> {
             for (Class<?> c : Arrays.asList(
-                    AngularRampLogger.class,
-                    ForwardRampLogger.class,
-                    LateralRampLogger.class,
-                    ManualFeedforwardTuner.class,
-                    MecanumMotorDirectionDebugger.class,
-                    ManualFeedbackTuner.class
+                AngularRampLogger.class,
+                ForwardRampLogger.class,
+                LateralRampLogger.class,
+                ManualFeedforwardTuner.class,
+                MecanumMotorDirectionDebugger.class,
+                ManualFeedbackTuner.class
             )) {
                 configRoot.putVariable(c.getSimpleName(), ReflectionConfig.createVariableFromClass(c));
             }