diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java
index a6df488..7d2e683 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java
@@ -86,10 +86,11 @@ public void initialize() {
         driverController = new GamepadEx(gamepad1);
         operatorController = new GamepadEx(gamepad2);
 
+        GlobalConstants.opModeType = GlobalConstants.OpModeType.TELEOP;
         savedLocation = SavedConfiguration.selectedLocation;
         savedAutonomousRoutine = SavedConfiguration.selectedAuto;
         savedAllianceColor = SavedConfiguration.selectedAlliance;
-        savedAutonomousEndingPosition = SavedConfiguration.pathEndPose;
+        savedAutonomousEndingPosition = SavedConfiguration.finalDrivetrainPose;
 
         teleopInitTime = getRuntime();
         configLocked = false;
@@ -102,10 +103,7 @@ public void initialize() {
         TeleopBindings.configureBindings(driverController, operatorController, drivetrain, intake, transfer, shooter, turret, lift, led);
         TeleopBindings.configureDefaultCommands(driverController, operatorController, drivetrain, intake, transfer, shooter, turret, led);
 
-        schedule(
-                new RunCommand(() -> telemetryManager.update(telemetry)),
-                new RunCommand(led::update)
-        );
+        schedule(new RunCommand(() -> telemetryManager.update(telemetry)));
     }
 
     private void readInputs() {
@@ -158,6 +156,7 @@ public void initialize_loop() {
         }
 
         telemetryManager.update(telemetry);
+        lift.onInitialization();
         led.update();
     }
 
@@ -190,7 +189,6 @@ private void drawLockedUI() {
 
     private void setGlobalSettings() {
         GlobalConstants.allianceColor = savedAllianceColor;
-        GlobalConstants.opModeType = GlobalConstants.OpModeType.TELEOP;
     }
 
     private <T> T cycleRight(T current, T[] values) {
@@ -228,7 +226,7 @@ public void run() {
             telemetryManager.addData(VisionConstants.kSubsystemName + " Estimated Robot Pose Y", visionPose2d.getY());
             telemetryManager.addData(VisionConstants.kSubsystemName + " Estimated Robot Pose θ", visionPose2d.getRotation().getRadians());
 
-            drivetrain.updateWithVision(visionPose2d);
+            //drivetrain.updateWithVision(visionPose2d);
         }
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java
index 4984e26..b418985 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java
@@ -1,10 +1,13 @@
 package org.firstinspires.ftc.teamcode;
 
+import org.firstinspires.ftc.library.command.Commands;
+import org.firstinspires.ftc.library.command.ConditionalCommand;
 import org.firstinspires.ftc.library.command.InstantCommand;
 import org.firstinspires.ftc.library.command.button.Trigger;
 import org.firstinspires.ftc.library.gamepad.GamepadEx;
 import org.firstinspires.ftc.library.gamepad.GamepadKeys;
 import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory;
+import org.firstinspires.ftc.teamcode.command_factories.LiftFactory;
 import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory;
 import org.firstinspires.ftc.teamcode.command_factories.SuperstructureFactory;
 import org.firstinspires.ftc.teamcode.command_factories.TransferFactory;
@@ -32,8 +35,8 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
 
         if(GlobalConstants.getCurrentDriverType() == GlobalConstants.DriverType.HANISH) {
             new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5)
-                    .whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1))
-                    .whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
+                    .whenActive(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1))
+                    .whenInactive(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0));
 
             new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
                     .whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 3500))
@@ -45,12 +48,12 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
             );
 
             driver.getGamepadButton(GamepadKeys.Button.CIRCLE).whenPressed(
-                    ShooterFactory.velocitySetpointCommand(shooter, () -> 3350).andThen(SuperstructureFactory.smartShootingCommand(intake, transfer, led))
+                    ShooterFactory.velocitySetpointCommand(shooter, () -> 3350).andThen(SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> true))
             );
 
             driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
-                    .whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1))
-                    .whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
+                    .whenPressed(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> -1))
+                    .whenReleased(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0));
 
             driver.getGamepadButton(GamepadKeys.Button.DPAD_UP)
                     .whenActive(() -> shooter.setHoodPosition(shooter.getHoodTargetPosition() + 0.001));
@@ -60,15 +63,19 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
                     .whenReleased(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));
         } else {
             new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5)
-                    .whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1))
-                    .whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
+                    .whenActive(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1))
+                    .whenInactive(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0));
 
             new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
-                    .whenActive(SuperstructureFactory.smartShootingCommand(intake, transfer, led));
+                    .whenActive(new ConditionalCommand(
+                            SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                            SuperstructureFactory.controlledShootCommand(intake, transfer, shooter, led, () -> false),
+                            drivetrain::isRobotinCloseZone
+                    ));
 
             driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
-                    .whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1))
-                    .whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
+                    .whenPressed(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> -1))
+                    .whenReleased(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0));
 
             driver.getGamepadButton(GamepadKeys.Button.RIGHT_STICK_BUTTON).toggleWhenPressed(
                     TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerAllowPosition),
@@ -117,6 +124,12 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
         operator.getGamepadButton(GamepadKeys.Button.SQUARE)
                 .whenPressed(TransferFactory.runKickerCycle(transfer));
 
+        operator.getGamepadButton(GamepadKeys.Button.TRIANGLE)
+                .whenPressed(LiftFactory.resetLiftToZeroCommand(lift));
+
+        //operator.getGamepadButton(GamepadKeys.Button.CIRCLE)
+        //        .whenPressed(new InstantCommand(() -> shooter.)
+
         new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
                 .whenActive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0.75))
                 .whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));
@@ -124,6 +137,13 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
         new Trigger(transfer::doesTransferContainAnyBalls)
                 .whenActive(new InstantCommand(() -> shooter.setFlywheelCommanded(true)))
                 .whenInactive(new InstantCommand(() -> shooter.setFlywheelCommanded(false)));
+
+        new Trigger(transfer::doesTransferContainAllBalls)
+                .whenActive(Commands.sequence(
+                        new InstantCommand(() -> driver.gamepad.rumble(1.0, 1.0, 200)),
+                        Commands.waitMillis(350),
+                        new InstantCommand(() -> driver.gamepad.rumble(1.0, 1.0, 200))
+                ));
     }
 
     public static void configureDefaultCommands(GamepadEx driver, GamepadEx operator, Drivetrain drivetrain, Intake intake, Transfer transfer, Shooter shooter, Turret turret, LED led) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto.java
