diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
deleted file mode 100644
index 23e0db9..0000000
--- a/src/main/deploy/pathplanner/navgrid.json
+++ /dev/null
@@ -1 +0,0 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
deleted file mode 100644
index 8e6437a..0000000
--- a/src/main/deploy/pathplanner/settings.json
+++ /dev/null
@@ -1,32 +0,0 @@
-{
-  "robotWidth": 0.837,
-  "robotLength": 0.837,
-  "holonomicMode": true,
-  "pathFolders": [],
-  "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 4.0,
-  "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
-  "defaultNominalVoltage": 12.0,
-  "robotMass": 50.0,
-  "robotMOI": 4.839,
-  "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.038,
-  "driveGearing": 5.08,
-  "maxDriveSpeed": 5.0,
-  "driveMotorType": "vortex",
-  "driveCurrentLimit": 40.0,
-  "wheelCOF": 0.5,
-  "flModuleX": 0.32,
-  "flModuleY": 0.32,
-  "frModuleX": 0.32,
-  "frModuleY": -0.32,
-  "blModuleX": -0.32,
-  "blModuleY": 0.32,
-  "brModuleX": -0.32,
-  "brModuleY": -0.32,
-  "bumperOffsetX": 0.0,
-  "bumperOffsetY": 0.0,
-  "robotFeatures": []
-}
\ No newline at end of file
diff --git a/src/main/java/org/blackknights/Robot.java b/src/main/java/org/blackknights/Robot.java
index 067e411..11826b9 100644
--- a/src/main/java/org/blackknights/Robot.java
+++ b/src/main/java/org/blackknights/Robot.java
@@ -50,7 +50,7 @@ public void disabledExit() {}
 
     @Override
     public void autonomousInit() {
-        autonomousCommand = robotContainer.getAutonomousCommand();
+        autonomousCommand = robotContainer.getAutonomousCommand().get();
 
         if (autonomousCommand != null) {
             autonomousCommand.schedule();
diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java
index c9c5d57..3ede56a 100644
--- a/src/main/java/org/blackknights/RobotContainer.java
+++ b/src/main/java/org/blackknights/RobotContainer.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.*;
@@ -47,9 +48,15 @@ public class RobotContainer {
                     Camera.CameraType.PHOTONVISION,
                     VisionConstants.RIGHT_CAM_TRANSFORM);
 
+    private final Camera centerCam =
+            new Camera(
+                    "centerCam",
+                    Camera.CameraType.PHOTONVISION,
+                    VisionConstants.CENTER_CAM_TRANSFORM);
+
     private final Odometry odometry = Odometry.getInstance();
     // Auto Chooser
-    SendableChooser<Command> superSecretMissileTech = new SendableChooser<>();
+    SendableChooser<Supplier<Command>> superSecretMissileTech = new SendableChooser<>();
 
     // CQ profile selector
     private final SendableChooser<CoralQueue.CoralQueueProfile> cqProfiles =
@@ -68,30 +75,35 @@ public RobotContainer() {
         // Autos
         superSecretMissileTech.addOption(
                 "LEFT_3",
-                new SequentialCommandGroup(
-                        getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")),
-                        getAutoIntakeCommand(IntakeSides.LEFT),
-                        getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")),
-                        getAutoIntakeCommand(IntakeSides.LEFT),
-                        getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4"))));
+                () ->
+                        new SequentialCommandGroup(
+                                getLocationPlaceCommand(
+                                        CoralQueue.CoralPosition.fromString("11L4")),
+                                getAutoIntakeCommand(IntakeSides.LEFT),
+                                getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")),
+                                getAutoIntakeCommand(IntakeSides.LEFT),
+                                getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")),
+                                getAutoIntakeCommand(IntakeSides.LEFT),
+                                getLocationPlaceCommand(
+                                        CoralQueue.CoralPosition.fromString("7L4"))));
 
         superSecretMissileTech.addOption(
                 "RIGHT_3",
-                new SequentialCommandGroup(
-                        getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")),
-                        getAutoIntakeCommand(IntakeSides.RIGHT),
-                        getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("6L4")),
-                        getAutoIntakeCommand(IntakeSides.RIGHT),
-                        getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("7L4"))));
+                () ->
+                        new SequentialCommandGroup(
+                                getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")),
+                                getAutoIntakeCommand(IntakeSides.RIGHT),
+                                getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("5L4")),
+                                getAutoIntakeCommand(IntakeSides.RIGHT),
+                                getLocationPlaceCommand(
+                                        CoralQueue.CoralPosition.fromString("4L4"))));
 
         superSecretMissileTech.addOption(
                 "CENTER_LEFT",
-                getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("12L4")));
+                () -> getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("12L4")));
         superSecretMissileTech.addOption(
                 "CENTER_RIGHT",
-                getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("1L4")));
-
-        superSecretMissileTech.addOption("INTAKE_TEST", getAutoIntakeCommand(IntakeSides.RIGHT));
+                () -> getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("1L4")));
     }
 
     /** Configure controller bindings */
@@ -125,25 +137,64 @@ private void configureBindings() {
         primaryController
                 .rightBumper()
                 .whileTrue(
-                        new ParallelCommandGroup(
-                                new DriveCommands(
-                                        swerveSubsystem,
-                                        primaryController::getLeftY,
-                                        primaryController::getLeftX,
-                                        () -> -primaryController.getRightX() * Math.PI,
-                                        true,
-                                        true),
-                                new ElevatorArmCommand(
-                                        elevatorSubsystem,
-                                        armSubsystem,
-                                        () -> ScoringConstants.ScoringHeights.INTAKE),
-                                new IntakeCommand(
-                                        intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)));
+                        new SequentialCommandGroup(
+                                new ParallelRaceGroup(
+                                        new DriveCommands(
+                                                swerveSubsystem,
+                                                primaryController::getLeftY,
+                                                primaryController::getLeftX,
+                                                () -> -primaryController.getRightX() * Math.PI,
+                                                true,
+                                                true),
+                                        new ElevatorArmCommand(
+                                                elevatorSubsystem,
+                                                armSubsystem,
+                                                () -> ScoringConstants.ScoringHeights.INTAKE),
+                                        new IntakeCommand(
+                                                intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)),
+                                new RunCommand(
+                                                () ->
+                                                        swerveSubsystem.drive(
+                                                                ConfigManager.getInstance()
+                                                                        .get("back_mps", -1.0),
+                                                                0.0,
+                                                                0.0,
+                                                                false,
+                                                                false,
+                                                                false),
+                                                swerveSubsystem)
+                                        .withTimeout(
+                                                ConfigManager.getInstance()
+                                                        .get("back_time_sec", 0.2))));
 
         elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem));
 
         primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro()));
 
+        primaryController
+                .a()
+                .whileTrue(
+                        new RunCommand(
+                                () ->
+                                        elevatorSubsystem.setVoltage(
+                                                ConfigManager.getInstance()
+                                                        .get("elevator_manual_zero", -2.0)),
+                                elevatorSubsystem));
+
+        primaryController
+                .x()
+                .whileTrue(new InstantCommand(() -> elevatorSubsystem.resetEncoders()));
+
+        //        primaryController
+        //                .povUp()
+        //                .whileTrue(
+        //                        new InstantCommand(
+        //                                () ->
+        //                                        elevatorSubsystem.setRightEncoder(
+        //                                                ConfigManager.getInstance()
+        //                                                        .get("break_right_encoder_pos",
+        // -10.0))));
+
         secondaryController
                 .rightStick()
                 .onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions));