index d098466..a9f3227 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto.java
@@ -8,5 +8,6 @@ public enum Auto {
     SIX_BALL,
     SIX_BALL_PICKUP,
     NINE_BALL,
-    NINE_BALL_PICKUP
+    NINE_BALL_PICKUP,
+    TWELVE_BALL
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoChooser.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoChooser.java
index f337689..6b59301 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoChooser.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoChooser.java
@@ -27,7 +27,7 @@ public AutoChooser(Drivetrain drivetrain, Intake intake, Transfer transfer, Turr
         routines.add(new AutoRoutine(Location.CLOSE, Auto.LEAVE, () -> autoFactory.initializeCloseLeave(GlobalConstants.allianceColor, Location.CLOSE.getPose())));
         routines.add(new AutoRoutine(Location.CLOSE, Auto.NINE_BALL, () -> autoFactory.initializeCloseNineBall(GlobalConstants.allianceColor, Location.CLOSE.getPose())));
         routines.add(new AutoRoutine(Location.CLOSE, Auto.NINE_BALL_PICKUP, () -> autoFactory.initializeCloseNineBallPickup(GlobalConstants.allianceColor, Location.CLOSE.getPose())));
-
+        routines.add(new AutoRoutine(Location.CLOSE, Auto.TWELVE_BALL, () -> autoFactory.initializeCloseTwelveBall(GlobalConstants.allianceColor, Location.CLOSE.getPose())));
 
         routines.add(new AutoRoutine(Location.FAR, Auto.IDLE, () -> autoFactory.initializeIdle(GlobalConstants.allianceColor, Location.FAR.getPose())));
         routines.add(new AutoRoutine(Location.FAR, Auto.LEAVE, () -> autoFactory.initializeFarLeave(GlobalConstants.allianceColor, Location.FAR.getPose())));
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFactory.java
index 6350a6e..9766b82 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFactory.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFactory.java
@@ -120,9 +120,14 @@ public Pair<Pose, Pair<Pose, Command>> initializeFarSixBall(GlobalConstants.Alli
                         DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue).getHeading()
                 )
                 .addPath(
-                        new BezierCurve(
+                        new BezierLine(
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue),
-                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOneControlPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOneReadyPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOneReadyPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOneReadyPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOnePositionBlue)
                         )
                 ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOnePositionBlue).getHeading())
@@ -137,10 +142,7 @@ public Pair<Pose, Pair<Pose, Command>> initializeFarSixBall(GlobalConstants.Alli
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue).getHeading()
-                )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue).getHeading())
                 .build();
 
         return Pair.of(
@@ -149,16 +151,18 @@ public Pair<Pose, Pair<Pose, Command>> initializeFarSixBall(GlobalConstants.Alli
                         DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue),
                         Commands.sequence(
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 1),
+                                SuperstructureFactory.controlledShootCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1),
                                 new WaitCommand(1000),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1),
+                                SuperstructureFactory.controlledShootCommand(intake, transfer, shooter, led, () -> false),
                                 new InstantCommand(turret::disableTurretAutoTracking),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 0),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0),
                                 new ParallelCommandGroup(
-                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1),
+                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(4), true, 1),
                                         TurretFactory.positionSetpointCommand(turret, () -> 0.0)
                                 )
                         )
@@ -181,9 +185,14 @@ public Pair<Pose, Pair<Pose, Command>> initializeFarNineBall(GlobalConstants.All
                         DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue).getHeading()
                 )
                 .addPath(
-                        new BezierCurve(
+                        new BezierLine(
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue),
-                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOneControlPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOneReadyPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOneReadyPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOneReadyPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOnePositionBlue)
                         )
                 ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupOnePositionBlue).getHeading())
@@ -194,18 +203,26 @@ public Pair<Pose, Pair<Pose, Command>> initializeFarNineBall(GlobalConstants.All
                         )
                 ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue).getHeading())
                 .addPath(
-                        new BezierCurve(
+                        new BezierLine(
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue),
-                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPreparePickupTwoControlPositionBlue),
-                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPreparePickupTwoPositionBlue)
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoReadyPositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPreparePickupTwoPositionBlue).getHeading()
-                )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoReadyPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoReadyPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoReadyPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoReadyPositionBlue).getHeading())
                 .addPath(
                         new BezierLine(
-                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPreparePickupTwoPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoReadyPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoPositionBlue)
                         )
                 ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoPositionBlue).getHeading())
@@ -214,18 +231,13 @@ public Pair<Pose, Pair<Pose, Command>> initializeFarNineBall(GlobalConstants.All
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarPickupTwoPositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue).getHeading()
-                ).addPath(
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue).getHeading())
+                .addPath(
                         new BezierLine(
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarShootingPositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue).getHeading()
-                )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue).getHeading())
                 .build();
 
         return Pair.of(
@@ -234,23 +246,29 @@ public Pair<Pose, Pair<Pose, Command>> initializeFarNineBall(GlobalConstants.All
                         DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoFarParkingPositionBlue),
                         Commands.sequence(
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 1),
+                                SuperstructureFactory.controlledShootCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1),
                                 new WaitCommand(1000),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 1),
-                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1),
                                 new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1),
+                                SuperstructureFactory.controlledShootCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(4), true, 1),
                                 new WaitCommand(1000),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(6), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(7), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(8), true, 1),
+                                SuperstructureFactory.controlledShootCommand(intake, transfer, shooter, led, () -> false),
                                 new InstantCommand(turret::disableTurretAutoTracking),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 0),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0),
                                 new ParallelCommandGroup(
-                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(6), true, 1),
+                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(9), true, 1),
                                         TurretFactory.positionSetpointCommand(turret, () -> 0.0)
                                 )
                         )
@@ -283,14 +301,16 @@ public Pair<Pose, Pair<Pose, Command>> initializeCloseNineBall(GlobalConstants.A
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupOnePositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupOnePositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading()
-                )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading())
                 .addPath(
-                        new BezierCurve(
+                        new BezierLine(
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue),
-                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoControlPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue)
                         )
                 ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue).getHeading())
@@ -299,19 +319,13 @@ public Pair<Pose, Pair<Pose, Command>> initializeCloseNineBall(GlobalConstants.A
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading()
-                )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading())
                 .addPath(
                         new BezierLine(
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseParkingPositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseParkingPositionBlue).getHeading()
-                )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseParkingPositionBlue).getHeading())
                 .build();
 
         return Pair.of(
@@ -320,21 +334,23 @@ public Pair<Pose, Pair<Pose, Command>> initializeCloseNineBall(GlobalConstants.A
                         DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseParkingPositionBlue),
                         Commands.sequence(
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1),
                                 new WaitCommand(1000),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1),
                                 new WaitCommand(1000),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(4), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
                                 new InstantCommand(turret::disableTurretAutoTracking),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 0),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0),
                                 new ParallelCommandGroup(
-                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1),
+                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(6), true, 1),
                                         TurretFactory.positionSetpointCommand(turret, () -> 0.0)
                                 )
                         )
@@ -367,14 +383,16 @@ public Pair<Pose, Pair<Pose, Command>> initializeCloseNineBallPickup(GlobalConst
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupOnePositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupOnePositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading()
-                )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading())
                 .addPath(
-                        new BezierCurve(
+                        new BezierLine(
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue),
-                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoControlPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue)
                         )
                 ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue).getHeading())
@@ -383,43 +401,171 @@ public Pair<Pose, Pair<Pose, Command>> initializeCloseNineBallPickup(GlobalConst
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue)
                         )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreeReadyPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreeReadyPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreeReadyPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue).getHeading())
+                .build();
+
+        return Pair.of(
+                refractoredPose,
+                Pair.of(
+                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue),
+                        Commands.sequence(
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(4), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                                new InstantCommand(turret::disableTurretAutoTracking),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1.0),
+                                new ParallelCommandGroup(
+                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(6), true, 1),
+                                        TurretFactory.positionSetpointCommand(turret, () -> 0.0)
+                                ),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(7), true, 1),
+                                new WaitCommand(1000),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0.0)
+                        )
+                )
+        );
+    }
+
+    public Pair<Pose, Pair<Pose, Command>> initializeCloseTwelveBall(GlobalConstants.AllianceColor alliance, Pose startingPose) {
+        PathBuilder pathBuilder = drivetrain.getPathBuilder();
+        Pose refractoredPose = DrivetrainConstants.decideToFlipPose(alliance, startingPose);
+
+        PathChain createdPath = pathBuilder
+                .addPath(
+                        new BezierLine(
+                                refractoredPose,
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue)
+                        )
                 ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue).getHeading(),
+                        refractoredPose.getHeading(),
                         DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading()
                 )
                 .addPath(
-                        new BezierCurve(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupOnePositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupOnePositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupOnePositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePrepareGateBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePrepareGateBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePrepareGateBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseGateBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseGateBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseGateBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoReadyPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupTwoPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue),
-                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreeControlPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreeReadyPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreeReadyPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreeReadyPositionBlue),
                                 DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue)
                         )
-                ).setLinearHeadingInterpolation(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading(),
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue).getHeading()
-                )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue).getHeading())
+                .addPath(
+                        new BezierLine(
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseShootingPositionBlue),
+                                DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseParkingPositionBlue)
+                        )
+                ).setConstantHeadingInterpolation(DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseParkingPositionBlue).getHeading())
                 .build();
 
         return Pair.of(
                 refractoredPose,
                 Pair.of(
-                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoClosePickupThreePositionBlue),
+                        DrivetrainConstants.decideToFlipPose(alliance, DrivetrainConstants.kAutoCloseParkingPositionBlue),
                         Commands.sequence(
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(0), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(1), true, 1),
                                 new WaitCommand(1000),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(2), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 1),
+                                new WaitCommand(1000),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(3), true, 1),
                                 new WaitCommand(1000),
                                 new FollowTrajectoryCommand(drivetrain, createdPath.getPath(4), true, 1),
-                                SuperstructureFactory.smartShootingCommand(intake, transfer, led),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(6), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(7), true, 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(8), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(9), true, 1),
+                                new WaitCommand(1000),
+                                new FollowTrajectoryCommand(drivetrain, createdPath.getPath(10), true, 1),
+                                SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
                                 new InstantCommand(turret::disableTurretAutoTracking),