@@ -206,6 +257,7 @@ private void configureBindings() {
     public void robotInit() {
         odometry.addCamera(leftCam);
         odometry.addCamera(rightCam);
+        odometry.addCamera(centerCam);
     }
 
     /** Runs every 20ms while the robot is on */
@@ -227,7 +279,7 @@ public void teleopInit() {
      *
      * @return The command to run
      */
-    public Command getAutonomousCommand() {
+    public Supplier<Command> getAutonomousCommand() {
         return superSecretMissileTech.getSelected();
     }
 
@@ -253,6 +305,7 @@ public Command getAutonomousCommand() {
     private Command getPlaceCommand(
             Supplier<CoralQueue.CoralPosition> currentSupplier,
             Supplier<CoralQueue.CoralPosition> nextSupplier) {
+
         return new SequentialCommandGroup(
                 new ParallelRaceGroup(
                         new AlignCommand(
@@ -263,17 +316,36 @@ private Command getPlaceCommand(
                                                 ConfigManager.getInstance()
                                                         .get("align_dist_back", 0.5)),
                                 false,
+                                true,
                                 "rough"),
                         new BaseCommand(elevatorSubsystem, armSubsystem)),
                 new ParallelRaceGroup(
-                        new AlignCommand(
-                                        swerveSubsystem,
-                                        () -> currentSupplier.get().getPose(),
-                                        true,
-                                        "fine")
-                                .withTimeout(
-                                        ConfigManager.getInstance()
-                                                .get("align_fine_max_time", 3.0)),
+                        new SequentialCommandGroup(
+                                new InstantCommand(
+                                        () -> {
+                                            if (currentSupplier.get().getSide()
+                                                    == ScoringConstants.ScoringSides.LEFT) {
+                                                rightCam.setEnabled(true);
+                                                leftCam.setEnabled(false);
+                                            } else {
+                                                leftCam.setEnabled(true);
+                                                rightCam.setEnabled(false);
+                                            }
+                                        }),
+                                new AlignCommand(
+                                                swerveSubsystem,
+                                                () -> currentSupplier.get().getPose(),
+                                                true,
+                                                false,
+                                                "fine")
+                                        .withTimeout(
+                                                ConfigManager.getInstance()
+                                                        .get("align_fine_max_time", 3.0)),
+                                new InstantCommand(
+                                        () -> {
+                                            rightCam.setEnabled(true);
+                                            leftCam.setEnabled(true);
+                                        })),
                         new RunCommand(
                                 () ->
                                         intakeSubsystem.setSpeed(
@@ -289,8 +361,29 @@ private Command getPlaceCommand(
                                 elevatorSubsystem,
                                 armSubsystem,
                                 () -> nextSupplier.get().getHeight()),
-                        new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)
-                                .withTimeout(1)));
+                        new IntakeCommand(
+                                        intakeSubsystem,
+                                        IntakeCommand.IntakeMode.OUTTAKE,
+                                        elevatorSubsystem.isAtTargetSupplier())
+                                .withTimeout(
+                                        ConfigManager.getInstance()
+                                                .get("outtake_max_time_sec", 5.0))),
+                new ParallelRaceGroup(
+                        new AutoEndCommand(),
+                        new BaseCommand(elevatorSubsystem, armSubsystem),
+                        new RunCommand(
+                                        () ->
+                                                swerveSubsystem.drive(
+                                                        ConfigManager.getInstance()
+                                                                .get("back_mps", -1.0),
+                                                        0.0,
+                                                        0.0,
+                                                        false,
+                                                        false,
+                                                        false),
+                                        swerveSubsystem)
+                                .withTimeout(
+                                        ConfigManager.getInstance().get("back_time_sec", 0.2))));
     }
 
     /**
@@ -315,22 +408,19 @@ private Command getAutoIntakeCommand(IntakeSides side) {
                 intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI)));
 
         return new ParallelRaceGroup(
-                new SequentialCommandGroup(
-                        new AlignCommand(
-                                swerveSubsystem,
-                                () ->
-                                        AlignUtils.getXDistBack(
-                                                intakePoseFinal,
-                                                ConfigManager.getInstance()
-                                                        .get("autointake_first_back", 1.0)),
-                                false,
-                                "auto"),
-                        new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "fine")),
-                new ElevatorArmCommand(
-                        elevatorSubsystem,
-                        armSubsystem,
-                        () -> ScoringConstants.ScoringHeights.INTAKE),
-                new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE));
+                        new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE),
+                        new ParallelCommandGroup(
+                                new AlignCommand(
+                                        swerveSubsystem,
+                                        () -> intakePoseFinal,
+                                        true,
+                                        false,
+                                        "rough"),
+                                new ElevatorArmCommand(
+                                        elevatorSubsystem,
+                                        armSubsystem,
+                                        () -> ScoringConstants.ScoringHeights.INTAKE)))
+                .withTimeout(5);
     }
 
     private static Pose2d getPose2d(IntakeSides side) {
@@ -356,4 +446,20 @@ private enum IntakeSides {
         LEFT,
         RIGHT
     }
+
+    private static class AutoEndCommand extends Command {
+        private double currTime = Timer.getFPGATimestamp() * 1000;
+
+        @Override
+        public void initialize() {
+            this.currTime = Timer.getFPGATimestamp() * 1000;
+        }
+
+        @Override
+        public boolean isFinished() {
+            return DriverStation.isAutonomous()
+                    && Timer.getFPGATimestamp() * 1000 - this.currTime
+                            > ConfigManager.getInstance().get("auto_place_backup_time_ms", 0.2);
+        }
+    }
 }
diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java
index 08eb67b..54ad445 100644
--- a/src/main/java/org/blackknights/commands/AlignCommand.java
+++ b/src/main/java/org/blackknights/commands/AlignCommand.java
@@ -1,53 +1,62 @@
 /* Black Knights Robotics (C) 2025 */
 package org.blackknights.commands;
 
-import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import java.util.function.Supplier;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
-import org.blackknights.constants.AlignConstants;
 import org.blackknights.framework.Odometry;
 import org.blackknights.subsystems.SwerveSubsystem;
+import org.blackknights.utils.AlignUtils;
 import org.blackknights.utils.ConfigManager;
 import org.blackknights.utils.NetworkTablesUtils;
 
-/** Align the robot in fieldspace */
+/**
+ * Align the robot in fieldspace Config Manager Keys: <br>
+ * - align_rot_p - Proportional gain for the rotation PID controller.<br>
+ * - align_rot_i - Integral gain for the rotation PID controller.<br>
+ * - align_rot_d - Derivative gain for the rotation PID controller.<br>
+ * - align_rot_max_vel_deg - Maximum rotational velocity (degrees per second).<br>
+ * - align_rot_max_accel_degps - Maximum rotational acceleration (degrees per second squared).<br>
+ * - align_rot_tolerance - Tolerance for considering the rotation PID at goal (degrees).<br>
+ * - align_[profile]_x_max_vel_m - Maximum velocity for X-axis movement (meters per second).<br>
+ * - align_[profile]_x_max_accel_mps - Maximum acceleration for X-axis movement (meters per second
+ * squared).<br>
+ * - align_[profile]_y_max_vel_m - Maximum velocity for Y-axis movement (meters per second).<br>
+ * - align_[profile]_y_max_accel_mps - Maximum acceleration for Y-axis movement (meters per second
+ * squared).<br>
+ * - align_[profile]_rotation_tolerance - Tolerance for rotational alignment (degrees).<br>
+ * - align_[profile]_pos_dist_tol - Position error tolerance before considering the robot aligned
+ * (meters).<br>
+ * - align_[profile]_vel_tol - Velocity tolerance for stopping criteria (meters per second).<br>
+ * - align_[profile]_halfmoon_dist - Distance from the target for the exclusion point(meters).<br>
+ * - align_[profile]_halfmoon_tol - The tolerance for the half moon exclusion point (radius of
+ * circle) (meters).<br>
+ * - align_[profile]_finish_time - Time in milliseconds the robot must stay within tolerances before
+ * stopping.<br>
+ * - align_trap_t_sec - Time step for trapezoidal motion profile calculations (seconds).<br>
+ */
 public class AlignCommand extends Command {
     private static final Logger LOGGER = LogManager.getLogger();
     private final SwerveSubsystem swerveSubsystem;
 
-    //    private final Controller controller;
-
-    private final ProfiledPIDController xAxisPid =
-            new ProfiledPIDController(
-                    AlignConstants.X_AXIS_P,
-                    AlignConstants.X_AXIS_I,
-                    AlignConstants.X_AXIS_D,
-                    AlignConstants.X_AXIS_CONSTRAINTS);
-
-    private final ProfiledPIDController yAxisPid =
-            new ProfiledPIDController(
-                    AlignConstants.Y_AXIS_P,
-                    AlignConstants.Y_AXIS_I,
-                    AlignConstants.Y_AXIS_D,
-                    AlignConstants.Y_AXIS_CONSTRAINTS);
-
-    private final ProfiledPIDController rotationPid =
-            new ProfiledPIDController(
-                    AlignConstants.ROTATION_P,
-                    AlignConstants.ROTATION_I,
-                    AlignConstants.ROTATION_D,
-                    AlignConstants.ROTATION_CONSTRAINTS);
+    private TrapezoidProfile distProfile;
+    private TrapezoidProfile rotationProfile;
+
+    private SimpleMotorFeedforward rotationFF;
 
     private final Odometry odometry = Odometry.getInstance();
     private final ConfigManager configManager = ConfigManager.getInstance();
 
     private final String profile;
     private final boolean stopWhenFinished;
+    private final boolean useHalfMoon;
 
     private final Supplier<Pose2d> pose2dSupplier;
 
@@ -58,6 +67,9 @@ public class AlignCommand extends Command {
     private double timeSenseFinished = -1;
     private boolean doUpdate = true;
 
+    private double distToTarget = Double.MAX_VALUE;
+    private double halfMoonDist = Double.MAX_VALUE;
+
     /**
      * Align to a fieldspace position with odometry
      *
@@ -72,29 +84,18 @@ public class AlignCommand extends Command {
      */
     public AlignCommand(
             SwerveSubsystem swerveSubsystem,
-            //            Controller controller,
             Supplier<Pose2d> poseSupplier,
             boolean stopWhenFinished,
+            boolean useHalfMoon,
             String profile) {
         this.swerveSubsystem = swerveSubsystem;
-        //        this.controller = controller;
         this.pose2dSupplier = poseSupplier;
         this.stopWhenFinished = stopWhenFinished;
+        this.useHalfMoon = useHalfMoon;
         this.profile = profile;
 
         LOGGER.debug("Created new align command with '{}' profile", this.profile);
 
-        this.xAxisPid.setTolerance(
-                configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05));
-        this.yAxisPid.setTolerance(
-                configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05));
-        this.rotationPid.setTolerance(
-                Math.toRadians(
-                        configManager.get(
-                                String.format("align_%s_rotation_tolerance", this.profile), 1)));
-
-        this.rotationPid.enableContinuousInput(-Math.PI, Math.PI);
-
         addRequirements(swerveSubsystem);
     }
 
@@ -103,115 +104,147 @@ public void initialize() {
         this.targetPos = pose2dSupplier.get();
         this.timeSenseFinished = -1;
         this.doUpdate = true;
+        this.distToTarget = Double.MAX_VALUE;
+        this.halfMoonDist = Double.MAX_VALUE;
 
         LOGGER.info("Initializing AlignCommand");
-        Pose3d robotPose = odometry.getRobotPose();
-
-        // PID updates
-        this.xAxisPid.setP(configManager.get("align_x_axis_p", 3));
-        this.yAxisPid.setP(configManager.get("align_y_axis_p", 3));
-        this.rotationPid.setP(configManager.get("align_rot_p", 7.3));
-
-        this.xAxisPid.setD(configManager.get("align_x_axis_d", .25));
-        this.yAxisPid.setD(configManager.get("align_y_axis_d", .25));
-        this.rotationPid.setD(configManager.get("align_rot_d", 0.5));
 
-        this.xAxisPid.setI(configManager.get("align_x_axis_i", 0.0));
-        this.yAxisPid.setI(configManager.get("align_y_axis_i", 0.0));
-        this.rotationPid.setI(configManager.get("align_rot_i", 0.0));
-
-        this.xAxisPid.setConstraints(
-                new TrapezoidProfile.Constraints(
-                        configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0),
-                        configManager.get(
-                                String.format("align_%s_x_max_accel_mps", this.profile), 2.5)));
-        this.yAxisPid.setConstraints(
-                new TrapezoidProfile.Constraints(
-                        configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0),
-                        configManager.get(
-                                String.format("align_%s_y_max_accel_mps", this.profile), 2.5)));
-        this.rotationPid.setConstraints(
-                new TrapezoidProfile.Constraints(
-                        Math.toRadians(
+        this.distProfile =
+                new TrapezoidProfile(
+                        new TrapezoidProfile.Constraints(
                                 configManager.get(
-                                        String.format("align_%s_rot_max_vel_deg", this.profile),
-                                        360)),
-                        Math.toRadians(
+                                        String.format("align_%s_max_vel_m", this.profile), 3.0),
                                 configManager.get(
-                                        String.format("align_%s_rot_max_accel_degps", this.profile),
-                                        360))));
-
-        this.xAxisPid.setTolerance(
-                configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05));
-        this.yAxisPid.setTolerance(
-                configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05));
-        this.rotationPid.setTolerance(
-                Math.toRadians(
-                        configManager.get(
-                                String.format("align_%s_rotation_tolerance", this.profile), 1)));
-
-        this.xAxisPid.reset(
-                robotPose.getX(),
-                swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond);
-        this.yAxisPid.reset(
-                robotPose.getY(),
-                swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond);
-        this.rotationPid.reset(
-                robotPose.getRotation().getZ(),
-                swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond);
-
-        this.xAxisPid.setGoal(robotPose.getX());
-        this.yAxisPid.setGoal(robotPose.getY());
-        this.rotationPid.setGoal(robotPose.getRotation().getZ());
-
-        this.xAxisPid.calculate(robotPose.getX());
-        this.yAxisPid.calculate(robotPose.getY());
-        this.rotationPid.calculate(robotPose.getRotation().getZ());
-
-        this.xAxisPid.setGoal(targetPos.getX());
-        this.yAxisPid.setGoal(targetPos.getY());
-        this.rotationPid.setGoal(targetPos.getRotation().getRadians());
+                                        String.format("align_%s_max_accel_mps", this.profile),
+                                        2.5)));
+
+        this.rotationProfile =
+                new TrapezoidProfile(
+                        new TrapezoidProfile.Constraints(
+                                Math.toRadians(
+                                        configManager.get(
+                                                String.format(
+                                                        "align_%s_rot_max_vel_deg", this.profile),
+                                                360)),
+                                Math.toRadians(
+                                        configManager.get(
+                                                String.format(
+                                                        "align_%s_rot_max_accel_degps",
+                                                        this.profile),
+                                                360))));
+
+        this.rotationFF =
+                new SimpleMotorFeedforward(
+                        configManager.get("align_rotation_ff_ks", 0.01622),
+                        configManager.get("align_rotation_ff_kv", 0.0),
+                        0.0,
+                        1);
     }
 
     @Override
     public void execute() {
-        // Set PIDs to target
-
         Pose3d robotPose = odometry.getRobotPose();
-
-        double xAxisCalc = this.xAxisPid.calculate(robotPose.getX());
-        double yAxisCalc = this.yAxisPid.calculate(robotPose.getY());
-        double rotationPidCalc = this.rotationPid.calculate(robotPose.getRotation().getZ());
-
-        debug.setEntry("X Pid Error", this.xAxisPid.getPositionError());
-        debug.setEntry("Y Pid Error", this.yAxisPid.getPositionError());
-        debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError());
-
-        debug.setEntry("X Pid setpoint", this.xAxisPid.atSetpoint());
-        debug.setEntry("X Pid goal", this.xAxisPid.atGoal());
-
-        debug.setEntry("y Pid setpoint", this.xAxisPid.atSetpoint());
-        debug.setEntry("Y Pid goal", this.yAxisPid.atGoal());
-
-        debug.setEntry("Rot Pid setpoint", this.rotationPid.atSetpoint());
-        debug.setEntry("Rot Pid goal", this.rotationPid.atGoal());
-        debug.setEntry("Robot rotation: ", robotPose.getRotation().getZ());
-        debug.setEntry("Rot setpoint", this.rotationPid.getSetpoint().position);
-
-        double finalX =
-                xAxisCalc
-                        + (xAxisPid.atGoal() || xAxisPid.atSetpoint()
-                                ? 0
-                                : Math.signum(xAxisCalc) * configManager.get("align_ff", 0.1));
-        double finalY =
-                yAxisCalc
-                        + (yAxisPid.atGoal() || yAxisPid.atSetpoint()
-                                ? 0
-                                : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1));
-
-        debug.setEntry("Xms", finalX);
-        debug.setEntry("Yms", finalY);
-        debug.setEntry("Rrads", rotationPidCalc);
+        double d_x = this.targetPos.getX() - robotPose.getX();
+        double d_y = this.targetPos.getY() - robotPose.getY();
+
+        this.distToTarget = Math.sqrt(Math.pow(d_x, 2) + Math.pow(d_y, 2));
+
+        Pose2d halfMoonClosePose =
+                AlignUtils.getXDistBack(
+                        this.targetPos,
+                        -configManager.get(
+                                String.format("align_%s_halfmoon_dist", this.profile), 0.5));
+        this.halfMoonDist =
+                Math.sqrt(
+                        Math.pow(robotPose.getX() - halfMoonClosePose.getX(), 2)
+                                + Math.pow(robotPose.getY() - halfMoonClosePose.getY(), 2));
+
+        double trapCalc =
+                -this.distProfile.calculate(
+                                configManager.get("align_trap_t_sec", 0.2),
+                                new TrapezoidProfile.State(
+                                        distToTarget,
+                                        -Math.sqrt(
+                                                Math.pow(
+                                                                swerveSubsystem
+                                                                        .getFieldRelativeChassisSpeeds()
+                                                                        .vxMetersPerSecond,
+                                                                2.0)
+                                                        + Math.pow(
+                                                                swerveSubsystem
+                                                                        .getFieldRelativeChassisSpeeds()
+                                                                        .vyMetersPerSecond,
+                                                                2.0))),
+                                new TrapezoidProfile.State(
+                                        0.0,
+                                        DriverStation.isAutonomous()
+                                                ? configManager.get(
+                                                        String.format(
+                                                                "align_%s_auto_ending_vel_mag",
+                                                                this.profile),
+                                                        0.0)
+                                                : configManager.get(
+                                                        String.format(
+                                                                "align_%s_ending_vel_mag",
+                                                                this.profile),
+                                                        1.0)))
+                        .velocity;
+
+        double a = Math.atan2(d_y, d_x);
+
+        debug.setEntry("Align/Trap Calc", trapCalc);
+        debug.setEntry("Align/Angle", Math.toDegrees(a));
+        debug.setEntry(
+                "Align/Robot Vel",
+                Math.sqrt(
+                        Math.pow(
+                                        swerveSubsystem.getFieldRelativeChassisSpeeds()
+                                                .vxMetersPerSecond,
+                                        2.0)
+                                + Math.pow(
+                                        swerveSubsystem.getFieldRelativeChassisSpeeds()
+                                                .vyMetersPerSecond,
+                                        2.0)));
+
+        double xAxisCalc = trapCalc * Math.cos(a);
+        double yAxisCalc = trapCalc * Math.sin(a);
+
+        double errorBound = Math.PI;
+        double goal =
+                MathUtil.inputModulus(
+                        targetPos.getRotation().getRadians() - robotPose.getRotation().getZ(),
+                        -errorBound,
+                        errorBound);
+
+        double rotCalc =
+                this.rotationProfile.calculate(
+                                0.1,
+                                new TrapezoidProfile.State(
+                                        robotPose.getRotation().getZ(),
+                                        swerveSubsystem.getFieldRelativeChassisSpeeds()
+                                                .omegaRadiansPerSecond),
+                                new TrapezoidProfile.State(
+                                        goal + robotPose.getRotation().getZ(), 0.0))
+                        .velocity;
+
+        rotCalc += this.rotationFF.calculate(rotCalc);
+
+        debug.setEntry("Dist to target (Error)", distToTarget);
+
+        debug.setEntry("X Error", Math.abs(d_x));
+        debug.setEntry("Y Error", Math.abs(d_y));
+
+        debug.setEntry("Align/Total time", this.distProfile.totalTime());
+
+        debug.setEntry(
+                "Rot diff",
+                Math.abs(
+                        Math.abs(this.targetPos.getRotation().getRadians())
+                                - Math.abs(odometry.getRobotPose().getRotation().getZ())));
+
+        debug.setEntry("Xms", xAxisCalc);
+        debug.setEntry("Yms", yAxisCalc);
+        debug.setEntry("Rrads", rotCalc);
 
         this.debug.setArrayEntry(
                 "target_pose",
@@ -221,20 +254,18 @@ public void execute() {
                     this.targetPos.getRotation().getRadians()
                 });
 