-                                IntakeFactory.openLoopSetpointCommand(intake, () -> 0),
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0),
                                 new ParallelCommandGroup(
-                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(5), true, 1),
+                                        new FollowTrajectoryCommand(drivetrain, createdPath.getPath(11), true, 1),
                                         TurretFactory.positionSetpointCommand(turret, () -> 0.0)
                                 )
                         )
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java
index bc6bd58..79cd2db 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/SelectableAutonomous.java
@@ -131,9 +131,7 @@ public void initialize_loop() {
         telemetryManager.update(telemetry);
 
         lastTriangle = triangle;
-
         lift.onInitialization();
-        led.update();
     }
 
     private void handleControllerConfirmation() {
@@ -186,7 +184,6 @@ private void scheduleRoutine() {
         //TODO: Change the turret position to always track the goal
         schedule(
             new RunCommand(drivetrain::update),
-            new RunCommand(led::update),
             new SequentialCommandGroup(
                 new WaitUntilCommand(this::opModeIsActive),
                 routine.getSecond().getSecond()
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java
index 1185c1e..72a3965 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/IntakeFactory.java
@@ -8,16 +8,15 @@
 import java.util.function.DoubleSupplier;
 
 public class IntakeFactory {
-    public static Command velocitySetpointCommand(Intake intake, DoubleSupplier setpoint) {
-        return Commands.run(() -> {
-            double velocity = setpoint.getAsDouble();
-            intake.setVelocitySetpoint(velocity);
-        }).withName("Intake Velocity");
+    public static Command setUnevenOpenLoopSetpointCommand(Intake intake, DoubleSupplier setpoint) {
+        return Commands.runOnce(() -> {
+            intake.setOpenLoopSetpoint(setpoint.getAsDouble());
+        }).withName("Intake Open Loop");
     }
 
-    public static Command openLoopSetpointCommand(Intake intake, DoubleSupplier setpoint) {
+    public static Command setEvenOpenLoopSetpointCommand(Intake intake, DoubleSupplier setpoint) {
         return Commands.runOnce(() -> {
-            intake.setOpenLoopSetpoint(setpoint.getAsDouble());
+            intake.setEqualOpenLoopSetpoint(setpoint.getAsDouble());
         }).withName("Intake Open Loop");
     }
 
@@ -32,4 +31,16 @@ public static Command setRearIntakeOpenLoopSetpointCommand(Intake intake, Double
             intake.setRearMotorOpenLoop(setpoint.getAsDouble());
         }).withName("Intake Open Loop");
     }
+
+    public static Command setFrontIntakeVelocitySetpointCommand(Intake intake, DoubleSupplier setpoint) {
+        return Commands.runOnce(() -> {
+            intake.setFrontVelocitySetpoint(setpoint.getAsDouble());
+        }).withName("Intake Open Loop");
+    }
+
+    public static Command setRearIntakeVelocitySetpointCommand(Intake intake, DoubleSupplier setpoint) {
+        return Commands.runOnce(() -> {
+            intake.setRearVelocitySetpoint(setpoint.getAsDouble());
+        }).withName("Intake Open Loop");
+    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LiftFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LiftFactory.java
index f0a4486..825bfd1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LiftFactory.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/LiftFactory.java
@@ -5,7 +5,7 @@
 import org.firstinspires.ftc.teamcode.subsystems.Lift;
 
 public class LiftFactory {
-    public Command resetLiftToZeroCommand(Lift lift) {
+    public static Command resetLiftToZeroCommand(Lift lift) {
         return Commands.startEnd(lift::resetToZero, lift::stopHoming, lift).until(lift::isHomingSwitchTriggered);
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/SuperstructureFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/SuperstructureFactory.java
index 7382cc1..c137761 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/SuperstructureFactory.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/SuperstructureFactory.java
@@ -12,6 +12,7 @@
 import org.firstinspires.ftc.library.command.ParallelCommandGroup;
 import org.firstinspires.ftc.library.command.ParallelDeadlineGroup;
 import org.firstinspires.ftc.library.command.WaitCommand;
+import org.firstinspires.ftc.library.command.WaitUntilCommand;
 import org.firstinspires.ftc.library.math.MathUtility;
 import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants;
 import org.firstinspires.ftc.teamcode.constants.LEDConstants;
@@ -24,55 +25,65 @@
 import org.firstinspires.ftc.teamcode.subsystems.Transfer;
 
 import java.util.concurrent.locks.Condition;
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 public class SuperstructureFactory {
-    @Deprecated
-    public static Command smartVelocityRampCommand(Intake intake, Transfer transfer, Shooter shooter, LED led, DoubleSupplier shooterRPM, DoubleSupplier hoodPosition) {
-        return Commands.sequence( // TODO: Add a check to see if we have balls, if not then just return (maybe add it to the blink array)
-                new ConditionalCommand(
-                        Commands.none(),
-                        new ParallelCommandGroup(
-                                TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerIdlePosition),
-                                LEDFactory.setAdvancedStandardBlinkingCommand(led, LEDConstants.ColorValue.ORANGE, LEDConstants.ColorValue.VIOLET, () -> 50)
+    public static Command rapidFireCommand(Intake intake, Transfer transfer, Shooter shooter, LED led, BooleanSupplier ignoreFlywheelSetpointReached) {
+        return Commands.sequence(
+                Commands.either(
+                        Commands.sequence(
+                                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0.0),
+                                new WaitUntilCommand(shooter::flywheelAtSetpoint).interruptOn(ignoreFlywheelSetpointReached),
+                                new ConditionalCommand(
+                                        TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerAllowPosition).alongWith(LEDFactory.setAdvancedStandardBlinkingCommand(led, LEDConstants.ColorValue.ORANGE, LEDConstants.ColorValue.VIOLET, () -> 130)),
+                                        Commands.none(),
+                                        transfer::isBlockerEngaged
+                                ),
+                                LEDFactory.setStandardBlinkingCommand(led, LEDConstants.ColorValue.GREEN, () -> 170),
+                                rapidFireSequence(intake, transfer)
                         ),
-                        transfer::isBlockerEngaged
+                        Commands.none(),
+                        () -> transfer.getCurrentNumberOfBalls() > 0
                 ),
-                IntakeFactory.openLoopSetpointCommand(intake, () -> 0),
-                new ParallelCommandGroup(
-                        ShooterFactory.velocitySetpointCommand(shooter, shooterRPM),
-                        ShooterFactory.hoodPositionCommand(shooter, hoodPosition),
-                        LEDFactory.setStandardBlinkingCommand(led, LEDConstants.ColorValue.YELLOW, () -> 100)
+                TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerIdlePosition)
+        );
+    }
+
+    private static Command rapidFireSequence(Intake intake, Transfer transfer) {
+        return Commands.sequence(
+                shootSingleBall(intake, transfer),
+                Commands.either(
+                        Commands.sequence(
+                                indexNextBall(intake, transfer),
+                                shootSingleBall(intake, transfer)
+                        ),
+                        Commands.none(),
+                        () -> transfer.getCurrentNumberOfBalls() >= 1
                 ),
-                LEDFactory.setStandardBlinkingCommand(led, LEDConstants.ColorValue.GREEN, () -> 50)
+                Commands.either(
+                        Commands.sequence(
+                                indexNextBall(intake, transfer),
+                                shootSingleBall(intake, transfer)
+                        ),
+                        Commands.none(),
+                        () -> transfer.getCurrentNumberOfBalls() >= 1
+                )
         );
     }
 
-    public static Command smartShootingCommand(Intake intake, Transfer transfer, LED led) {
+    public static Command controlledShootCommand(Intake intake, Transfer transfer, Shooter shooter, LED led, BooleanSupplier ignoreFlywheelSetpointReached) {
         return Commands.sequence(
                 Commands.either(
                         Commands.sequence(
-                                new ParallelCommandGroup(
-                                        TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerAllowPosition),
-                                        LEDFactory.setAdvancedStandardBlinkingCommand(led, LEDConstants.ColorValue.ORANGE, LEDConstants.ColorValue.VIOLET, () -> 50)
-                                ),
-                                shootOneBallSensoredCommand(intake, transfer),
-                                Commands.either(
-                                        Commands.sequence(
-                                                indexBallsSensoredCommand(intake, transfer),
-                                                shootOneBallSensoredCommand(intake, transfer)
-                                        ),
+                                new ConditionalCommand(
+                                        TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerAllowPosition).alongWith(LEDFactory.setAdvancedStandardBlinkingCommand(led, LEDConstants.ColorValue.ORANGE, LEDConstants.ColorValue.VIOLET, () -> 130)),
                                         Commands.none(),
-                                        () -> transfer.getCurrentNumberOfBalls() > 2  // Check at sequence start
+                                        transfer::isBlockerEngaged
                                 ),
-                                Commands.either(
-                                        Commands.sequence(
-                                                indexBallsSensoredCommand(intake, transfer),
-                                                shootOneBallSensoredCommand(intake, transfer)
-                                        ),
-                                        Commands.none(),
-                                        () -> transfer.getCurrentNumberOfBalls() > 1  // Check at sequence start
-                                )
+                                new WaitUntilCommand(shooter::flywheelAtSetpoint).interruptOn(ignoreFlywheelSetpointReached),
+                                LEDFactory.setStandardBlinkingCommand(led, LEDConstants.ColorValue.GREEN, () -> 170),
+                                controlledFireSequence(intake, transfer)
                         ),
                         Commands.none(),
                         () -> transfer.getCurrentNumberOfBalls() > 0
@@ -81,6 +92,56 @@ public static Command smartShootingCommand(Intake intake, Transfer transfer, LED
         );
     }
 
+    private static Command controlledFireSequence(Intake intake, Transfer transfer) {
+        return Commands.sequence(
+                shootSingleBall(intake, transfer),
+                Commands.either(
+                        Commands.sequence(
+                                indexNextBall(intake, transfer),
+                                Commands.waitMillis(500),  // Flywheel ramp-up delay
+                                shootSingleBall(intake, transfer)
+                        ),
+                        Commands.none(),
+                        () -> transfer.getCurrentNumberOfBalls() >= 1
+                ),
+                Commands.either(
+                        Commands.sequence(
+                                indexNextBall(intake, transfer),
+                                Commands.waitMillis(500),  // Flywheel ramp-up delay
+                                shootSingleBall(intake, transfer)
+                        ),
+                        Commands.none(),
+                        () -> transfer.getCurrentNumberOfBalls() >= 1
+                )
+        );
+    }
+
+    private static Command shootSingleBall(Intake intake, Transfer transfer) {
+        return Commands.sequence(
+                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1.0),
+                Commands.waitUntil(transfer::isThirdBeamBroken).withTimeout(750),
+                Commands.waitMillis(400), // increase if ball doent reach
+                new ConditionalCommand(
+                        TransferFactory.runKickerCycle(transfer),
+                        Commands.none(),
+                        transfer::doesTransferContainSingleBall
+                ),
+                Commands.waitUntil(() -> !transfer.isThirdBeamBroken()).withTimeout(500),
+                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0.0)
+        );
+    }
+
+    private static Command indexNextBall(Intake intake, Transfer transfer) {
+        return Commands.sequence(
+                Commands.waitMillis(100),
+                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1),
+                Commands.waitUntil(transfer::isThirdBeamBroken).withTimeout(1000),
+                Commands.waitMillis(400), // increase if ball doent reach
+                IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0.0),
+                Commands.waitMillis(50)
+        );
+    }
+
     public static Command auomaticallyParkAndLiftCommand(Drivetrain drivetrain, Lift lift) {
         return Commands.runEnd(
                 () -> {
@@ -104,29 +165,4 @@ public static Command auomaticallyParkAndLiftCommand(Drivetrain drivetrain, Lift
                 drivetrain, lift
         ).until(lift::isAtSetpoint);
     }
-
-    private static Command shootOneBallSensoredCommand(Intake intake, Transfer transfer) {
-        return Commands.sequence(
-                IntakeFactory.openLoopSetpointCommand(intake, () -> 1.0),
-                Commands.waitMillis(100),
-                Commands.waitUntil(transfer::isThirdBeamBroken).withTimeout(750),
-                new ConditionalCommand(
-                        TransferFactory.runKickerCycle(transfer),
-                        Commands.none(),
-                        transfer::doesTransferContainSingleBall
-                ),
-                Commands.waitUntil(() -> !transfer.isThirdBeamBroken()).withTimeout(500),
-                IntakeFactory.openLoopSetpointCommand(intake, () -> 0.0)
-        );
-    }
-
-    private static Command indexBallsSensoredCommand(Intake intake, Transfer transfer) {
-        return Commands.sequence(
-                Commands.waitMillis(250),
-                IntakeFactory.openLoopSetpointCommand(intake, () -> 0.3),
-                Commands.waitUntil(transfer::isThirdBeamBroken).withTimeout(2000),
-                IntakeFactory.openLoopSetpointCommand(intake, () -> 0.0),
-                Commands.waitMillis(100)
-        );
-    }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/TransferFactory.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/TransferFactory.java
index 9c8e2bc..9a7e41f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/TransferFactory.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/command_factories/TransferFactory.java
@@ -14,10 +14,12 @@ public static Command runKickerCycle(Transfer transfer) {
         return Commands.sequence(
                 Commands.runOnce(() -> {
                     transfer.setKickerPosition(TransferConstants.kickerFeedPosition);
+                    transfer.setKickerEngaged(true);
                 }),
-                new WaitCommand(700),
+                new WaitCommand(500),
                 new InstantCommand(() -> transfer.setKickerPosition(TransferConstants.kickerIdlePosition)),
-                new WaitCommand(700)
+                new WaitCommand(500),
+                new InstantCommand(() -> transfer.setKickerEngaged(false))
         );
     }
 
@@ -26,8 +28,11 @@ public static Command engageBlocker(Transfer transfer, DoubleSupplier setpoint)
                 Commands.runOnce(() -> {
                     double position = setpoint.getAsDouble();
                     transfer.setBlockerPosition(position);
+
+                    if(position == TransferConstants.blockerIdlePosition) transfer.setBlockerEngaged(true);
+                    if(position == TransferConstants.blockerAllowPosition) transfer.setBlockerEngaged(false);
                 }),
-                new WaitCommand(1000)
+                new WaitCommand(500)
         );
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java
index 1b167ac..8a1614e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/DrivetrainConstants.java
@@ -18,7 +18,7 @@ public class DrivetrainConstants {
     public static final double kMaximumRotationRadiansPerSecond = 2 * Math.PI;
 
     public static final Pose kCloseGoalStartingPoseBlue = new Pose(24.250, 130.250, Math.toRadians(144));
-    public static final Pose kFarStartingPoseBlue = new Pose(56.000, 8.75, Math.toRadians(90));
+    public static final Pose kFarStartingPoseBlue = new Pose(56.000, 8, Math.toRadians(90));
 
     public static final Pose kTopLeftParkingPoseBlue = new Pose(98, 26.5, Math.toRadians(315));
     public static final Pose kTopRightParkingPoseBlue = new Pose(98, 40, Math.toRadians(225));
@@ -32,18 +32,20 @@ public class DrivetrainConstants {
 
     public static final Pose kAutoCloseShootingPositionBlue = new Pose(56, 84, Math.toRadians(180));
     public static final Pose kAutoClosePickupOnePositionBlue = new Pose(15, 86, Math.toRadians(180));
-    public static final Pose kAutoClosePickupTwoControlPositionBlue = new Pose(59, 58, Math.toRadians(180));
-    public static final Pose kAutoClosePickupTwoPositionBlue = new Pose(17, 60, Math.toRadians(180));
-    public static final Pose kAutoClosePickupThreeControlPositionBlue = new Pose(62, 31, Math.toRadians(180));
-    public static final Pose kAutoClosePickupThreePositionBlue = new Pose(15, 36, Math.toRadians(180));
+    public static final Pose kAutoClosePickupTwoReadyPositionBlue = new Pose(43.5, 60, Math.toRadians(180));
+    public static final Pose kAutoClosePickupTwoPositionBlue = new Pose(12, 60, Math.toRadians(180));
+    public static final Pose kAutoClosePickupThreeReadyPositionBlue = new Pose(43, 36, Math.toRadians(180));
+    public static final Pose kAutoClosePickupThreePositionBlue = new Pose(13, 36, Math.toRadians(180));
     public static final Pose kAutoCloseParkingPositionBlue = new Pose(20, 100, Math.toRadians(90));
 
+    public static final Pose kAutoClosePrepareGateBlue = new Pose(26, 71, Math.toRadians(180));
+    public static final Pose kAutoCloseGateBlue = new Pose(15, 71, Math.toRadians(180));
+
     public static final Pose kAutoFarShootingPositionBlue = new Pose(59.6, 18.3, Math.toRadians(180));
-    public static final Pose kAutoFarPickupOnePositionBlue = new Pose(15.2, 35.9, Math.toRadians(180));
-    public static final Pose kAutoFarPickupOneControlPositionBlue = new Pose(64, 37.5, Math.toRadians(180));
-    public static final Pose kAutoFarPreparePickupTwoPositionBlue = new Pose(17.75, 16, Math.toRadians(200));
-    public static final Pose kAutoFarPreparePickupTwoControlPositionBlue = new Pose(45.5, 35, Math.toRadians(200));
-    public static final Pose kAutoFarPickupTwoPositionBlue = new Pose(9, 10.5, Math.toRadians(200));
+    public static final Pose kAutoFarPickupOnePositionBlue = new Pose(11, 35.5, Math.toRadians(180));
+    public static final Pose kAutoFarPickupOneReadyPositionBlue = new Pose(46, 35.5, Math.toRadians(180));
+    public static final Pose kAutoFarPickupTwoReadyPositionBlue = new Pose(22, 10, Math.toRadians(180));
+    public static final Pose kAutoFarPickupTwoPositionBlue = new Pose(10.2, 10, Math.toRadians(180));
     public static final Pose kAutoFarParkingPositionBlue = new Pose(40, 20, Math.toRadians(90));
 
     public static Pose decideToFlipPose(GlobalConstants.AllianceColor alliance, Pose poseToPotentiallyFlip) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java
index 03034ee..64abfd6 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/IntakeConstants.java
@@ -9,17 +9,27 @@ public class IntakeConstants {
     public static final String frontIntakeMotorID = "frontIntakeMotor";
 
     public static final double intakeMaximumRPM = 435;
-    public static final double intakeMotorCPR = 384.5;
+    public static final double kRearIntakeMotorCPR = 384.5;
+    public static final double kFrontIntakeMotorCPR = 537.7;
 
     public static final double frontIntakeRatio = (96 / 24.00);
     public static final double rearIntakeRatio = 1;
 
-    public static double kP = 0.001;
-    public static double kI = 0.0;
-    public static double kD = 0.0;
-    public static double kF = 0.0;
+    public static double fP = 0.001;
+    public static double fI = 0.0;
+    public static double fD = 0.0;
+    public static double fF = 0.0;
 
-    public static double kS = 0.0;
-    public static double kV = 0.0;
-    public static double kA = 0.0;
+    public static double fS = 0.0;
+    public static double fV = 0.0;
+    public static double fA = 0.0;
+
+    public static double sP = 0.001;
+    public static double sI = 0.0;
+    public static double sD = 0.0;
+    public static double sF = 0.0;
+
+    public static double sS = 0.0;
+    public static double sV = 0.0;
+    public static double sA = 0.0;
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/LiftConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/LiftConstants.java
index 1091b67..911abf1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/LiftConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/LiftConstants.java
@@ -13,8 +13,12 @@ public class LiftConstants {
     public static final double kD = 0.00;
     public static final double kF = 0.00;
 
+    public static final double kS = 0.00; // This system has insane torque it might actually be needed
+    public static final double kV = 0.00;
+    public static final double kA = 0.00;
+
     public static final double liftServoUpPosition = 0.0;
-    public static final double liftServoDownPosition = 0.0;
+    public static final double liftServoDownPosition = 36.0;
 
 
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java
index e7a6ab2..2667dd5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TransferConstants.java
@@ -5,7 +5,6 @@
 @Configurable
 public class TransferConstants {
     public static final String kSubsystemName = "Transfer ";
-
     public static final String blockerServoID = "blockServo";
     public static final String kickerServoID = "kickerServo";
 
@@ -20,6 +19,6 @@ public class TransferConstants {
     public static final double blockerAllowPosition = 0;
     public static final double kickerFeedPosition = 0.1;
 
-    public static final double kFirstColorSensorDistanceThreshold = 2.00; // In inches
-    public static final double kSensorDebounceTime = 0.35;
+    public static final double kFirstColorSensorDistanceThreshold = 1.8; // In inches
+    public static final double kSensorDebounceTime = 0.6;
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java
index c4e7e1b..9e2cfb1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TurretConstants.java
@@ -15,7 +15,7 @@ public class TurretConstants {
 
     public static double pP = 2.5;
     public static double pI = 0.0;
-    public static double pD = 0.1;
+    public static double pD = 0.125;
     public static double pF = 0.093;
 
     public static double tuningSetpoint = 0;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedropathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedropathing/Tuning.java
index 5a472c6..fe4e21a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedropathing/Tuning.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedropathing/Tuning.java
@@ -1411,7 +1411,7 @@ public void loop() {
  * @author Lazar - 19234
  * @version 1.1, 5/19/2025
  */
-public class Drawing {
+class Drawing {
     public static final double ROBOT_RADIUS = 9; // woah
     private static final FieldManager panelsField = PanelsField.INSTANCE.getField();
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
index 8378ab1..41d67c8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
@@ -22,7 +22,6 @@
 import org.firstinspires.ftc.teamcode.constants.DrivetrainConstants;
 import org.firstinspires.ftc.teamcode.constants.GlobalConstants;
 import org.firstinspires.ftc.teamcode.pedropathing.Constants;
-import org.firstinspires.ftc.teamcode.pedropathing.Drawing;
 
 import java.util.List;
 
@@ -71,7 +70,7 @@ public Drivetrain(HardwareMap hMap, TelemetryManager telemetryM) {
         poseHistory = follower.getPoseHistory();
 
         initializeImu(hMap);
-        drawCurrent();
+        //drawCurrent();
     }
 
     public void initializeImu(HardwareMap hardwareMap) {
@@ -94,7 +93,7 @@ public void periodic() {
             hub.clearBulkCache();
         }
 
-        drawCurrentAndHistory();
+        //drawCurrentAndHistory();
     }
 
     public void updateWithVision(Pose2d estimatedVisionPose) {
@@ -141,6 +140,16 @@ public double getDistanceToPose3D(Pose targetPose, double targetHeight, double t
         return Math.sqrt(dx * dx + dy * dy + dz * dz);
     }
 
+    public boolean isRobotinCloseZone() {
+        return getDistanceToPose3D(
+                GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? GlobalConstants.kBlueGoalPose : GlobalConstants.kRedGoalPose,
+                38,
+                12
+        ) < 120;
+    }
+
+    /*
+
     public static void drawCurrent() {
         try {
             Drawing.drawRobot(follower.getPose());
@@ -155,6 +164,8 @@ public static void drawCurrentAndHistory() {
         drawCurrent();
     }
 
+     */
+
     public PathBuilder getPathBuilder() {
         return new PathBuilder(follower);
     }
@@ -208,7 +219,7 @@ public void toggleRobotFieldCentric() {
     }
 
     public void manualResetPoseForAlliance() {
-        resetPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? new Pose(8.75, 7.5, 180).mirror() : new Pose(8.75, 7.5, 180));
+        resetPose(GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? new Pose(8.75, 7.5, 0).mirror() : new Pose(8.75, 7.5, 0));
     }
 
     public boolean isFollowingTrajectory() {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
index c2d905f..25ed51d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
@@ -18,8 +18,11 @@ public class Intake extends SubsystemBase {
     private DcMotorEx frontIntakeMotor;
     private DcMotorEx rearIntakeMotor;
 
-    private PIDFController velocityPIDFController;
-    private SimpleMotorFeedforward velocityFeedforward;
+    private PIDFController firstRollerVelocityController;
+    private SimpleMotorFeedforward firstRollerVelocityFeedForward;
+
+    private PIDFController secondRollerVelocityController;
+    private SimpleMotorFeedforward secondRollerVelocityFeedForward;
 
     @IgnoreConfigurable
     static TelemetryManager telemetryM;
@@ -31,11 +34,17 @@ public Intake(HardwareMap hMap, TelemetryManager telemetryM) {
         frontIntakeMotor.setDirection(DcMotorSimple.Direction.REVERSE);
         rearIntakeMotor.setDirection(DcMotorSimple.Direction.REVERSE);
 
+        frontIntakeMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rearIntakeMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
         frontIntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         rearIntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-        velocityPIDFController = new PIDFController(IntakeConstants.kP, IntakeConstants.kI, IntakeConstants.kD, IntakeConstants.kF);
-        velocityFeedforward = new SimpleMotorFeedforward(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA);
+        firstRollerVelocityController = new PIDFController(IntakeConstants.fP, IntakeConstants.fI, IntakeConstants.fD, IntakeConstants.fF);
+        firstRollerVelocityFeedForward = new SimpleMotorFeedforward(IntakeConstants.fS, IntakeConstants.fV, IntakeConstants.fA);
+
+        secondRollerVelocityController = new PIDFController(IntakeConstants.sP, IntakeConstants.sI, IntakeConstants.sD, IntakeConstants.sF);
+        secondRollerVelocityFeedForward = new SimpleMotorFeedforward(IntakeConstants.sS, IntakeConstants.sV, IntakeConstants.sA);
 
         this.telemetryM = telemetryM;
     }
@@ -48,18 +57,30 @@ public void periodic() {
         telemetryM.addData("Rear " + IntakeConstants.kSubsystemName + "Current Open Loop", rearIntakeMotor.getPower());
     }
 
-    public void setVelocitySetpoint(double targetRPM) {
-        telemetryM.addData(IntakeConstants.kSubsystemName + "Setpoint Velocity", targetRPM);
-        telemetryM.addData(IntakeConstants.kSubsystemName + "Velocity Error", velocityPIDFController.getPositionError());
-        telemetryM.addData(IntakeConstants.kSubsystemName + "At Setpoint?", velocityPIDFController.atSetPoint());
+    public void setFrontVelocitySetpoint(double targetRPM) {
+        telemetryM.addData("Front " + IntakeConstants.kSubsystemName + "Setpoint Velocity", targetRPM);
+        telemetryM.addData("Front " + IntakeConstants.kSubsystemName + "Velocity Error", firstRollerVelocityController.getPositionError());
+        telemetryM.addData("Front " + IntakeConstants.kSubsystemName + "At Setpoint?", firstRollerVelocityController.atSetPoint());
+
+        if(GlobalConstants.kTuningMode) {
+            firstRollerVelocityController.setPIDF(IntakeConstants.fP, IntakeConstants.fI, IntakeConstants.fD, IntakeConstants.fF);
+            firstRollerVelocityFeedForward.setCoefficient(IntakeConstants.fS, IntakeConstants.fV, IntakeConstants.fA);
+        }
+
+        frontIntakeMotor.setPower(firstRollerVelocityController.calculate(getFrontVelocity(), targetRPM) + firstRollerVelocityFeedForward.calculate(targetRPM));
+    }
+
+    public void setRearVelocitySetpoint(double targetRPM) {
+        telemetryM.addData("Rear " + IntakeConstants.kSubsystemName + "Setpoint Velocity", targetRPM);
+        telemetryM.addData("Rear " + IntakeConstants.kSubsystemName + "Velocity Error", firstRollerVelocityController.getPositionError());
+        telemetryM.addData("Rear " + IntakeConstants.kSubsystemName + "At Setpoint?", firstRollerVelocityController.atSetPoint());
 
         if(GlobalConstants.kTuningMode) {
-            velocityPIDFController.setPIDF(IntakeConstants.kP, IntakeConstants.kI, IntakeConstants.kD, IntakeConstants.kF);
-            velocityFeedforward.setCoefficient(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA);
+            secondRollerVelocityController.setPIDF(IntakeConstants.sP, IntakeConstants.sI, IntakeConstants.sD, IntakeConstants.sF);
+            secondRollerVelocityFeedForward.setCoefficient(IntakeConstants.sS, IntakeConstants.sV, IntakeConstants.sA);
         }
 
-        frontIntakeMotor.setPower(velocityPIDFController.calculate(getFrontVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM));
-        rearIntakeMotor.setPower(velocityPIDFController.calculate(getRearVelocity(), targetRPM) + velocityFeedforward.calculate(targetRPM));
+        rearIntakeMotor.setPower(secondRollerVelocityController.calculate(getFrontVelocity(), targetRPM) + secondRollerVelocityFeedForward.calculate(targetRPM));
     }
 
     public void setFrontMotorOpenLoop(double speed) {
@@ -79,17 +100,17 @@ public void setOpenLoopSetpoint(double speed) {
     }
 
     public void setEqualOpenLoopSetpoint(double rearSpeed) {
-        double frontSpeed = rearSpeed;
+        double frontSpeed = rearSpeed / IntakeConstants.frontIntakeRatio;
 
         setFrontMotorOpenLoop(frontSpeed);
         setRearMotorOpenLoop(rearSpeed);
     }
 
     public double getFrontVelocity() {
-        return ((frontIntakeMotor.getVelocity() / IntakeConstants.intakeMotorCPR) * IntakeConstants.frontIntakeRatio) * 60;
+        return ((frontIntakeMotor.getVelocity() / IntakeConstants.kFrontIntakeMotorCPR) * IntakeConstants.frontIntakeRatio) * 60;
     }
 
     public double getRearVelocity() {
-        return ((rearIntakeMotor.getVelocity() / IntakeConstants.intakeMotorCPR) * IntakeConstants.rearIntakeRatio) * 60;
+        return ((rearIntakeMotor.getVelocity() / IntakeConstants.kRearIntakeMotorCPR) * IntakeConstants.rearIntakeRatio) * 60;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java
index b22c5d9..d2fe557 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java
@@ -33,6 +33,10 @@ public class LED extends SubsystemBase {
     private int patternStep = 0;
     private long intervalMs = 200;
 
+    private int blinkCount = 0;
+    private int targetBlinkCount = 0;
+    private LEDConstants.ColorValue finalColor = LEDConstants.ColorValue.ORANGE;
+
     @IgnoreConfigurable
     static LightsManager lightsManager;
 
@@ -44,7 +48,8 @@ public enum LedState {
         OFF,
         SOLID,
         BLINK_SIMPLE,
-        BLINK_PATTERN
+        BLINK_PATTERN,
+        COUNTED_BLINK
     }
 
     public LED(HardwareMap hMap, TelemetryManager telemetryM, LightsManager lightsManager) {
@@ -52,7 +57,7 @@ public LED(HardwareMap hMap, TelemetryManager telemetryM, LightsManager lightsMa
         this.telemetryM = telemetryM;
         this.lightsManager = lightsManager;
 
-        this.ledIndicator = new RGBIndicator("Colored Light #2");
+        this.ledIndicator = new RGBIndicator("Colored Light #1");
         lightsManager.initLights(ledIndicator);
     }
 
@@ -95,6 +100,19 @@ public void setDefaultSimpleBlink(LEDConstants.ColorValue color, long intervalMs
         setSimpleBlink(color, LEDConstants.ColorValue.OFF, intervalMs);
     }
 
+    public void setCountedBlink(LEDConstants.ColorValue colorA, LEDConstants.ColorValue colorB, long intervalMs, int numberOfBlinks, LEDConstants.ColorValue finalColor) {
+        mode = LedState.COUNTED_BLINK;
+
+        primaryColorA = colorA;
+        primaryColorB = colorB;
+        this.finalColor = finalColor;
+
+        blinkCount = 0;
+        targetBlinkCount = numberOfBlinks;
+
+        resetTimer(intervalMs);
+    }
+
 
     public void setComplexBlink(LEDConstants.ColorValue allianceColor, LEDConstants.ColorValue accentColor, LEDConstants.ColorValue idleColor, long intervalMs) {
         mode = LedState.BLINK_PATTERN;
@@ -120,6 +138,7 @@ public void update() {
             case SOLID:
                 setColor(primaryColorA);
                 break;
+
             case BLINK_SIMPLE:
                 if (ledTimer.done()) {
                     ledTimer.start();
@@ -128,6 +147,32 @@ public void update() {
 
                 setColor(patternStep == 0 ? primaryColorA : primaryColorB);
                 break;
+
+            case COUNTED_BLINK:
+                if (blinkCount >= targetBlinkCount) {
+                    // Transition to solid final color
+                    mode = LedState.SOLID;
+                    primaryColorA = finalColor;
+                    setColor(finalColor);
+
+                    if (ledTimer.isTimerOn()) {
+                        ledTimer.pause();
+                    }
+                } else {
+                    if (ledTimer.done()) {
+                        ledTimer.start();
+                        patternStep ^= 1;
+
+                        // Count complete blinks (when transitioning from colorB back to colorA)
+                        if (patternStep == 0) {
+                            blinkCount++;
+                        }
+                    }
+
+                    setColor(patternStep == 0 ? primaryColorA : primaryColorB);
+                }
+                break;
+
             case BLINK_PATTERN:
                 if (ledTimer.done()) {
                     ledTimer.start();
@@ -141,15 +186,19 @@ public void update() {
                     case 3: setColor(secondaryColorB); break;
                 }
                 break;
+
             case OFF:
                 if (ledTimer.isTimerOn()) {
                     ledTimer.pause();
-                    ledTimer = new Timing.Timer(0, TimeUnit.MILLISECONDS); // Reset
+                    ledTimer = new Timing.Timer(0, TimeUnit.MILLISECONDS);
                 }
 
                 patternStep = 0;
+                blinkCount = 0;
+                targetBlinkCount = 0;
                 setColor(LEDConstants.ColorValue.OFF);
                 break;
+
             default:
                 setColor(LEDConstants.ColorValue.OFF);
                 break;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
index 1cde760..5ea02fe 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
@@ -11,7 +11,10 @@
 import org.firstinspires.ftc.library.controller.PIDFController;
 import org.firstinspires.ftc.library.hardware.AnalogEncoder;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.constants.GlobalConstants;
 import org.firstinspires.ftc.teamcode.constants.LiftConstants;
+import org.firstinspires.ftc.teamcode.constants.ShooterConstants;
+import org.firstinspires.ftc.teamcode.utilities.tuning.pidf.SimpleMotorFeedforward;
 
 import lombok.Getter;
 import lombok.Setter;
@@ -22,6 +25,7 @@ public class Lift  extends SubsystemBase {
     private final RevTouchSensor homingSwitch;
 
     private PIDFController positonController;
+    private SimpleMotorFeedforward feedforward;
 
     @Getter
     @Setter
@@ -76,6 +80,13 @@ public void onInitialization() {
     }
 
     public void setPosition(double positionRotations) {
+        telemetryM.addData(LiftConstants.kSubsystemName + "Setpoint Position", positionRotations);
+
+        if(GlobalConstants.kTuningMode) {
+            //positonController.setPIDF(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD, ShooterConstants.kF);
+            //feed.setCoefficient(ShooterConstants.kS, ShooterConstants.kV, ShooterConstants.kA);
+        }
+
         double motorPower = positonController.calculate(getRelativePosition(), positionRotations);
         setPower(motorPower);
     }
@@ -103,7 +114,7 @@ private void updateRelativePosition() {
     }
 
     public void resetToZero() {
-        setPower(-0.5);
+        setPower(-1);
         isMechanismHomed = false;
     }
 
@@ -124,7 +135,7 @@ public double getAbsolutePosition() {
     }
 
     public boolean isHomingSwitchTriggered() {
-        return homingSwitch.isPressed();
+        return !homingSwitch.isPressed();
     }
 
     public void resetRelativePosition() {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
index 9156c3b..4fea5a5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java
@@ -42,24 +42,24 @@ public Shooter(HardwareMap hardwareMap, TelemetryManager telemetryM) {
         hoodServo = hardwareMap.get(Servo.class, ShooterConstants.hoodServoID);
 
         shooterInteroperableMap = new InterpLUT()
-            .add(20, 3150)
+            .add(18, 3150)
             .add(47.259, 3600)
             .add(63.864, 3800)
             .add(80.426, 4100)
             .add(96.217, 4300)
             .add(132.282, 4700)
             .add(148.92, 5000)
-            .add(200, 5600)
+            .add(300, 5600)
             .createLUT();
 
         hoodInteroperableMap = new InterpLUT()
-            .add(20, 1)
+            .add(18, 1)
             .add(47.259, 0.685)
             .add(63.864, 0.45)
             .add(80.426, 0.3)
             .add(96.217, 0.2)
             .add(132.282, 0.0)
-            .add(200, 0.0)
+            .add(300, 0.0)
             .createLUT();
 
         velocityPIDFController = new PIDFController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD, ShooterConstants.kF);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
index d3375f6..a550a12 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java
@@ -82,16 +82,16 @@ public void periodic() {
         boolean secondDebounced = secondSensorDebouncer.calculate(secondRaw);
         boolean thirdDebounced = thirdSensorDebouncer.calculate(thirdRaw);
 
-        trackBallMovement(firstDebounced, secondDebounced, thirdDebounced);
+        setCurrentNumberOfBalls((firstDebounced ? 1 : 0) + (secondDebounced ? 1 : 0) + (thirdDebounced ? 1 : 0));
+        //trackBallMovement(firstDebounced, secondDebounced, thirdDebounced);
 
         prevFirstDebounced = firstDebounced;
         prevSecondDebounced = secondDebounced;
         prevThirdDebounced = thirdDebounced;
 
-        telemetryM.addData(TransferConstants.kSubsystemName + "Ball Count", currentNumberOfBalls);
-        telemetryM.addData(TransferConstants.kSubsystemName + "fCS Distance Reading", firstCSDistance());
-        telemetryM.addData(TransferConstants.kSubsystemName + "sBB Status",  isSecondBeamBroken());
-        telemetryM.addData(TransferConstants.kSubsystemName + "tBB Status", isThirdBeamBroken());
+        telemetryM.addData(TransferConstants.kSubsystemName + "fCS Raw Distance Reading", firstCSDistance());
+        telemetryM.addData(TransferConstants.kSubsystemName + "Ball Count", (firstDebounced ? 1 : 0) + (secondDebounced ? 1 : 0) + (thirdDebounced ? 1 : 0));
+        telemetryM.addData(TransferConstants.kSubsystemName + "Debounced States", String.format("F:%b S:%b T:%b", firstDebounced, secondDebounced, thirdDebounced));
     }
 
     public void onInitialization(boolean initKicker, boolean initBlocker) {
@@ -109,13 +109,12 @@ public void onInitialization(boolean initKicker, boolean initBlocker) {
         boolean secondBroken = isSecondBeamBroken();
         boolean thirdBroken = isThirdBeamBroken();
 
-        setCurrentNumberOfBalls((firstBroken ? 1 : 0) + (secondBroken ? 1 : 0) + (thirdBroken ? 1 : 0));
+        currentNumberOfBalls = 0;
 
         firstSensorDebouncer.reset(firstBroken);
         secondSensorDebouncer.reset(secondBroken);
         thirdSensorDebouncer.reset(thirdBroken);
 
-        // Initialize previous states
         prevFirstDebounced = firstBroken;
         prevSecondDebounced = secondBroken;
         prevThirdDebounced = thirdBroken;
@@ -135,12 +134,10 @@ private void trackBallMovement(boolean first, boolean second, boolean third) {
 
     private void onBallEntered() {
         currentNumberOfBalls = Math.min(currentNumberOfBalls + 1, 3);
-        telemetryM.addData(TransferConstants.kSubsystemName + "Event", "Ball Entered");
     }
 
     private void onBallExited() {
         currentNumberOfBalls = Math.max(currentNumberOfBalls - 1, 0);
-        telemetryM.addData(TransferConstants.kSubsystemName + "Event", "Ball Exited");
     }
 
     @SuppressLint("DefaultLocale")
@@ -175,6 +172,9 @@ public boolean doesTransferContainAnyBalls() {
         return currentNumberOfBalls > 0;
     }
 
+    public boolean doesTransferContainAllBalls() {
+        return currentNumberOfBalls == 3;
+    }
 
     private double firstCSDistance() {
         return firstColorSensor.getDistance(DistanceUnit.INCH);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
index a9363cf..2c53899 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
@@ -72,7 +72,7 @@ public void setPosition(double radians) {
         }
 
         primaryPositionController.setSetPoint(radians);
-        turretMotor.setPower(MathUtility.clamp(primaryPositionController.calculate(getCurrentPosition(), radians), -0.55, 0.55));
+        turretMotor.setPower(MathUtility.clamp(primaryPositionController.calculate(getCurrentPosition(), radians), -0.45, 0.45));
     }
 
     public double computeAngle(Pose2d robotPose, Pose targetPose, double turretOffsetX, double turretOffsetY) {
@@ -91,7 +91,7 @@ public double computeAngle(Pose2d robotPose, Pose targetPose, double turretOffse
         double normalizedAngle = AngleUnit.normalizeRadians(desiredTurretAngle);
 
         telemetryM.addData(TurretConstants.kSubsystemName + "Computed Desired Angle to Goal", normalizedAngle);
-        return MathUtility.clamp(normalizedAngle, -Math.PI / 2, Math.PI / 2);
+        return MathUtility.clamp(normalizedAngle, -Math.PI / 2, 2 * Math.PI / 3);
     }
 
     public void setManualPower(double speed) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/SavedConfiguration.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/SavedConfiguration.java
index 5818ea7..01c55a6 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/SavedConfiguration.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/SavedConfiguration.java
@@ -14,7 +14,7 @@ private SavedConfiguration() {}
     public static GlobalConstants.AllianceColor selectedAlliance = GlobalConstants.AllianceColor.BLUE;
     public static Pose pathEndPose = new Pose(8.75, 7.5, 0);
 
-    public static Pose finalDrivetrainPose = GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? new Pose(8.75, 7.5, 0).mirror() : new Pose(8.75, 7.5, 0);
+    public static Pose finalDrivetrainPose = GlobalConstants.getCurrentAllianceColor() == GlobalConstants.AllianceColor.BLUE ? new Pose(8.75, 7.5, 180).mirror() : new Pose(8.75, 7.5, 180);
     public static double finalDrivetrainVelocity = 0.0;
     public static double savedTurretPosition = 0.0;