-        //        if (controller.hasStickInput(0.03) || !odometry.hasSeenTarget()) {
-        //            swerveSubsystem.drive(
-        //                    controller.getLeftX(),
-        //                    controller.getLeftY(),
-        //                    controller.getRightX(),
-        //                    true,
-        //                    true,
-        //                    false);
-        //        } else {
-        swerveSubsystem.drive(finalX, finalY, rotationPidCalc, true, true, true);
-        //        }
-
-        if (xAxisPid.atGoal() && yAxisPid.atGoal() && rotationPid.atGoal() && this.doUpdate) {
-            LOGGER.debug("Hit goal, waiting for time to expire");
+        if (Math.abs(xAxisCalc)
+                        < configManager.get(String.format("align_%s_min_vel", this.profile), 0.002)
+                && Math.abs(yAxisCalc)
+                        < configManager.get(
+                                String.format("align_%s_min_vel", this.profile), 0.002)) {
+            swerveSubsystem.zeroVoltage();
+        } else {
+            swerveSubsystem.drive(xAxisCalc, yAxisCalc, rotCalc, true, false, true);
+        }
+
+        if (checkAtGoal() && doUpdate) {
+            LOGGER.info("Hit goal, waiting for time to expire");
             this.timeSenseFinished = Timer.getFPGATimestamp() * 1000;
             this.doUpdate = false;
         }
@@ -242,9 +273,7 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        return xAxisPid.atGoal()
-                && yAxisPid.atGoal()
-                && rotationPid.atGoal()
+        return checkAtGoal()
                 && Timer.getFPGATimestamp() * 1000 - this.timeSenseFinished
                         > configManager.get(
                                 String.format("align_%s_finish_time", this.profile), 200.0);
@@ -254,4 +283,96 @@ public boolean isFinished() {
     public void end(boolean interrupted) {
         if (stopWhenFinished) swerveSubsystem.zeroVoltage();
     }
+
+    private boolean checkAtGoal() {
+        debug.setEntry(
+                "Align/Dist Check",
+                distToTarget
+                        <= configManager.get(
+                                String.format("align_%s_pos_dist_tol", this.profile), 0.0));
+
+        debug.setEntry(
+                "Align/Half moon check",
+                (!useHalfMoon
+                        || halfMoonDist
+                                >= configManager.get(
+                                        String.format("align_%s_halfmoon_tol", this.profile),
+                                        0.0)));
+
+        debug.setEntry(
+                "Align/Rotation check",
+                Math.abs(
+                                Math.abs(Odometry.getInstance().getRobotPose().getRotation().getZ())
+                                        - Math.abs(targetPos.getRotation().getRadians()))
+                        <= Math.toRadians(
+                                ConfigManager.getInstance()
+                                        .get(
+                                                String.format("align_%s_rot_tol_deg", this.profile),
+                                                1.0)));
+        debug.setEntry(
+                "The value",
+                Math.abs(
+                        Math.abs(Odometry.getInstance().getRobotPose().getRotation().getZ())
+                                - Math.abs(targetPos.getRotation().getRadians())));
+
+        debug.setEntry(
+                "The value current",
+                Math.abs(Odometry.getInstance().getRobotPose().getRotation().getZ()));
+
+        debug.setEntry("The value target", Math.abs(targetPos.getRotation().getRadians()));
+
+        debug.setEntry(
+                "Align/X Vel Check",
+                MathUtil.isNear(
+                        configManager.get(
+                                String.format("align_%s_x_target_end_vel", this.profile), 0.0),
+                        swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond,
+                        configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0)));
+
+        debug.setEntry(
+                "Align/Y Vel Check",
+                MathUtil.isNear(
+                        configManager.get(
+                                String.format("align_%s_y_target_end_vel", this.profile), 0.0),
+                        swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond,
+                        configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0)));
+
+        return distToTarget
+                        <= configManager.get(
+                                String.format("align_%s_pos_dist_tol", this.profile), 0.0)
+                && Math.abs(
+                                Math.abs(Odometry.getInstance().getRobotPose().getRotation().getZ())
+                                        - Math.abs(
+                                                targetPos.getRotation().getRadians() > Math.PI
+                                                        ? targetPos.getRotation().getRadians()
+                                                                - Math.PI * 2
+                                                        : targetPos.getRotation().getRadians()))
+                        <= Math.toRadians(
+                                ConfigManager.getInstance()
+                                        .get(
+                                                String.format("align_%s_rot_tol_deg", this.profile),
+                                                1.0))
+                && (!useHalfMoon
+                        || halfMoonDist
+                                >= configManager.get(
+                                        String.format("align_%s_halfmoon_tol", this.profile), 0.0))
+                && (!stopWhenFinished
+                        || MathUtil.isNear(
+                                configManager.get(
+                                        String.format("align_%s_ending_vel_mag", this.profile),
+                                        1.0),
+                                Math.sqrt(
+                                        Math.pow(
+                                                        swerveSubsystem
+                                                                .getFieldRelativeChassisSpeeds()
+                                                                .vxMetersPerSecond,
+                                                        2)
+                                                + Math.pow(
+                                                        swerveSubsystem
+                                                                .getFieldRelativeChassisSpeeds()
+                                                                .vxMetersPerSecond,
+                                                        2)),
+                                configManager.get(
+                                        String.format("align_%s_vel_tol", this.profile), 0.0)));
+    }
 }
diff --git a/src/main/java/org/blackknights/commands/BaseCommand.java b/src/main/java/org/blackknights/commands/BaseCommand.java
index 6c6c191..b87985e 100644
--- a/src/main/java/org/blackknights/commands/BaseCommand.java
+++ b/src/main/java/org/blackknights/commands/BaseCommand.java
@@ -32,7 +32,9 @@ public void initialize() {
     @Override
     public void execute() {
         armSubsystem.setPivotAngle(ConfigManager.getInstance().get("arm_base_angle", 0.1));
-        if (armSubsystem.getPivotAngle() <= -Math.PI / 4 || armSubsystem.getPivotAngle() >= 0.2) {
+        if (armSubsystem.getPivotAngle() <= -Math.PI / 4
+                || armSubsystem.getPivotAngle()
+                        >= ConfigManager.getInstance().get("arm_movement_max", 0.2)) {
             elevatorSubsystem.holdPosition();
         } else {
             elevatorSubsystem.zeroElevator();
diff --git a/src/main/java/org/blackknights/commands/DriveCommands.java b/src/main/java/org/blackknights/commands/DriveCommands.java
index 4dbbf84..4b00e6d 100644
--- a/src/main/java/org/blackknights/commands/DriveCommands.java
+++ b/src/main/java/org/blackknights/commands/DriveCommands.java
@@ -6,6 +6,7 @@
 import java.util.function.DoubleSupplier;
 import org.blackknights.subsystems.SwerveSubsystem;
 import org.blackknights.utils.ConfigManager;
+import org.blackknights.utils.NetworkTablesUtils;
 
 /** Command to drive swerve */
 public class DriveCommands extends Command {
@@ -56,12 +57,18 @@ public void execute() {
                         radians.getAsDouble(),
                         ConfigManager.getInstance().get("controller_deadband", 0.06));
 
+        NetworkTablesUtils debug = NetworkTablesUtils.getTable("debug");
+
+        debug.setEntry("Forward desired", forwardDesired);
+        debug.setEntry("Sideways desired", sidewaysDesired);
+        debug.setEntry("Rotation desired", radiansDesired);
+
         swerveSubsystem.drive(
                 forwardDesired,
                 sidewaysDesired,
                 radiansDesired,
                 fieldRelativeFromButton,
-                true,
+                false,
                 false);
     }
 
diff --git a/src/main/java/org/blackknights/commands/IntakeCommand.java b/src/main/java/org/blackknights/commands/IntakeCommand.java
index d9114a5..fd72bc0 100644
--- a/src/main/java/org/blackknights/commands/IntakeCommand.java
+++ b/src/main/java/org/blackknights/commands/IntakeCommand.java
@@ -1,14 +1,20 @@
 /* Black Knights Robotics (C) 2025 */
 package org.blackknights.commands;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import java.util.function.BooleanSupplier;
 import org.blackknights.subsystems.IntakeSubsystem;
 import org.blackknights.utils.ConfigManager;
+import org.blackknights.utils.NetworkTablesUtils;
 
 /** Command to intake and outtake */
 public class IntakeCommand extends Command {
     private final IntakeSubsystem intakeSubsystem;
     private final IntakeMode mode;
+    private final BooleanSupplier elevatorAtTargetSupplier;
+
+    private double elevatorAtTargetTime;
 
     /**
      * Create a new intake command
@@ -17,13 +23,38 @@ public class IntakeCommand extends Command {
      * @param mode The intake mode ({@link IntakeMode})
      */
     public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) {
+        this.elevatorAtTargetSupplier = () -> true;
         this.intakeSubsystem = intakeSubsystem;
         this.mode = mode;
         addRequirements(intakeSubsystem);
     }
 
+    /**
+     * Create a new intake command
+     *
+     * @param intakeSubsystem The instance of {@link IntakeSubsystem}
+     * @param mode The intake mode ({@link IntakeMode})
+     */
+    public IntakeCommand(
+            IntakeSubsystem intakeSubsystem,
+            IntakeMode mode,
+            BooleanSupplier elevatorAtTargetSupplier) {
+        this.elevatorAtTargetSupplier = elevatorAtTargetSupplier;
+        this.intakeSubsystem = intakeSubsystem;
+        this.mode = mode;
+        addRequirements(intakeSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        this.elevatorAtTargetTime = 0;
+    }
+
     @Override
     public void execute() {
+        if (this.elevatorAtTargetTime == 0 && elevatorAtTargetSupplier.getAsBoolean()) {
+            this.elevatorAtTargetTime = Timer.getFPGATimestamp() * 1000;
+        }
         switch (mode) {
             case INTAKE:
                 {
@@ -33,8 +64,18 @@ public void execute() {
                 }
             case OUTTAKE:
                 {
-                    intakeSubsystem.setVoltage(
-                            ConfigManager.getInstance().get("outtake_speed", -8.0));
+                    NetworkTablesUtils.getTable("debug/IntakeCmd")
+                            .setEntry(
+                                    "Time running",
+                                    Timer.getFPGATimestamp() * 1000 - this.elevatorAtTargetTime);
+
+                    if (Timer.getFPGATimestamp() * 1000 - this.elevatorAtTargetTime
+                                    > ConfigManager.getInstance().get("outtake_wait_time_ms", 250.0)
+                            && elevatorAtTargetSupplier.getAsBoolean()) {
+
+                        intakeSubsystem.setVoltage(
+                                ConfigManager.getInstance().get("outtake_speed", -8.0));
+                    }
                     break;
                 }
         }
@@ -47,7 +88,13 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak();
+        return (mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak())
+                || (mode.equals(IntakeMode.OUTTAKE)
+                        && !intakeSubsystem.getLinebreak()
+                        && Timer.getFPGATimestamp() * 1000 - this.elevatorAtTargetTime
+                                > (ConfigManager.getInstance().get("outtaking_time_ms", 200)
+                                        + ConfigManager.getInstance()
+                                                .get("outtake_wait_time_ms", 250)));
     }
 
     /** Enum of the different intake modes */
diff --git a/src/main/java/org/blackknights/constants/DrivetrainConstants.java b/src/main/java/org/blackknights/constants/DrivetrainConstants.java
index c5cfefe..40f6380 100644
--- a/src/main/java/org/blackknights/constants/DrivetrainConstants.java
+++ b/src/main/java/org/blackknights/constants/DrivetrainConstants.java
@@ -55,7 +55,7 @@ public class DrivetrainConstants {
     // The MAXSwerve module can be configured with one of three pinion gears: 12T,
     // 13T, or 14T. This changes the drive speed of the module (a pinion gear with
     // more teeth will result in a robot that drives faster).
-    public static final int DRIVING_MOTOR_PINION_TEETH = 14;
+    public static final int DRIVING_MOTOR_PINION_TEETH = 13;
     public static final int BEVEL_GEAR_TEETH = 45;
     public static final int FIRST_STAGE_SPUR_GEAR_TEETH = 22;
     public static final int BEVEL_PINION_TEETH = 15;
diff --git a/src/main/java/org/blackknights/constants/ScoringConstants.java b/src/main/java/org/blackknights/constants/ScoringConstants.java
index c03bbbe..d526ce1 100644
--- a/src/main/java/org/blackknights/constants/ScoringConstants.java
+++ b/src/main/java/org/blackknights/constants/ScoringConstants.java
@@ -98,6 +98,11 @@ public enum ScoringHeights {
         INTAKE,
     }
 
+    public enum ScoringSides {
+        LEFT,
+        RIGHT
+    }
+
     /**
      * Get a scoring position from an april tag id
      *
diff --git a/src/main/java/org/blackknights/constants/VisionConstants.java b/src/main/java/org/blackknights/constants/VisionConstants.java
index 4e94d5c..a66cf18 100644
--- a/src/main/java/org/blackknights/constants/VisionConstants.java
+++ b/src/main/java/org/blackknights/constants/VisionConstants.java
@@ -3,6 +3,7 @@
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import org.blackknights.utils.ConfigManager;
 
 /** Vision related constants */
 public class VisionConstants {
@@ -10,8 +11,34 @@ public class VisionConstants {
     public static double VISION_TRUST = 0.5;
 
     public static final Transform3d LEFT_CAM_TRANSFORM =
-            new Transform3d(0.235, 0.36, 0.16, new Rotation3d(0.0, 0.0, Math.toRadians(-13)));
-
+            new Transform3d(
+                    ConfigManager.getInstance().get("left_cam_x", 0.253),
+                    0.336,
+                    0.229,
+                    new Rotation3d(
+                            0.0,
+                            0.0,
+                            Math.toRadians(
+                                    ConfigManager.getInstance().get("left_cam_angle", -10.0))));
     public static final Transform3d RIGHT_CAM_TRANSFORM =
-            new Transform3d(0.235, -0.36, 0.16, new Rotation3d(0.0, 0.0, Math.toRadians(10)));
+            new Transform3d(
+                    ConfigManager.getInstance().get("right_cam_x", .253),
+                    -0.3995,
+                    0.229,
+                    new Rotation3d(
+                            0.0,
+                            0.0,
+                            Math.toRadians(
+                                    ConfigManager.getInstance().get("right_cam_angle", 10.0))));
+
+    public static final Transform3d CENTER_CAM_TRANSFORM =
+            new Transform3d(
+                    0.1, //  0.341122 0.3832
+                    0.0,
+                    0.2040382,
+                    new Rotation3d(
+                            0.0,
+                            Math.toRadians(
+                                    ConfigManager.getInstance().get("center_cam_pitch", 45.0)),
+                            0.0));
 }
diff --git a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java
index 7ede542..29f7aa0 100644
--- a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java
+++ b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java
@@ -3,6 +3,7 @@
 
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
@@ -12,15 +13,21 @@
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkFlexConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import org.blackknights.utils.ConfigManager;
+import org.blackknights.utils.NetworkTablesUtils;
 
 /** A wrapper class for swerve modules */
 public class MAXSwerveModule {
     private final SparkFlex drivingSpark;
     private final SparkMax turningSpark;
 
+    private final int drivingCanId;
+
     private final RelativeEncoder drivingEncoder;
     private final AbsoluteEncoder turningEncoder;
 
@@ -30,6 +37,9 @@ public class MAXSwerveModule {
     private final double chassisAngularOffset;
     private SwerveModuleState desiredState = new SwerveModuleState(0.0, new Rotation2d());
 
+    private SimpleMotorFeedforward feedforward =
+            new SimpleMotorFeedforward(0.096286, 2.3216, 0.41854, 1);
+
     /**
      * Constructs a MAXSwerveModule and configures the driving and turning motor, encoder, and PID
      * controller. This configuration is specific to the REV MAXSwerve Module built with NEOs,
@@ -38,6 +48,7 @@ public class MAXSwerveModule {
     public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
         drivingSpark = new SparkFlex(drivingCANId, MotorType.kBrushless);
         turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
+        this.drivingCanId = drivingCANId;
 
         drivingEncoder = drivingSpark.getEncoder();
         turningEncoder = turningSpark.getAbsoluteEncoder();
@@ -103,9 +114,29 @@ public void setDesiredState(SwerveModuleState desiredState) {
         // Optimize the reference state to avoid spinning further than 90 degrees.
         correctedDesiredState.optimize(new Rotation2d(turningEncoder.getPosition()));
 
-        // Command driving and turning SPARKS towards their respective setpoints.
+        double ffOutput =
+                feedforward.calculateWithVelocities(
+                        drivingEncoder.getVelocity(), correctedDesiredState.speedMetersPerSecond);
+
+        NetworkTablesUtils.getTable("debug")
+                .setEntry(String.format("ID(%s) - Swerve FF Output", drivingCanId), ffOutput);
+        NetworkTablesUtils.getTable("debug")
+                .setEntry(
+                        String.format("ID(%s) - Swerve target mps", drivingCanId),
+                        correctedDesiredState.speedMetersPerSecond);
+
+        // Command driving and turning SPARKS towards their respective endpoints.
         drivingClosedLoopController.setReference(
-                correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
+                correctedDesiredState.speedMetersPerSecond,
+                ControlType.kVelocity,
+                ClosedLoopSlot.kSlot0,
+                MathUtil.isNear(
+                                0.0,
+                                correctedDesiredState.speedMetersPerSecond,
+                                ConfigManager.getInstance().get("swerve_min_velocity", 0.01))
+                        ? 0.0
+                        : ffOutput);
+
         turningClosedLoopController.setReference(
                 correctedDesiredState.angle.getRadians(), ControlType.kPosition);
 
diff --git a/src/main/java/org/blackknights/controllers/MAXSwerveModuleConfig.java b/src/main/java/org/blackknights/controllers/MAXSwerveModuleConfig.java
index bda4e55..ba71863 100644
--- a/src/main/java/org/blackknights/controllers/MAXSwerveModuleConfig.java
+++ b/src/main/java/org/blackknights/controllers/MAXSwerveModuleConfig.java
@@ -19,7 +19,6 @@ public class MAXSwerveModuleConfig {
                         * Math.PI
                         / DrivetrainConstants.DRIVING_MOTOR_REDUCTION;
         double turningFactor = 2 * Math.PI;
-        double drivingVelocityFeedForward = 1 / DrivetrainConstants.DRIVE_WHEEL_FREE_SPEED_RPS;
 
         drivingConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(50);
         drivingConfig
@@ -30,8 +29,8 @@ public class MAXSwerveModuleConfig {
                 .closedLoop
                 .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
                 // These are example gains you may need to them for your own robot!
-                .pid(0.04, 0, 0)
-                .velocityFF(drivingVelocityFeedForward)
+                .pid(0.11185, 0, 0)
+                .velocityFF(0.0)
                 .outputRange(-1, 1);
 
         turningConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(20);
diff --git a/src/main/java/org/blackknights/framework/CoralQueue.java b/src/main/java/org/blackknights/framework/CoralQueue.java
index 4c34ea8..a7e8257 100644
--- a/src/main/java/org/blackknights/framework/CoralQueue.java
+++ b/src/main/java/org/blackknights/framework/CoralQueue.java
@@ -145,6 +145,7 @@ public static class CoralPosition {
         private final String stringId;
         private final Pose2d pose;
         private final ScoringConstants.ScoringHeights height;
+        private final ScoringConstants.ScoringSides side;
 
         /**
          * Create a new coral position
@@ -153,10 +154,15 @@ public static class CoralPosition {
          * @param pose A {@link Pose2d} for the soring position
          * @param height The target height
          */
-        public CoralPosition(String stringId, Pose2d pose, ScoringConstants.ScoringHeights height) {
+        public CoralPosition(
+                String stringId,
+                Pose2d pose,
+                ScoringConstants.ScoringHeights height,
+                ScoringConstants.ScoringSides side) {
             this.stringId = stringId;
             this.pose = pose;
             this.height = height;
+            this.side = side;
         }
 
         /** Create an empty coral position */
@@ -164,6 +170,7 @@ public CoralPosition() {
             this.stringId = "";
             this.pose = new Pose2d();
             this.height = ScoringConstants.ScoringHeights.L1;
+            this.side = ScoringConstants.ScoringSides.RIGHT;
         }
 
         public static CoralPosition fromString(String string) {
@@ -184,7 +191,14 @@ public static CoralPosition fromString(String string) {
                 posIdx += 12;
             }
 
-            LOGGER.debug("CoralQ: posIdx = {}, heightString = {}", posIdx, heightString);
+            LOGGER.info(
+                    "CoralP: name = {}, posIdx = {}, heightString = {}, side = {}",
+                    string,
+                    posIdx,
+                    heightString,
+                    posIdx % 2 == 0
+                            ? ScoringConstants.ScoringSides.RIGHT
+                            : ScoringConstants.ScoringSides.LEFT);
 
             return new CoralPosition(
                     string,
@@ -192,7 +206,10 @@ public static CoralPosition fromString(String string) {
                             ScoringConstants.CORAL_POSITIONS[posIdx].getX(),
                             ScoringConstants.CORAL_POSITIONS[posIdx].getY(),
                             ScoringConstants.CORAL_POSITIONS[posIdx].getRotation()),
-                    ScoringConstants.ScoringHeights.valueOf(heightString.toUpperCase()));
+                    ScoringConstants.ScoringHeights.valueOf(heightString.toUpperCase()),
+                    posIdx % 2 == 0
+                            ? ScoringConstants.ScoringSides.RIGHT
+                            : ScoringConstants.ScoringSides.LEFT);
         }
 
         /**
@@ -213,6 +230,10 @@ public ScoringConstants.ScoringHeights getHeight() {
             return this.height;
         }
 
+        public ScoringConstants.ScoringSides getSide() {
+            return this.side;
+        }
+
         /**
          * Return the pose as a double array
          *
@@ -234,8 +255,8 @@ public boolean[] getBooleanHeights() {
         @Override
         public String toString() {
             return String.format(
-                    "CoralPosition(stringId: %s, pose: %s, height: %s)",
-                    this.stringId, this.pose.toString(), this.height);
+                    "CoralPosition(stringId: %s, pose: %s, height: %s, side: %s)",
+                    this.stringId, this.pose.toString(), this.height, this.side);
         }
 
         @Override
diff --git a/src/main/java/org/blackknights/framework/Odometry.java b/src/main/java/org/blackknights/framework/Odometry.java
index 4a3c5d9..79782b9 100644
--- a/src/main/java/org/blackknights/framework/Odometry.java
+++ b/src/main/java/org/blackknights/framework/Odometry.java
@@ -7,7 +7,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import java.util.ArrayList;
+import java.util.HashMap;
 import java.util.Optional;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
@@ -20,7 +20,7 @@
 /** System for all odometry related stuff */
 public class Odometry {
     /** List of cameras used for vision-based measurements to refine odometry. */
-    private ArrayList<Camera> cameras = new ArrayList<>();
+    private HashMap<String, Camera> cameras = new HashMap<>();
 
     /** Singleton instance of the OdometrySubsystem. */
     private static Odometry INSTANCE = null;
@@ -29,7 +29,7 @@ public class Odometry {
     private static final Logger LOGGER = LogManager.getLogger();
 
     private final NetworkTablesUtils NTTelemetry = NetworkTablesUtils.getTable("Telemetry");
-    private final NetworkTablesUtils debug = NetworkTablesUtils.getTable("debug");
+    private final NetworkTablesUtils debug = NetworkTablesUtils.getTable("debug/Odometry");
 
     private Optional<Pose3d> targetPose = Optional.of(new Pose3d());
 
@@ -92,7 +92,7 @@ public static Odometry getInstance() {
      * @param camera The {@link Camera} to add to the system.
      */
     public void addCamera(Camera camera) {
-        this.cameras.add(camera);
+        this.cameras.put(camera.getName(), camera);
     }
 
     public SwerveDrivePoseEstimator3d getPoseEstimator() {
@@ -106,11 +106,11 @@ public SwerveDrivePoseEstimator3d getPoseEstimator() {
      * @return Either an empty optional if the camera does not exist, or the camera
      */
     public Optional<Camera> getCamera(String name) {
-        for (Camera c : this.cameras) {
-            if (c.getName().equalsIgnoreCase(name)) return Optional.of(c);
+        if (!this.cameras.containsKey(name)) {
+            return Optional.empty();
         }
 
-        return Optional.empty();
+        return Optional.of(this.cameras.get(name));
     }
 
     /**
@@ -172,18 +172,25 @@ public void periodic() {
                     this.getRobotPose().getY(),
                     this.getRobotPose().getRotation().getZ()
                 });
-        for (Camera c : this.cameras) {
+        for (Camera c : this.cameras.values()) {
             Optional<Pose3d> pose = c.getPoseFieldSpace(this.getRobotPose());
-            if (pose.isPresent()) {
+            debug.setEntry(String.format("%s/enabled", c.getName()), c.isEnabled());
+            if (pose.isPresent() && c.isEnabled()) {
                 double dist =
                         Math.sqrt(
                                 Math.pow(c.getTargetPose().getX(), 2)
                                         + Math.pow(c.getTargetPose().getY(), 2));
 
-                debug.setEntry(String.format("%s_dist_to_target", c.getName()), dist);
+                debug.setEntry(String.format("%s/dist_to_target", c.getName()), dist);
+                debug.setArrayEntry(
+                        String.format("%s/pose", c.getName()),
+                        new double[] {
+                            pose.get().getX(), pose.get().getY(), pose.get().getRotation().getZ()
+                        });
 
                 if (dist <= ConfigManager.getInstance().get("vision_cutoff_distance", 3)
                         && dist > ConfigManager.getInstance().get("vision_min_distance", 0.5)) {
+                    debug.setEntry(String.format("%s/Adding target", c.getName()), true);
 
                     this.hasSeenTarget = true;
                     LOGGER.debug("Added vision measurement from `{}`", c.getName());
@@ -195,6 +202,8 @@ public void periodic() {
                                             c.getTargetPose().getZ(),
                                             c.getTargetPose().getRotation()));
                     this.poseEstimator.addVisionMeasurement(pose.get(), c.getTimestamp());
+                } else {
+                    debug.setEntry(String.format("%s/Adding target", c.getName()), false);
                 }
             } else {
                 this.targetPose = Optional.empty();
diff --git a/src/main/java/org/blackknights/subsystems/ArmSubsystem.java b/src/main/java/org/blackknights/subsystems/ArmSubsystem.java
index 51db177..57029bd 100644
--- a/src/main/java/org/blackknights/subsystems/ArmSubsystem.java
+++ b/src/main/java/org/blackknights/subsystems/ArmSubsystem.java
@@ -69,6 +69,8 @@ public ArmSubsystem() {
                 pivotConfig,
                 SparkBase.ResetMode.kResetSafeParameters,
                 SparkBase.PersistMode.kPersistParameters);
+
+        pivotPID.enableContinuousInput(-Math.PI, Math.PI);
     }
 
     /**
@@ -107,10 +109,15 @@ public double getPivotAngle() {
         //                ? pivotAbsEncoder.getPosition() - 2 * Math.PI -
         // ArmConstants.PIVOT_ENCODER_OFFSET
         //                : pivotAbsEncoder.getPosition() - ArmConstants.PIVOT_ENCODER_OFFSET;
-        return Math.PI * 2
-                - pivotAbsEncoder.getPosition()
-                - ConfigManager.getInstance()
-                        .get("arm_encoder_offset", ArmConstants.PIVOT_ENCODER_OFFSET);
+        double x =
+                Math.PI * 2
+                        - pivotAbsEncoder.getPosition()
+                        - ConfigManager.getInstance()
+                                .get("arm_encoder_offset", ArmConstants.PIVOT_ENCODER_OFFSET);
+
+        if (x >= Math.PI) x -= Math.PI * 2;
+
+        return x;
     }
 
     public double getPivotSpeed() {
diff --git a/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java b/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java
index c36c84a..43a5be6 100644
--- a/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java
+++ b/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java
@@ -20,6 +20,7 @@ public class ButtonBoardSubsystem extends SubsystemBase {
 
     private Pose2d currentPose = null;
     private ScoringConstants.ScoringHeights currentHeight = null;
+    private ScoringConstants.ScoringSides currentSide = null;
 
     /**
      * Create a new Instance of the subsystem for the button board
@@ -40,9 +41,13 @@ public void periodic() {
         if (this.currentHeight != null && this.currentPose != null) {
             coralQueue.interruptQueue(
                     new CoralQueue.CoralPosition(
-                            "BBInterrupt", this.currentPose, this.currentHeight));
+                            "BBInterrupt",
+                            this.currentPose,
+                            this.currentHeight,
+                            this.currentSide = null));
             this.currentPose = null;
             this.currentHeight = null;
+            this.currentSide = null;
         }
     }
 
@@ -75,6 +80,10 @@ public void bind() {
                                                 String.format("L%d", finalB - 12));
                             } else {
                                 int arrId = finalB + (isBlue ? 12 : 0) - 1;
+                                this.currentSide =
+                                        arrId % 2 == 0
+                                                ? ScoringConstants.ScoringSides.RIGHT
+                                                : ScoringConstants.ScoringSides.LEFT;
                                 this.currentPose = ScoringConstants.CORAL_POSITIONS[arrId];
                             }
                         }
diff --git a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java
index 553a444..06a30ed 100644
--- a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.networktables.DoubleEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.function.BooleanSupplier;
 import org.blackknights.constants.ElevatorConstants;
 import org.blackknights.utils.ConfigManager;
 
@@ -146,7 +147,7 @@ public void setTargetPosition(double position) {
         elevatorPID.setGoal(position);
 
         double pidCalc = elevatorPID.calculate(getElevatorPosition());
-        double ffCalc = elevatorFF.calculate(0.0);
+        double ffCalc = elevatorFF.calculate(elevatorPID.getSetpoint().velocity);
 
         //        if (Math.abs(this.getLeftEncoderPosition() - this.getRightEncoderPosition())
         //                > ConfigManager.getInstance().get("max_roation_diff", 1)) {
@@ -178,6 +179,19 @@ public double getRightEncoderPosition() {
         return rightEncoder.getPosition();
     }
 
+    public void resetEncoders() {
+        rightEncoder.setPosition(0.0);
+        leftEncoder.setPosition(0.0);
+    }
+
+    public void setRightEncoder(double position) {
+        rightEncoder.setPosition(position);
+    }
+
+    public void setLeftEncoder(double position) {
+        leftEncoder.setPosition(position);
+    }
+
     /** Reset elevator PID */
     public void resetPID() {
         elevatorPID.reset(getElevatorPosition());
@@ -206,6 +220,10 @@ public double getElevatorVelocity() {
         return encoderAverageVel * ElevatorConstants.ROTATIONS_TO_METERS;
     }
 
+    public BooleanSupplier isAtTargetSupplier() {
+        return this.elevatorPID::atGoal;
+    }
+
     /**
      * Check if elevator is at target position
      *
diff --git a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java
index 27a10de..2a9bec8 100644
--- a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java
+++ b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java
@@ -5,12 +5,13 @@
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.blackknights.constants.ArmConstants;
+import org.blackknights.utils.NetworkTablesUtils;
 
 /** Subsystem to manage the intake (NOT HAND) */
 public class IntakeSubsystem extends SubsystemBase {
     private final WPI_TalonSRX motor = new WPI_TalonSRX(ArmConstants.MOTOR_ID);
 
-    private final DigitalInput intakeLinebreak = new DigitalInput(0);
+    private final DigitalInput intakeLinebreak = new DigitalInput(1);
 
     /** Create a new intake subsystem */
     public IntakeSubsystem() {
@@ -41,4 +42,9 @@ public void setVoltage(double voltage) {
     public boolean getLinebreak() {
         return !intakeLinebreak.get();
     }
+
+    @Override
+    public void periodic() {
+        NetworkTablesUtils.getTable("debug").setEntry("Intake/linebreak", this.getLinebreak());
+    }
 }
diff --git a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java
index 9d7df57..9c734fb 100644
--- a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java
+++ b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java
@@ -1,8 +1,6 @@
 /* Black Knights Robotics (C) 2025 */
 package org.blackknights.subsystems;
 
-import com.revrobotics.spark.config.SparkFlexConfig;
-import com.revrobotics.spark.config.SparkMaxConfig;
 import com.studica.frc.AHRS;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
@@ -24,7 +22,6 @@
 import org.apache.logging.log4j.Logger;
 import org.blackknights.constants.DrivetrainConstants;
 import org.blackknights.controllers.MAXSwerveModule;
-import org.blackknights.controllers.MAXSwerveModuleConfig;
 import org.blackknights.framework.Odometry;
 import org.blackknights.utils.ConfigManager;
 import org.blackknights.utils.NetworkTablesUtils;
@@ -58,7 +55,7 @@ public class SwerveSubsystem extends SubsystemBase {
                     DrivetrainConstants.REAR_RIGHT_CHASSIS_ANGULAR_OFFSET);
 
     // The gyro sensor
-    private final AHRS gyro = new AHRS(AHRS.NavXComType.kMXP_SPI);
+    private final AHRS gyro = new AHRS(AHRS.NavXComType.kUSB1);
 
     // Slew Rate Constants
     private double currentRotation = 0.0;
@@ -133,26 +130,26 @@ public SwerveSubsystem() {
                     .getDoubleTopic("rlpos")
                     .getEntry(rearLeft.getPosition().angle.getRadians());
 
-    public void reconfigure() {
-        ConfigManager cm = ConfigManager.getInstance();
-
-        SparkFlexConfig drivingConfig = MAXSwerveModuleConfig.drivingConfig;
-        SparkMaxConfig turningConfig = MAXSwerveModuleConfig.turningConfig;
-
-        drivingConfig.closedLoop.pid(
-                cm.get("swerve_drive_p", 0.5),
-                cm.get("swerve_drive_i", 0.0),
-                cm.get("swerve_drive_d", 0.0));
-        turningConfig.closedLoop.pid(
-                cm.get("swerve_turning_p", 1),
-                cm.get("swerve_turning_i", 0.0),
-                cm.get("swerve_turning_d", 0.0));
-
-        frontLeft.reconfigure(drivingConfig, turningConfig);
-        rearRight.reconfigure(drivingConfig, turningConfig);
-        rearRight.reconfigure(drivingConfig, turningConfig);
-        rearLeft.reconfigure(drivingConfig, turningConfig);
-    }
+    //    public void reconfigure() {
+    //        ConfigManager cm = ConfigManager.getInstance();
+    //
+    //        SparkFlexConfig drivingConfig = MAXSwerveModuleConfig.drivingConfig;
+    //        SparkMaxConfig turningConfig = MAXSwerveModuleConfig.turningConfig;
+    //
+    //        drivingConfig.closedLoop.pid(
+    //                cm.get("swerve_drive_p", 0.5),
+    //                cm.get("swerve_drive_i", 0.0),
+    //                cm.get("swerve_drive_d", 0.0));
+    //        turningConfig.closedLoop.pid(
+    //                cm.get("swerve_turning_p", 1),
+    //                cm.get("swerve_turning_i", 0.0),
+    //                cm.get("swerve_turning_d", 0.0));
+    //
+    //        frontLeft.reconfigure(drivingConfig, turningConfig);
+    //        rearRight.reconfigure(drivingConfig, turningConfig);
+    //        rearRight.reconfigure(drivingConfig, turningConfig);
+    //        rearLeft.reconfigure(drivingConfig, turningConfig);
+    //    }
 
     @Override
     public void periodic() {
@@ -243,7 +240,6 @@ public void drive(
 
         if (rateLimit) {
 
-            // Math that calculates important stuff about where the robot is heading
             double inputTranslationDirection =
                     Math.atan2(sidewaysMetersPerSecond, forwardMetersPerSecond);
             double inputTranslationMagnitude =
@@ -251,64 +247,48 @@ public void drive(
                             Math.pow(forwardMetersPerSecond, 2.0)
                                     + Math.pow(sidewaysMetersPerSecond, 2.0));
 
+            double currentTranslationMagnitude =
+                    Math.sqrt(
+                            Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2.0)
+                                    + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2.0));
+
             double directionSlewRate;
             if (currentTranslationMagnitude != 0.0) {
                 directionSlewRate =
                         Math.abs(
-                                DrivetrainConstants.DIRECTION_SLEW_RATE
+                                ConfigManager.getInstance()
+                                                .get(
+                                                        "drive_direction_slew_rate",
+                                                        DrivetrainConstants.DIRECTION_SLEW_RATE)
                                         / currentTranslationMagnitude);
             } else {
-                directionSlewRate = 500.0; // super high number means slew is instantaneous
+                directionSlewRate =
+                        500.0; // super high number means change in direction is instantaneous
             }
 
             double currentTime = WPIUtilJNI.now() * 1e-6;
             double elapsedTime = currentTime - previousTime;
 
-            double angleDifference =
-                    SwerveUtils.angleDifference(
-                            inputTranslationDirection, currentTranslationDirection);
-            if (angleDifference < 0.45 * Math.PI) {
-                currentTranslationDirection =
-                        SwerveUtils.stepTowardsCircular(
-                                currentTranslationDirection,
-                                inputTranslationDirection,
-                                directionSlewRate * elapsedTime);
-                currentTranslationMagnitude = magnitudeLimiter.calculate(inputTranslationMagnitude);
-            } else if (angleDifference > 0.85 * Math.PI) {
-                if (currentTranslationMagnitude
-                        > 1e-4) { // small number avoids floating-point errors
-                    currentTranslationMagnitude = magnitudeLimiter.calculate(0.0);
-                } else {
-                    currentTranslationDirection =
-                            SwerveUtils.wrapAngle(currentTranslationDirection + Math.PI);
-                    currentTranslationMagnitude =
-                            magnitudeLimiter.calculate(inputTranslationMagnitude);
-                }
-            } else {
-                currentTranslationDirection =
-                        SwerveUtils.stepTowardsCircular(
-                                currentTranslationDirection,
-                                inputTranslationDirection,
-                                directionSlewRate * elapsedTime);
-                currentTranslationMagnitude = magnitudeLimiter.calculate(inputTranslationMagnitude);
-            }
+            currentTranslationDirection =
+                    SwerveUtils.stepTowardsCircular(
+                            currentTranslationDirection,
+                            inputTranslationDirection,
+                            directionSlewRate * elapsedTime);
 
             previousTime = currentTime;
 
-            xSpeedCommanded = currentTranslationMagnitude * Math.cos(currentTranslationDirection);
-            ySpeedCommanded = currentTranslationMagnitude * Math.sin(currentTranslationDirection);
-            currentRotation = rotationLimiter.calculate(radiansPerSecond);
+            xSpeedCommanded = inputTranslationMagnitude * Math.cos(currentTranslationDirection);
+            ySpeedCommanded = inputTranslationMagnitude * Math.sin(currentTranslationDirection);
 
         } else {
             // If there's no rate limit, robot does the exact inputs given.
             xSpeedCommanded = forwardMetersPerSecond;
             ySpeedCommanded = sidewaysMetersPerSecond;
-            currentRotation = radiansPerSecond;
         }
 
         double xSpeedDelivered = xSpeedCommanded;
         double ySpeedDelivered = ySpeedCommanded;
-        double rotationDelivered = currentRotation;
+        double rotationDelivered = radiansPerSecond;
 
         var swerveModuleStates =
                 DrivetrainConstants.DRIVE_KINEMATICS.toSwerveModuleStates(
@@ -328,6 +308,7 @@ public void drive(
                                         xSpeedDelivered, ySpeedDelivered, rotationDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
                 swerveModuleStates, DrivetrainConstants.MAX_SPEED_METERS_PER_SECOND);
+
         frontLeft.setDesiredState(swerveModuleStates[0]);
         frontRight.setDesiredState(swerveModuleStates[1]);
         rearLeft.setDesiredState(swerveModuleStates[2]);
diff --git a/src/main/java/org/blackknights/utils/Camera.java b/src/main/java/org/blackknights/utils/Camera.java
index 4ff5c7c..b60153e 100644
--- a/src/main/java/org/blackknights/utils/Camera.java
+++ b/src/main/java/org/blackknights/utils/Camera.java
@@ -17,17 +17,18 @@
 
 /** */
 public class Camera {
-    private Optional<NetworkTablesUtils> limelightTable;
+    private NetworkTablesUtils limelightTable;
 
-    private Optional<PhotonCamera> photonCamera;
-    private Optional<PhotonPoseEstimator> photonPoseEstimator;
+    private PhotonCamera photonCamera;
+    private PhotonPoseEstimator photonPoseEstimator;
+
+    private final Transform3d camOffset;
+    private final String name;
+    private final CameraType camType;
 
-    private Transform3d camOffset;
     private double photonTimestamp;
     private Pose3d targetPose;
-    private String name;
-
-    private final CameraType camType;
+    private boolean enabled;
 
     private static final Logger LOGGER = LogManager.getLogger();
 
@@ -41,24 +42,22 @@ public Camera(String name, CameraType camType, Transform3d camOffset) {
         this.camType = camType;
         this.camOffset = camOffset;
         this.name = name;
+        this.enabled = true;
 
         switch (this.camType) {
             case PHOTONVISION -> {
-                photonCamera = Optional.of(new PhotonCamera(name));
+                photonCamera = new PhotonCamera(name);
                 photonPoseEstimator =
-                        Optional.of(
-                                new PhotonPoseEstimator(
-                                        AprilTagFieldLayout.loadField(
-                                                AprilTagFields.k2025ReefscapeWelded),
-                                        PhotonPoseEstimator.PoseStrategy
-                                                .MULTI_TAG_PNP_ON_COPROCESSOR,
-                                        this.camOffset));
-                limelightTable = Optional.empty();
+                        new PhotonPoseEstimator(
+                                AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded),
+                                PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+                                this.camOffset);
+                limelightTable = null;
             }
             case LIMELIGHT -> {
-                photonCamera = Optional.empty();
-                photonPoseEstimator = Optional.empty();
-                limelightTable = Optional.of(NetworkTablesUtils.getTable(name));
+                photonCamera = null;
+                photonPoseEstimator = null;
+                limelightTable = NetworkTablesUtils.getTable(name);
             }
         }
     }
@@ -72,12 +71,12 @@ public Camera(String name, CameraType camType, Transform3d camOffset) {
     public Optional<Pose3d> getPoseFieldSpace(Pose3d prevPosition) {
         switch (this.camType) {
             case PHOTONVISION -> {
-                assert this.photonPoseEstimator.isPresent();
-                assert this.photonCamera.isPresent();
+                assert this.photonPoseEstimator != null;
+                assert this.photonCamera != null;
 
                 PhotonPipelineResult res;
                 try {
-                    res = this.photonCamera.get().getAllUnreadResults().get(0);
+                    res = this.photonCamera.getAllUnreadResults().get(0);
                     Transform3d worstTarget = res.getBestTarget().getBestCameraToTarget();
                     this.targetPose =
                             new Pose3d(worstTarget.getTranslation(), worstTarget.getRotation());
@@ -87,9 +86,8 @@ public Optional<Pose3d> getPoseFieldSpace(Pose3d prevPosition) {
 
                 if (!res.hasTargets()) return Optional.empty();
 
-                this.photonPoseEstimator.get().setReferencePose(prevPosition);
+                this.photonPoseEstimator.setReferencePose(prevPosition);
                 return photonPoseEstimator
-                        .get()
                         .update(res)
                         .map(
                                 (e) -> {
@@ -105,14 +103,13 @@ public Optional<Pose3d> getPoseFieldSpace(Pose3d prevPosition) {
     }
 
     private Optional<Pose3d> getPose3dLimelight() {
-        assert this.limelightTable.isPresent();
+        assert this.limelightTable != null;
 
-        double[] rawPose =
-                this.limelightTable.get().getArrayEntry("botpose_wpiblue", new double[0]);
+        double[] rawPose = this.limelightTable.getArrayEntry("botpose_wpiblue", new double[0]);
         if (rawPose.length != 6) return Optional.empty();
 
         double[] targetPoseRaw =
-                this.limelightTable.get().getArrayEntry("targetpose_cameraspace", new double[0]);
+                this.limelightTable.getArrayEntry("targetpose_cameraspace", new double[0]);
         if (targetPoseRaw.length == 6) {
             Pose3d cameraPose =
                     new Pose3d(
@@ -138,15 +135,13 @@ private Optional<Pose3d> getPose3dLimelight() {
     public double getTimestamp() {
         switch (this.camType) {
             case LIMELIGHT -> {
-                assert this.limelightTable.isPresent();
+                assert this.limelightTable != null;
                 return Timer.getFPGATimestamp()
-                        - (this.limelightTable.get().getEntry("tl", Double.POSITIVE_INFINITY)
-                                / 1000)
-                        - (this.limelightTable.get().getEntry("cl", Double.POSITIVE_INFINITY)
-                                / 1000);
+                        - (this.limelightTable.getEntry("tl", Double.POSITIVE_INFINITY) / 1000)
+                        - (this.limelightTable.getEntry("cl", Double.POSITIVE_INFINITY) / 1000);
             }
             case PHOTONVISION -> {
-                assert this.photonCamera.isPresent();
+                assert this.photonCamera != null;
 
                 return photonTimestamp;
             }
@@ -154,6 +149,14 @@ public double getTimestamp() {
         return 0;
     }
 
+    public void setEnabled(boolean e) {
+        this.enabled = e;
+    }
+
+    public boolean isEnabled() {
+        return this.enabled;
+    }
+
     /**
      * Get the name of the camera
      *
diff --git a/src/test/java/org/blackknights/framework/CoralQueueTest.java b/src/test/java/org/blackknights/framework/CoralQueueTest.java
index 7d2cdd3..0527395 100644
--- a/src/test/java/org/blackknights/framework/CoralQueueTest.java
+++ b/src/test/java/org/blackknights/framework/CoralQueueTest.java
@@ -24,7 +24,8 @@ void testListToQueue() {
                 new CoralQueue.CoralPosition(
                         "11L4",
                         ScoringConstants.CORAL_POSITIONS[10],
-                        ScoringConstants.ScoringHeights.L4);
+                        ScoringConstants.ScoringHeights.L4,
+                        ScoringConstants.ScoringSides.RIGHT);
 
         assertEquals(expected, coralQueue.getNext());
 
@@ -32,7 +33,8 @@ void testListToQueue() {
                 new CoralQueue.CoralPosition(
                         "1L1",
                         ScoringConstants.CORAL_POSITIONS[0],
-                        ScoringConstants.ScoringHeights.L1);
+                        ScoringConstants.ScoringHeights.L1,
+                        ScoringConstants.ScoringSides.RIGHT);
 
         assertEquals(expected, coralQueue.getNext());
 
@@ -40,7 +42,8 @@ void testListToQueue() {
                 new CoralQueue.CoralPosition(
                         "3L3",
                         ScoringConstants.CORAL_POSITIONS[2],
-                        ScoringConstants.ScoringHeights.L3);
+                        ScoringConstants.ScoringHeights.L3,
+                        ScoringConstants.ScoringSides.RIGHT);
 
         assertEquals(expected, coralQueue.getNext());
     }
@@ -53,7 +56,8 @@ void testSkipNextValue() {
                 new CoralQueue.CoralPosition(
                         "1L1",
                         ScoringConstants.CORAL_POSITIONS[0],
-                        ScoringConstants.ScoringHeights.L1);
+                        ScoringConstants.ScoringHeights.L1,
+                        ScoringConstants.ScoringSides.RIGHT);
         assertEquals(expected, coralQueue.getNext());
     }
 
@@ -67,22 +71,25 @@ void testBlueAlliancePositionMapping() {
         CoralQueue.CoralPosition expected =
                 new CoralQueue.CoralPosition(
                         "6L1",
-                        ScoringConstants.CORAL_POSITIONS[5 + 12],
-                        ScoringConstants.ScoringHeights.L1);
+                        ScoringConstants.CORAL_POSITIONS[17],
+                        ScoringConstants.ScoringHeights.L1,
+                        ScoringConstants.ScoringSides.LEFT);
         assertEquals(expected, coralQueue.getNext());
 
         expected =
                 new CoralQueue.CoralPosition(
                         "12L4",
-                        ScoringConstants.CORAL_POSITIONS[11 + 12],
-                        ScoringConstants.ScoringHeights.L4);
+                        ScoringConstants.CORAL_POSITIONS[23],
+                        ScoringConstants.ScoringHeights.L4,
+                        ScoringConstants.ScoringSides.LEFT);
         assertEquals(expected, coralQueue.getNext());
 
         expected =
                 new CoralQueue.CoralPosition(
                         "9L4",
-                        ScoringConstants.CORAL_POSITIONS[8 + 12],
-                        ScoringConstants.ScoringHeights.L4);
+                        ScoringConstants.CORAL_POSITIONS[20],
+                        ScoringConstants.ScoringHeights.L4,
+                        ScoringConstants.ScoringSides.RIGHT);
 
         assertEquals(expected, coralQueue.getNext());
 
diff --git a/tuning/backups/colorado_v1.1.json b/tuning/backups/colorado_v1.1.json
new file mode 100755
index 0000000..92da67a
--- /dev/null
+++ b/tuning/backups/colorado_v1.1.json
@@ -0,0 +1 @@
+{"align_enable_slow_mode":false,"back_time_sec":1.0,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":7.5,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.7,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.25,"align_dist_back":0.82,"swerve_drive_p":0.04,"outtake_wait_time_ms":280.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.48,"arm_max_vel_degs":90.0,"scoring_left_y":0.195,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"center_cam_pitch":-45.0,"controller_deadband":0.2,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"back_mps":-3.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.7,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.0,"scoring_right_x":0.48,"scoring_right_y":-0.14,"arm_encoder_offset":2.556,"elevator_intake":0.13,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":-0.22,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.55,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":0.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":7.5}
diff --git a/tuning/backups/colorado_v1.json b/tuning/backups/colorado_v1.json
new file mode 100755
index 0000000..79a4ca6
--- /dev/null
+++ b/tuning/backups/colorado_v1.json
@@ -0,0 +1 @@
+{"align_enable_slow_mode":false,"back_time_sec":0.0,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":7.5,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.7,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.25,"align_dist_back":0.82,"swerve_drive_p":0.04,"outtake_wait_time_ms":280.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.48,"arm_max_vel_degs":90.0,"scoring_left_y":0.195,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"center_cam_pitch":-45.0,"controller_deadband":0.2,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"back_mps":-3.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.7,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.0,"scoring_right_x":0.48,"scoring_right_y":-0.14,"arm_encoder_offset":2.556,"elevator_intake":0.1,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":-0.22,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.5,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":0.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":7.5}
diff --git a/tuning/backups/tuning_2025_03_17:20:25:13.json b/tuning/backups/tuning_2025_03_17:20:25:13.json
new file mode 100755
index 0000000..bdf3230
--- /dev/null
+++ b/tuning/backups/tuning_2025_03_17:20:25:13.json
@@ -0,0 +1 @@
+{"align_enable_slow_mode":false,"back_time_sec":1.0,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":7.5,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.7,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.5,"align_dist_back":0.82,"swerve_drive_p":0.04,"outtake_wait_time_ms":200.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.5,"arm_max_vel_degs":90.0,"scoring_left_y":0.18,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"center_cam_pitch":-45.0,"controller_deadband":0.2,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"back_mps":-3.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.7,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.11,"scoring_right_x":0.5,"scoring_right_y":-0.2,"arm_encoder_offset":2.556,"elevator_intake":0.1,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":-0.22,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.63,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":0.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":7.5}
diff --git a/tuning/backups/tuning_2025_03_28:22:20:37.json b/tuning/backups/tuning_2025_03_28:22:20:37.json
new file mode 100755
index 0000000..ffd88b9
--- /dev/null
+++ b/tuning/backups/tuning_2025_03_28:22:20:37.json
@@ -0,0 +1 @@
+{"align_enable_slow_mode":false,"back_time_sec":0.0,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"outtake_max_time_sec":5.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":2.0,"align_rough_rotation_tolerance":40,"align_fine_x_max_vel_m":1.0,"align_close_y_p":3.1,"align_rough_vel_tol":0.5,"arm_tol":1.5,"vision_min_distance":0.5,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"outtaking_time_ms":550.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":1.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.5,"align_x_max_accel_mps":3.5,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":3.5,"arm_intake":0.7,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_fine_vel_tol":0.05,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.25,"align_dist_back":0.82,"swerve_drive_p":0.04,"outtake_wait_time_ms":300.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_fine_y_target_end_vel":0.0,"align_rough_y_target_end_vel":0.0,"align_x_p":3.0,"scoring_left_x":0.565,"arm_max_vel_degs":90.0,"scoring_left_y":0.184,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"center_cam_pitch":-45.0,"controller_deadband":0.2,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":3.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":4.5,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_fine_pos_dist_tol":0.02,"align_ff_ks":1.0,"back_mps":-3.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"align_ff_kv":1.0,"elevator_l1":0.2,"elevator_l2":0.5,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.982,"elevator_l4":1.67,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":2.5,"align_y_max_accel_mps":3.5,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":2.5,"placeholder":0.0,"align_trap_t_sec":0.2,"arm_kg":0.0,"scoring_right_x":0.565,"align_rough_x_target_end_vel":0.0,"drive_direction_slew_rate":18.0,"scoring_right_y":-0.152,"arm_encoder_offset":-0.153,"elevator_intake":0.13,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_fine_x_target_end_vel":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":4.5,"autointake_dist_back":-0.22,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.5,"arm_l3":-0.6,"align_vel_tol":0.25,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":0.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"fake_pose_dist_back":0.5,"align_rough_x_max_vel_m":4.5}
diff --git a/tuning/backups/tuning_2025_03_Fri:21:16:57.json b/tuning/backups/tuning_2025_03_Fri:21:16:57.json
new file mode 100755
index 0000000..7faffa1
--- /dev/null
+++ b/tuning/backups/tuning_2025_03_Fri:21:16:57.json
@@ -0,0 +1 @@
+{"align_enable_slow_mode":false,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.1,"driver_max_speed":3.5,"align_rough_rotation_tolerance":10,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.3,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.5,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":5.0,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.64,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_dist_back":0.72,"swerve_drive_p":0.04,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.5,"arm_max_vel_degs":90.0,"scoring_left_y":0.21,"align_fine_pos_tolerance":0.03,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":65.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.64,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.82,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":5.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.1,"scoring_right_x":0.5,"scoring_right_y":-0.16,"arm_encoder_offset":1.679,"elevator_intake":0.14,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":0.6,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.636,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":1.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":5.0}
diff --git a/tuning/backups/tuning_2025_03_Mon:20:05:39.json b/tuning/backups/tuning_2025_03_Mon:20:05:39.json
new file mode 100755
index 0000000..65d058f
--- /dev/null
+++ b/tuning/backups/tuning_2025_03_Mon:20:05:39.json
@@ -0,0 +1 @@
+{"align_enable_slow_mode":false,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.3,"align_rough_rotation_tolerance":10,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":3.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.0,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":3.5,"arm_intake":0.64,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_dist_back":0.72,"swerve_drive_p":0.04,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.5,"arm_max_vel_degs":90.0,"scoring_left_y":0.21,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.2,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":3.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":25.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.875,"elevator_l4":1.64,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.0,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.82,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"arm_ks":0.0,"align_fine_finish_time":400.0,"elevator_max_vel":5.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.0,"placeholder":0.0,"arm_kg":0.1,"scoring_right_x":0.5,"scoring_right_y":-0.13,"arm_encoder_offset":1.679,"elevator_intake":0.14,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":0.6,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.636,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":200.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":360,"align_rough_x_max_vel_m":4.0}
diff --git a/tuning/backups/tuning_2025_04_04:08:14:51.json b/tuning/backups/tuning_2025_04_04:08:14:51.json
new file mode 100755
index 0000000..6a61b10
--- /dev/null
+++ b/tuning/backups/tuning_2025_04_04:08:14:51.json
@@ -0,0 +1 @@
+{"align_fine_halfmoon_dist":0.5,"shooter_high_pass":0.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"left_cam_x":0.253,"align_fine_x_max_vel_m":1.0,"align_close_y_p":3.1,"vision_min_distance":0.5,"inf_switch_dist":2.5,"shooter_low_pass":0.0,"align_rough_max_accel_mps":8.5,"align_close_y_d":0.1,"drive_magnitude_slew_rate":22.0,"outtaking_time_ms":450.0,"outtake_speed":10,"align_fine_y_max_vel_m":1.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":727.0,"align_ff":0.1,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_fine_ending_vel_mag":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.5,"odom_vision_trust":0.5,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.5,"arm_intake":0.75,"swerve_drive_i":0.0,"align_rough_rot_tol_deg":90.0,"swerve_drive_d":0.0,"align_fine_vel_tol":0.25,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.25,"swerve_drive_p":0.04,"outtake_wait_time_ms":250.0,"outtake_l4_wait_time_ms":250,"align_rough_y_target_end_vel":0.0,"scoring_left_x":0.6,"align_fine_max_vel_m":2.0,"scoring_left_y":0.16,"align_fine_pos_tolerance":0.05,"center_cam_pitch":-45.0,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_fine_rot_vel_tol":0.005,"align_x_axis_p":3,"swerve_max_translational_speed_mps":4.5,"align_rough_x_max_accel_mps":4.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_x_max_vel_m":4.5,"align_rot_i":0.0,"align_x_axis_i":0.0,"intake_speed":-8,"align_rot_p":7.3,"odom_vision_trust_theta":0.08726646259971647,"align_rough_back":0.5,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"arm_max_accel_degs":90.0,"elevator_d":0.0,"align_rough_min_vel":0.01,"l1_elevator_position":1.0,"outtaking_l4_time_ms":200,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_rotation_ff_kv":0.0,"align_rotation_ff_ks":0.04,"arm_i":0.0,"arm_d":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"align_rough_halfmoon_dist":0.5,"placeholder":0.0,"align_fine_ms_before_rotate":0.0,"scoring_right_x":0.6,"align_rough_x_target_end_vel":0.0,"drive_direction_slew_rate":0.1,"scoring_right_y":-0.14,"elevator_intake":0.09,"align_fine_max_accel_mps":2.0,"align_fine_max_time":3.0,"align_rough_halfmoon_tol":0.0,"outtake_l1_wait_time_ms":250,"align_fine_rotation_tolerance":1,"align_y_max_vel_m":4.5,"align_vel_tol":0.25,"align_rough_finish_time":0.0,"align_rough_rot_max_accel_degps":720,"align_rough_x_max_vel_m":4.5,"align_enable_slow_mode":false,"back_time_sec":1.0,"align_close_speed_scaler":0.5,"align_rough_ms_before_rotate":0.0,"align_close__y_i":0.0,"align_close_distance":1.0,"outtake_max_time_sec":5.0,"align_rough_max_vel_m":5.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_rough_vel_tol":0.5,"arm_tol":1.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","swerve_max_rotation_velocity_radsps":10.5,"swerve_turning_p":1.0,"right_cam_x":0.163,"swerve_turning_i":0.0,"swerve_turning_d":0.0,"max_roation_diff":3.0,"swerve_abs_max_module_speed_mps":4.5,"elevator_pid_tolerance":0.05,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"align_x_max_accel_mps":3.5,"vision_cutoff_distance":3.0,"align_fine_rot_tol_deg":3,"elevator_current_max":20.0,"outtaking_l1_time_ms":200,"align_dist_back":1.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_fine_y_target_end_vel":0.0,"align_x_p":3.0,"arm_max_vel_degs":90.0,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"align_pos_rough_tol":0.25,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_y_axis_d":0.25,"driver_max_speed_rot":325,"align_slow_more":false,"align_auto_finish_time":200.0,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"shooter_speaker":0.0,"align_fine_pos_dist_tol":0.05,"align_ff_ks":1.0,"back_mps":-3.0,"align_auto_rotation_tolerance":1,"align_rough_ending_vel_mag":0.5,"align_ff_kv":1.0,"elevator_l1":0.2,"elevator_l2":0.45,"elevator_l3":0.982,"swerve_min_velocity":0.01,"elevator_l4":1.67,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":4.0,"align_y_max_accel_mps":3.5,"align_rough_rot_max_vel_deg":720,"align_fine_min_vel":0.002,"align_close_rot_p":3.0,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":4.0,"align_trap_t_sec":0.1,"arm_kg":0.0,"arm_encoder_offset":-0.153,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_fine_x_target_end_vel":0.0,"align_slow_mode":false,"test_drive_y":0.0,"right_cam_angle":5.0,"test_drive_x":0.1,"elevator_zero_voltage":-3.0,"autointake_dist_back":-0.25,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.5,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"elevator_zeroing_voltage":0.0,"fake_pose_dist_back":0.5}
diff --git a/tuning/backups/tuning_2025_04_04:14:26:12.json b/tuning/backups/tuning_2025_04_04:14:26:12.json
new file mode 100755
index 0000000..899f2e0
--- /dev/null
+++ b/tuning/backups/tuning_2025_04_04:14:26:12.json
@@ -0,0 +1 @@
+{"align_fine_halfmoon_dist":0.5,"shooter_high_pass":0.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"left_cam_x":0.253,"align_fine_x_max_vel_m":1.0,"align_close_y_p":3.1,"vision_min_distance":0.5,"inf_switch_dist":2.5,"shooter_low_pass":0.0,"align_rough_max_accel_mps":8.5,"align_close_y_d":0.1,"drive_magnitude_slew_rate":22.0,"outtaking_time_ms":200.0,"outtake_speed":10,"align_fine_y_max_vel_m":1.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":727.0,"align_ff":0.1,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_fine_ending_vel_mag":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.5,"odom_vision_trust":0.5,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.5,"arm_intake":0.75,"swerve_drive_i":0.0,"align_rough_rot_tol_deg":90.0,"swerve_drive_d":0.0,"align_fine_vel_tol":0.25,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.1,"swerve_drive_p":0.04,"outtake_wait_time_ms":0.0,"outtake_l4_wait_time_ms":10000,"align_rough_y_target_end_vel":0.0,"scoring_left_x":0.6,"align_fine_max_vel_m":2.0,"scoring_left_y":0.18,"align_fine_pos_tolerance":0.05,"center_cam_pitch":-45.0,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_fine_rot_vel_tol":0.005,"align_x_axis_p":3,"swerve_max_translational_speed_mps":4.5,"align_rough_x_max_accel_mps":4.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_x_max_vel_m":4.5,"align_rot_i":0.0,"align_x_axis_i":0.0,"intake_speed":-8,"align_rot_p":7.3,"odom_vision_trust_theta":0.08726646259971647,"align_rough_back":0.5,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"arm_max_accel_degs":90.0,"elevator_d":0.0,"align_rough_min_vel":0.01,"l1_elevator_position":1.0,"outtaking_l4_time_ms":200,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_rotation_ff_kv":0.0,"align_rotation_ff_ks":0.04,"arm_i":0.0,"arm_d":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"align_rough_halfmoon_dist":0.5,"placeholder":0.0,"align_fine_ms_before_rotate":0.0,"scoring_right_x":0.6,"align_rough_x_target_end_vel":0.0,"drive_direction_slew_rate":0.1,"scoring_right_y":-0.17,"elevator_intake":0.09,"align_fine_max_accel_mps":2.0,"align_fine_max_time":8.0,"align_rough_halfmoon_tol":0.0,"outtake_l1_wait_time_ms":250,"align_fine_rotation_tolerance":1,"align_y_max_vel_m":4.5,"align_vel_tol":0.25,"align_rough_finish_time":0.0,"align_rough_rot_max_accel_degps":720,"align_rough_x_max_vel_m":4.5,"align_enable_slow_mode":false,"back_time_sec":1.0,"align_close_speed_scaler":0.5,"align_rough_ms_before_rotate":0.0,"align_close__y_i":0.0,"align_close_distance":1.0,"outtake_max_time_sec":5.0,"align_rough_max_vel_m":5.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_rough_vel_tol":0.5,"arm_tol":1.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","swerve_max_rotation_velocity_radsps":10.5,"swerve_turning_p":1.0,"right_cam_x":0.163,"swerve_turning_i":0.0,"swerve_turning_d":0.0,"max_roation_diff":3.0,"swerve_abs_max_module_speed_mps":4.5,"elevator_pid_tolerance":0.05,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"align_x_max_accel_mps":3.5,"vision_cutoff_distance":3.0,"align_fine_rot_tol_deg":3,"elevator_current_max":20.0,"outtaking_l1_time_ms":200,"align_dist_back":1.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_fine_y_target_end_vel":0.0,"align_x_p":3.0,"arm_max_vel_degs":90.0,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"align_pos_rough_tol":0.25,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_y_axis_d":0.25,"driver_max_speed_rot":325,"align_slow_more":false,"align_auto_finish_time":200.0,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"shooter_speaker":0.0,"align_fine_pos_dist_tol":0.02,"align_ff_ks":1.0,"back_mps":-3.0,"align_auto_rotation_tolerance":1,"align_rough_ending_vel_mag":0.0,"align_ff_kv":1.0,"elevator_l1":0.2,"elevator_l2":0.45,"elevator_l3":0.982,"swerve_min_velocity":0.01,"elevator_l4":1.67,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":4.0,"align_y_max_accel_mps":3.5,"align_rough_rot_max_vel_deg":720,"align_fine_min_vel":0.002,"align_close_rot_p":3.0,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":4.0,"align_trap_t_sec":0.1,"arm_kg":0.0,"arm_encoder_offset":-0.153,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_fine_x_target_end_vel":0.0,"align_slow_mode":false,"test_drive_y":0.0,"right_cam_angle":5.0,"test_drive_x":0.1,"elevator_zero_voltage":-3.0,"autointake_dist_back":-0.25,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.5,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"elevator_zeroing_voltage":0.0,"fake_pose_dist_back":0.5}
diff --git a/tuning/fetch_tuning.sh b/tuning/fetch_tuning.sh
new file mode 100755
index 0000000..978928d
--- /dev/null
+++ b/tuning/fetch_tuning.sh
@@ -0,0 +1,3 @@
+#!/usr/bin/env bash
+
+scp admin@10.20.36.2:/home/lvuser/deploy/tuning.json backups/tuning_$(date +%Y_%m_%d:%T).json