Last commit of 23344...

cowtown end
match 69
2026-06-12 22:52:51 -04:00 · 2026-06-11 15:55:56 -05:00 · 2026-06-10 10:43:02 -05:00 · 2026-06-09 22:48:11 -05:00 · 2026-06-09 17:44:13 -05:00 · 2026-06-09 09:29:32 -05:00
14 changed files with 785 additions and 205 deletions
--- a/README.md
+++ b/README.md
@@ -1,3 +1,5 @@
+Farewell, this is the last commit for the team 23344...to any who may use this repository, please feel free to contact us (keshavanandofficial@gmail.com), and we hope our code is of use to you and your team.
+
 ## NOTICE

 This repository contains the public FTC SDK for the DECODE (2025-2026) competition season.
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
@@ -18,6 +18,7 @@ import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
+import org.firstinspires.ftc.teamcode.tests.NewShooterTest;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;

@@ -226,6 +227,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                }
                break;
            case OPENGATE:
+                robot.setIntakePower(0);
                if (currentTime - timeStamp > openGateTime){
                    follower.followPath(openGate_shoot1, true);
                    pathState = PathState.DRIVE_SHOOT1;
@@ -274,7 +276,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                break;
            case DRIVE_PICKUP3:
                if (!follower.isBusy()){
-                    shooter.setFlywheelVelocity(2200);
+                    shooter.setFlywheelVelocity(2250);
                    robot.setHoodPos(0.75);
                    follower.followPath(drivePickup3_pickup3, intakePower, false);
                    pathState = PathState.PICKUP3;
@@ -369,6 +371,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                park.unpark();
            }

+            NewShooterTest.transferPower = -0.8;
+
            if (initializeRobot){
                //add obelisk read here
                shooter.setState(Shooter.ShooterState.READ_OBELISK);
@@ -390,7 +394,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
        if (isStopRequested()) return;

        while (opModeIsActive()){
-            robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
            park.unpark();

            shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto15Ball_Back.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto15Ball_Back.java
@@ -18,6 +18,7 @@ import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
+import org.firstinspires.ftc.teamcode.tests.NewShooterTest;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;

@@ -37,8 +38,8 @@ public class Auto15Ball_Back extends LinearOpMode {
    SpindexerTransferIntake spindexer;

    // Wait Times
-    public static double rapidWaitTime = 0.5;
-    public static double rapidShootTime = 0.8;
+    public static double rapidWaitTime = 0.7;
+    public static double rapidShootTime = 2.6;
    public static double loadPickupTime = 3;

    // Initialize path state machine
@@ -51,20 +52,20 @@ public class Auto15Ball_Back extends LinearOpMode {
    PathState pathState = PathState.WAIT_VELOCITY;

    // Poses
-    public static double startPoseX = 12, startPoseY = -64, startPoseH = 90;
+    public static double startPoseX = 16, startPoseY = -64, startPoseH = 90;
    public static double pickup3ControlX = 12, pickup3ControlY = -37;
    public static double pickup3X = 61, pickup3Y = -37, pickup3H = 0;
-    public static double shoot3X = 16, shoot3Y = -57, shoot3H = 0;
+    public static double shoot3X = 16, shoot3Y = -55, shoot3H = 0;
    public static double pickupLoadControlX = 21.23654066437572, pickupLoadControlY = -62.311626575028637;
    public static double pickupLoadX = 63, pickupLoadY = -63, pickupLoadH = 0;
    public static double shootLoadControlX = 21.23654066437572, shootLoadControlY = -62.311626575028637;
-    public static double shootLoadX = 16, shootLoadY = -57, shootLoadH = 0;
+    public static double shootLoadX = 16, shootLoadY = -55, shootLoadH = 0;
    public static double intakeGateControl1X = 51.9656357388316, intakeGateControl1Y = -65.506277205040073;
    public static double intakeGateControl2X = 60.13459335624285, intakeGateControl2Y = -67.300458190148911;
-    public static double intakeGateX = 59, intakeGateY = -12, intakeGateH = 90;
+    public static double intakeGateX = 63, intakeGateY = -35, intakeGateH = 90;
    public static double shootGateControlX = 53.8705612829324, shootGateControlY = -35.14501718213059;
-    public static double shootGateX = 16, shootGateY = -57, shootGateH = 0;
-    public static double leaveX = 40, leaveY = 14, leaveH = 0;
+    public static double shootGateX = 16, shootGateY = -55, shootGateH = 0;
+    public static double leaveX = 36, leaveY = -58, leaveH = 0;
    double[] xPoses = {startPoseX, pickup3ControlX, pickup3X, shoot3X,
            pickupLoadControlX, pickupLoadX, shootLoadControlX, shootLoadX,
            intakeGateControl1X, intakeGateControl2X, intakeGateX, shootGateControlX, shootGateX, leaveX};
@@ -146,29 +147,27 @@ public class Auto15Ball_Back extends LinearOpMode {
        double currentTime = (double) System.currentTimeMillis() / 1000;
        switch(pathState){
            case WAIT_VELOCITY:
-                spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
-                shooter.setFlywheelVelocity(2400);
-                robot.setHoodPos(0.64);
+                shooter.setFlywheelVelocity(3250);
+                robot.setHoodPos(0.65);
                if (flywheel.getSteady()){
                    startAuto++;
                }
-                if (startAuto > 5){
-                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                if (startAuto > 6){
+                    spindexer.startBackShooting();
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT0;
                }
+                timeStamp = currentTime;
                break;
            case WAIT_SHOOT0:
-                if (currentTime - timeStamp > rapidShootTime ||
-                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
-                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
+                if (currentTime - timeStamp > rapidShootTime){
                    follower.followPath(startPose_pickup3, false);
                    pathState = PathState.PICKUP3;
                }
                break;
            case PICKUP3:
                if (!follower.isBusy()){
-                    follower.followPath(pickup3_shoot3, false);
+                    follower.followPath(pickup3_shoot3, true);
                    pathState = PathState.DRIVE_SHOOT3;
                    timeStamp = currentTime;
                }
@@ -177,15 +176,13 @@ public class Auto15Ball_Back extends LinearOpMode {
                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT3;
-                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                    spindexer.startBackShooting();
                } else if (follower.isBusy()){
                    timeStamp = currentTime;
                }
                break;
            case WAIT_SHOOT3:
-                if (currentTime - timeStamp > rapidShootTime ||
-                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
-                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
+                if (currentTime - timeStamp > rapidShootTime){
                    follower.followPath(shoot3_pickupLoad, false);
                    pathState = PathState.PICKUP_LOAD;
                    timeStamp = currentTime;
@@ -193,7 +190,7 @@ public class Auto15Ball_Back extends LinearOpMode {
                break;
            case PICKUP_LOAD:
                if (currentTime - timeStamp > loadPickupTime){
-                    follower.followPath(pickupLoad_shootLoad, false);
+                    follower.followPath(pickupLoad_shootLoad, true);
                    pathState = PathState.DRIVE_SHOOT_LOAD;
                    timeStamp = currentTime;
                }
@@ -202,15 +199,13 @@ public class Auto15Ball_Back extends LinearOpMode {
                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT_LOAD;
-                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                    spindexer.startBackShooting();
                } else if (follower.isBusy()){
                    timeStamp = currentTime;
                }
                break;
            case WAIT_SHOOT_LOAD:
-                if (currentTime - timeStamp > rapidShootTime ||
-                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
-                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
+                if (currentTime - timeStamp > rapidShootTime){
                    follower.followPath(shootLoad_intakeGate, false);
                    pathState = PathState.INTAKE_GATE;
                    timeStamp = currentTime;
@@ -218,7 +213,7 @@ public class Auto15Ball_Back extends LinearOpMode {
                break;
            case INTAKE_GATE:
                if (!follower.isBusy()){
-                    follower.followPath(intakeGate_shootGate, false);
+                    follower.followPath(intakeGate_shootGate, true);
                    pathState = PathState.DRIVE_SHOOT_GATE;
                    timeStamp = currentTime;
                }
@@ -227,19 +222,17 @@ public class Auto15Ball_Back extends LinearOpMode {
                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT_GATE;
-                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                    spindexer.startBackShooting();
                } else if (follower.isBusy()){
                    timeStamp = currentTime;
                }
                break;
            case WAIT_SHOOT_GATE:
-                if (currentTime - timeStamp > rapidShootTime ||
-                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
-                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
-                    follower.followPath(shootGate_intakeGate, false);
-                    pathState = PathState.INTAKE_GATE;
+                if (currentTime - timeStamp > rapidShootTime){
+//                    follower.followPath(shootGate_intakeGate, false);
+                    follower.followPath(shootGate_leave, true);
+                    pathState = PathState.LEAVE;
                    timeStamp = currentTime;
-                    // TODO: add logic for leave
                }
                break;
            case LEAVE:
@@ -285,14 +278,14 @@ public class Auto15Ball_Back extends LinearOpMode {
        while (opModeInInit()){

            park.unpark();
-            robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);

            follower.update();

            if (gamepad1.squareWasPressed()){
                robot.setSpinPos(ServoPositions.spindexer_A2);
-                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
-                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
+                robot.setTurretPos(Turret.neutralPosition);
            }

            if (gamepad1.crossWasPressed() && !initializeRobot){
@@ -320,6 +313,8 @@ public class Auto15Ball_Back extends LinearOpMode {
                park.unpark();
            }

+            NewShooterTest.transferPower = -0.6;
+
            TELE.addData("Red Alliance?", Color.redAlliance);
            TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot);
            TELE.addData("Start Pose", follower.getPose());
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java
@@ -14,10 +14,12 @@ import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
+import org.firstinspires.ftc.teamcode.tests.NewShooterTest;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;

@@ -38,13 +40,15 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
    double runtime = 0;

    // Wait Times
-    public static double rapidWaitTime = 0.4;
-    public static double rapidShootTime = 0.45;
-    public static double openGate1Time = 1.5;
-    public static double openGate2Time = 1.5;
+    public static double rapidWaitTimeShoot0 = 0.2;
+    public static double rapidWaitTimeShoot1 = 0.2;
+    public static double rapidWaitTimeShoot2 = 0.2;
+    public static double rapidWaitTimeShootGate = 0.4;
+    public static double rapidShootTime = 0.4;
+    public static double openGate1Time = 1.8;
+    public static double openGate2Time = 1;
    public static double openGateWaitTimeMax = 3;
-    public static double openGateWaitTimeMin = 1.75;
-    public static int maxLoopCycles = 3;
+    public static int maxLoopCycles = 4;

    // Initialize path state machine
    private enum PathState {
@@ -69,11 +73,12 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
    public static double openGate2ControlX = 45.9782359679267, openGate2ControlY = -15.106643757159245;
    public static double openGate2X = 57, openGate2Y = -8, openGate2H = 0;
    public static double shoot2ControlX = 57, shoot2ControlY = -8;
-    public static double shoot2X = 25, shoot2Y = 11, shoot2H = -30;
-    public static double intakeGateControlX = 61, intakeGateControlY = -11;
-    public static double intakeGateX = 61, intakeGateY = -11, intakeGateH = 20;
-    public static double shootGateControlX = 56, shootGateControlY = -10;
-    public static double shootGateX = 25, shootGateY = 11, shootGateH = -30;
+    public static double shoot2X = 16, shoot2Y = 4, shoot2H = -30;
+    public static double intakeGateControlX = 60, intakeGateControlY = -12;
+    public static double toGateX = 60, toGateY = -10;
+    public static double intakeGateX = 62, intakeGateY = -12, intakeGateH = 25;
+    public static double shootGateControlX = 40, shootGateControlY = -5;
+    public static double shootGateX = 16, shootGateY = 4, shootGateH = -30;
    public static double shootLeaveControlX = 56, shootLeaveControlY = -10;
    public static double shootLeaveX = 16, shootLeaveY = 36, shootLeaveH = -50;
    public static double leaveX = 45, leaveY = 10, leaveH = 0;
@@ -82,22 +87,22 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
            pickup1ControlX, pickup1X, openGate1ControlX, openGate1X, shoot1ControlX, shoot1X,
            pickup2ControlX, pickup2X, openGate2ControlX, openGate2X, shoot2ControlX, shoot2X,
            intakeGateControlX, intakeGateX, shootGateControlX, shootGateX,
-            shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX};
+            shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX, toGateX};
    double[] yPoses = {startPoseY, shoot0Y,
            pickup1ControlY, pickup1Y, openGate1ControlY, openGate1Y, shoot1ControlY, shoot1Y,
            pickup2ControlY, pickup2Y, openGate2ControlY, openGate2Y, shoot2ControlY, shoot2Y,
            intakeGateControlY, intakeGateY, shootGateControlY, shootGateY,
-            shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY};
+            shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY, toGateY};
    double[] headings = {startPoseH, shoot0H,
            0, pickup1H, 0, openGate1H, 0, shoot1H,
            0, pickup2H, 0, openGate2H, 0, shoot2H,
            0, intakeGateH, 0, shootGateH,
-            0, shootLeaveH, leaveH, awayFromGateH};
+            0, shootLeaveH, leaveH, awayFromGateH, 0};
    Pose startPose, shoot0,
            pickup1Control, pickup1, openGate1Control, openGate1, shoot1Control, shoot1,
            pickup2Control, pickup2, openGate2Control, openGate2, shoot2Control, shoot2,
            intakeGateControl, intakeGate, shootGateControl, shootGate,
-            shootLeaveControl, shootLeave, leave, awayFromGate;
+            shootLeaveControl, shootLeave, leave, awayFromGate, toGate;
    private void initializePoses(){
        startPose = new Pose(xPoses[0], yPoses[0], Math.toRadians(headings[0]));
        shoot0 = new Pose(xPoses[1], yPoses[1], Math.toRadians(headings[1]));
@@ -121,12 +126,13 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
        shootLeave = new Pose(xPoses[19], yPoses[19], Math.toRadians(headings[19]));
        leave = new Pose(xPoses[20], yPoses[20], Math.toRadians(headings[20]));
        awayFromGate = new Pose(xPoses[21], yPoses[21], Math.toRadians(headings[21]));
+        toGate = new Pose(xPoses[22], yPoses[22]);
    }

    //Building Paths
    PathChain startPose_shoot0, shoot0_pickup1, pickup1_openGate1, openGate1_shoot1,
-        shoot1_pickup2, pickup2_openGate2, openGate2_shoot2,
-        shoot2_intakeGate, intakeGate_shootGate, shootGate_intakeGate, intakeGate_shootLeave, shootGate_leave, intakeGate_awayFromGate;
+        shoot1_pickup2, pickup2_openGate2, openGate2_shoot2, shootGate_intakeGate,
+        shoot2_intakeGate, intakeGate_shootGate, intakeGate_shootLeave, shootGate_leave, intakeGate_awayFromGate;
    private void buildPaths(){
        startPose_shoot0 = follower.pathBuilder()
                .addPath(new BezierLine(startPose, shoot0))
@@ -159,27 +165,29 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                .build();

        openGate2_shoot2 = follower.pathBuilder()
-                .addPath(new BezierCurve(openGate2, shoot2Control, shoot2))
-                .setLinearHeadingInterpolation(openGate2.getHeading(), shoot2.getHeading())
+                .addPath(new BezierLine(openGate2, shoot2))
+                .setTangentHeadingInterpolation()
+                .setReversed()
                .build();

        shoot2_intakeGate = follower.pathBuilder()
-                .addPath(new BezierCurve(shoot2, intakeGateControl, intakeGate))
+                .addPath(new BezierCurve(shoot2, intakeGateControl, toGate))
                .setLinearHeadingInterpolation(shoot2.getHeading(), intakeGate.getHeading())
                .build();

        intakeGate_shootGate = follower.pathBuilder()
                .addPath(new BezierCurve(intakeGate, shootGateControl, shootGate))
-                .setLinearHeadingInterpolation(intakeGate.getHeading(), shootGate.getHeading())
+                .setTangentHeadingInterpolation()
+                .setReversed()
                .build();

        shootGate_intakeGate = follower.pathBuilder()
-                .addPath(new BezierCurve(shootGate, intakeGateControl, intakeGate))
+                .addPath(new BezierLine(shootGate, intakeGate))
                .setLinearHeadingInterpolation(shootGate.getHeading(), intakeGate.getHeading())
                .build();

        intakeGate_shootLeave = follower.pathBuilder()
-                .addPath(new BezierCurve(intakeGate, shootLeaveControl, shootLeave))
+                .addPath(new BezierCurve(intakeGate, pickup2Control, shootLeave))
                .setLinearHeadingInterpolation(intakeGate.getHeading(), shootLeave.getHeading())
                .build();

@@ -198,6 +206,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
    private boolean startAuto = true;
    private double timeStamp = 0;
    private int cycle = 0;
+    boolean toGateBool = false;
    private void pathStateMachine(){
        double currentTime = (double) System.currentTimeMillis() / 1000;
        switch(pathState){
@@ -209,11 +218,11 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                    follower.followPath(startPose_shoot0, false);
                    startAuto = false;
                }
-                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                if ((follower.atParametricEnd() || !follower.isBusy())  && currentTime - timeStamp > rapidWaitTimeShoot0){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT0;
                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
-                } else if (follower.isBusy()){
+                } else if (follower.isBusy() && !follower.atParametricEnd()){
                    timeStamp = currentTime;
                }
                break;
@@ -240,7 +249,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                }
                break;
            case DRIVE_SHOOT1:
-                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTimeShoot1){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT1;
                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -258,6 +267,8 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                break;
            case PICKUP2:
                if (!follower.isBusy()){
+                    shooter.setFlywheelVelocity(2500);
+                    robot.setHoodPos(0.6);
                    follower.followPath(pickup2_openGate2, false);
                    pathState = PathState.OPENGATE2;
                    timeStamp = currentTime;
@@ -271,7 +282,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                }
                break;
            case DRIVE_SHOOT2:
-                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTimeShoot2){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT2;
                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -283,20 +294,22 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                if (currentTime - timeStamp > rapidShootTime ||
                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
-                    follower.followPath(shoot2_intakeGate, false);
+                    follower.followPath(shoot2_intakeGate, true);
                    pathState = PathState.INTAKE_GATE;
                    timeStamp = currentTime;
+                    toGateBool = true;
                }
                break;
            case INTAKE_GATE:
-                if ((currentTime - timeStamp > openGateWaitTimeMax || (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()))
-                        && (currentTime - timeStamp > openGateWaitTimeMin)){
-                    if (getRuntime() - runtime > 27){
-                        follower.followPath(intakeGate_awayFromGate, true);
-                        pathState = PathState.WAIT_SHOOT_LEAVE;
-                    } else if (getRuntime() - runtime > 22 || cycle >= maxLoopCycles - 1){
+                if ((currentTime - timeStamp > openGateWaitTimeMax)){
+                    if (cycle == 0){
+                        openGateWaitTimeMax = openGateWaitTimeMax + 0.75;
+                    }
+                    if (cycle >= maxLoopCycles - 1){
                        follower.followPath(intakeGate_shootLeave, true);
                        pathState = PathState.DRIVE_SHOOT_LEAVE;
+                        shooter.setFlywheelVelocity(2300);
+                        robot.setHoodPos(0.68);
                    } else {
                        follower.followPath(intakeGate_shootGate, false);
                        pathState = PathState.DRIVE_SHOOT_GATE;
@@ -306,7 +319,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                // TODO: add logic to shoot gate
                break;
            case DRIVE_SHOOT_GATE:
-                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTimeShootGate){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT_GATE;
                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -323,14 +336,15 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                        follower.followPath(shootGate_leave, true);
                        pathState = PathState.WAIT_SHOOT_LEAVE;
                    } else {
-                        follower.followPath(shootGate_intakeGate, false);
+                        follower.followPath(shootGate_intakeGate, true);
                        pathState = PathState.INTAKE_GATE;
+                        toGateBool = true;
                    }
                    timeStamp = currentTime;
                }
                break;
            case DRIVE_SHOOT_LEAVE:
-                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTimeShootGate){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT_LEAVE;
                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
@@ -413,6 +427,8 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                park.unpark();
            }

+            NewShooterTest.transferPower = -0.8;
+
            TELE.addData("Red Alliance?", Color.redAlliance);
            TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot);
            TELE.addData("Start Pose", follower.getPose());
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
@@ -16,6 +16,7 @@ public class ServoPositions {
    public static double spindexer_A3 = 0.54;
    public static double spindexer_B1 = 0.73;
    public static double spindexer_B2 = 0.92;
+    public static double spindexer_StopShooting = 0.44;


    public static double spindexer_intakePos1 = 0.18; //0.13;
@@ -36,7 +37,7 @@ public class ServoPositions {

    public static double transferServo_out = 0.28;

-    public static double transferServo_in = 0.54;
+    public static double transferServo_in = 0.52;

    public static double hoodAuto = 0.27;

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
@@ -8,7 +8,9 @@ import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
 import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
@@ -30,12 +32,14 @@ public class TeleopV4 extends LinearOpMode {
    ParkTilter parkTilter;
    MeasuringLoopTimes loopTimes;

-    public static Pose relocalizePose = new Pose(56, 11, 0);
+    public static Pose relocalizePose = new Pose(54.7, 11.4, 0);
    public static Pose teleStart = new Pose(0,0,0);

    private boolean firstTickFull = true;
    private boolean intakeFull = true;
    private boolean shooting = false;
+    public static int flywheelOffset = 0;
+    public static double hoodOffset = 0;

    @Override
    public void runOpMode() throws InterruptedException {
@@ -85,12 +89,15 @@ public class TeleopV4 extends LinearOpMode {
            if (gamepad1.triangleWasPressed()){
                VelocityCommander.lockFront = true;
                VelocityCommander.lockBack = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
            } else if (gamepad1.squareWasPressed()){
                VelocityCommander.lockBack = true;
                VelocityCommander.lockFront = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK);
            } else if (gamepad1.circleWasPressed()){
                VelocityCommander.lockBack = false;
                VelocityCommander.lockFront = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
            }
            TELE.addLine("Initialization is done");
            TELE.addData("Starting Position", follower.getPose());
@@ -116,17 +123,20 @@ public class TeleopV4 extends LinearOpMode {

            if (gamepad1.crossWasPressed()){
                if (Color.redAlliance){
-                    relocalizePose = new Pose(57.5, 5, 0);
+                    relocalizePose = new Pose(54.4, 12, 0);
                } else {
-                    relocalizePose = new Pose(-57.5, 5, Math.toRadians(180));
+                    relocalizePose = new Pose(-54.4, 12, Math.toRadians(180));
                }
                follower.setPose(relocalizePose);
                sleep(500);
                gamepad1.rumble(100);
            }

+            if (!isStopRequested()){
                follower.update();
+            }
            Pose currentPose = follower.getPose();
+            teleStart = currentPose;

            if (gamepad1.dpadLeftWasPressed()){
                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
@@ -137,23 +147,29 @@ public class TeleopV4 extends LinearOpMode {
            }

            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
-                shooter.setFlywheelVelocity(2500);
-                robot.setHoodPos(0.6);
+                shooter.setFlywheelVelocity(2500 + flywheelOffset);
+                robot.setHoodPos(0.6 + hoodOffset);
            }

            if (gamepad1.triangleWasPressed()){
                VelocityCommander.lockFront = true;
                VelocityCommander.lockBack = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.INTAKE);
                TELE.addData("Front?:", true);
                TELE.addData("Back?:", false);
            } else if (gamepad1.squareWasPressed()){
                VelocityCommander.lockBack = true;
                VelocityCommander.lockFront = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK);
+                spindexerTransferIntake.setSortedIntakeMode(SpindexerTransferIntake.SortedIntakeStates.IDLE);
                TELE.addData("Front?:", false);
                TELE.addData("Back?:", true);
            } else if (gamepad1.circleWasPressed()){
                VelocityCommander.lockBack = false;
                VelocityCommander.lockFront = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.INTAKE);
                TELE.addData("Front?:", false);
                TELE.addData("Back?:", true);
            }
@@ -161,6 +177,20 @@ public class TeleopV4 extends LinearOpMode {
            shooter.update(robot.voltage.getVoltage());
            spindexerTransferIntake.update();

+            if (VelocityCommander.lockBack){
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+                if (gamepad1.leftBumperWasPressed()){
+                    spindexerTransferIntake.startBackShooting();
+                    firstTickFull = true;
+                }
+
+                if (spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE && firstTickFull){
+                    gamepad1.rumble(100);
+                    firstTickFull = false;
+                }
+                TELE.addData("Intake State", spindexerTransferIntake.getSortedIntakeStates());
+            } else {
+                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
                SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();

                if (gamepad1.leftBumperWasPressed() &&
@@ -181,16 +211,11 @@ public class TeleopV4 extends LinearOpMode {
                    shooting = false;
                }

-            if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
-                intakeFull = true;
-            } else {
-                intakeFull = false;
-                firstTickFull = true;
-            }
-
-            if (intakeFull && firstTickFull){
-                gamepad1.rumble(100);
+                if (SpindexerTransferIntake.intakeFull && firstTickFull){
+                    gamepad1.rumble(250);
                    firstTickFull = false;
+                } else if (!SpindexerTransferIntake.intakeFull){
+                    firstTickFull = true;
                }

                if (gamepad1.right_trigger > 0.5 &&
@@ -206,6 +231,7 @@ public class TeleopV4 extends LinearOpMode {
                    robot.setIntakePower(1);
                    robot.setTransferPower(-0.7);
                }
+            }

            if (gamepad2.leftBumperWasPressed()){
                limelightUsed = false;
@@ -219,16 +245,53 @@ public class TeleopV4 extends LinearOpMode {
                parkTilter.unpark();
            }

+            if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+            } else if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
+            }
+
+            if (gamepad2.dpadUpWasPressed()){
+                hoodOffset-=0.02;
+            } else if (gamepad2.dpadDownWasPressed()){
+                hoodOffset+=0.02;
+            }
+
+            if (gamepad2.dpadRightWasPressed()){
+                flywheelOffset+=50;
+            } else if (gamepad2.dpadLeftWasPressed()){
+                flywheelOffset-=50;
+            }
+
+            if (gamepad2.crossWasPressed()){
+                flywheelOffset = 0;
+                hoodOffset = 0;
+            }
+
            loopTimes.loop();
 //            TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
 //            TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
 //            TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
 //
-            TELE.addData("Distance From Goal", commander.getDistance());
+//            TELE.addData("Distance From Goal", commander.getDistance());
 //            TELE.addData("Hood Position", commander.getHoodPredicted());
 //            TELE.addData("Transfer Power", robot.transfer.getPower());
-            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
-            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+//            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+//            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+//            TELE.addData("Velocity 1", flywheel.getVelo1());
+//            TELE.addData("Velocity 2", flywheel.getVelo2());
+            TELE.addData("Flywheel Offset", flywheelOffset);
+            TELE.addData("Hood Offset", hoodOffset);
+
+//            TELE.addData("FR Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("FL Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("BL Drive", robot.backLeft.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("BR Drive", robot.backRight.getCurrent(CurrentUnit.AMPS));
+
+//            TELE.addData("Flywheel 1", robot.shooter1.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Flywheel 2", robot.shooter2.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Transfer", robot.transfer.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Intake", robot.intake.getCurrent(CurrentUnit.AMPS));
 //
 //            TELE.addData("Current Position", currentPose);
 //
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java
@@ -131,7 +131,7 @@ public class Hardware_Tester extends LinearOpMode {

            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM));
-            TELE.addData("REV Green", revColor.blue / (revColor.green + revColor.blue + revColor.red));
+            TELE.addData("REV Green", revColor.green / (revColor.green + revColor.blue + revColor.red));

            TELE.addData("Voltage Sensor", robot.voltage.getVoltage());

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
@@ -1,7 +1,5 @@
 package org.firstinspires.ftc.teamcode.tests;

-import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
-
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -13,8 +11,11 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
+import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
 import org.firstinspires.ftc.teamcode.utilsv2.*;

+import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
+
@TeleOp
@Config
 public class NewShooterTest extends LinearOpMode {
@@ -28,11 +29,14 @@ public class NewShooterTest extends LinearOpMode {
    Flywheel flywheel;
    VelocityCommander commander;
    ParkTilter parkTilter;
+    MeasuringLoopTimes loopTimes;

+    private boolean firstTickFull = true;
+    private boolean intakeFull = true;
+    private boolean shooting = false;
    public static int flywheelVelo = 0;
    public static double hoodPos = 0.5;
    public static double transferPower = -0.8;
-    public static boolean overrideTransferPower = false;

    @Override
    public void runOpMode() throws InterruptedException {
@@ -51,6 +55,7 @@ public class NewShooterTest extends LinearOpMode {
        follower.setStartingPose(new Pose(0,0,0));
        sleep(500);
        follower.setPose(new Pose(0,0,0));
+        follower.update();

        flywheel = new Flywheel(robot);
        turret = new Turret(robot);
@@ -58,6 +63,9 @@ public class NewShooterTest extends LinearOpMode {
        parkTilter = new ParkTilter(robot);
        parkTilter.unpark();

+        loopTimes = new MeasuringLoopTimes();
+        loopTimes.init();
+
        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
        shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
        shooter.setRedAlliance(Color.redAlliance);
@@ -69,9 +77,30 @@ public class NewShooterTest extends LinearOpMode {

        limelightUsed = true;

-        TELE.addLine("Initialization Complete");
+        TELE.addLine("Initialization is done");
        TELE.update();

+        while (opModeInInit()){
+            if (gamepad1.triangleWasPressed()){
+                VelocityCommander.lockFront = true;
+                VelocityCommander.lockBack = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+            } else if (gamepad1.squareWasPressed()){
+                VelocityCommander.lockBack = true;
+                VelocityCommander.lockFront = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK);
+                spindexerTransferIntake.setSortedIntakeMode(SpindexerTransferIntake.SortedIntakeStates.IDLE);
+            } else if (gamepad1.circleWasPressed()){
+                VelocityCommander.lockBack = false;
+                VelocityCommander.lockFront = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+            }
+            TELE.addLine("Initialization is done");
+            TELE.addData("Front?:", VelocityCommander.lockFront);
+            TELE.addData("Back?:", VelocityCommander.lockBack);
+            TELE.update();
+        }
+
        waitForStart();

        if (isStopRequested()) return;
@@ -97,6 +126,7 @@ public class NewShooterTest extends LinearOpMode {
            }

            follower.update();
+            Pose currentPose = follower.getPose();

            if (gamepad1.dpadLeftWasPressed()){
                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
@@ -111,17 +141,40 @@ public class NewShooterTest extends LinearOpMode {
                robot.setHoodPos(hoodPos);
            }

+            if (gamepad1.triangleWasPressed()){
+                VelocityCommander.lockFront = true;
+                VelocityCommander.lockBack = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                TELE.addData("Front?:", true);
+                TELE.addData("Back?:", false);
+            } else if (gamepad1.squareWasPressed()){
+                VelocityCommander.lockBack = true;
+                VelocityCommander.lockFront = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SPINDEXER_BACK);
+                TELE.addData("Front?:", false);
+                TELE.addData("Back?:", true);
+            } else if (gamepad1.circleWasPressed()){
+                VelocityCommander.lockBack = false;
+                VelocityCommander.lockFront = false;
+                spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                TELE.addData("Front?:", false);
+                TELE.addData("Back?:", true);
+            }

-//            shooter.setTurretPosition(turretPos);
            shooter.update(robot.voltage.getVoltage());
            spindexerTransferIntake.update();

-            if (gamepad2.leftBumperWasPressed()){
-                limelightUsed = false;
-            } else if (gamepad2.rightBumperWasPressed()){
-                limelightUsed = true;
+            if (VelocityCommander.lockBack){
+                if (gamepad1.leftBumperWasPressed() && spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE){
+                    spindexerTransferIntake.startBackShooting();
+                    firstTickFull = true;
                }

+                if (spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE && firstTickFull){
+                    gamepad1.rumble(100);
+                    firstTickFull = false;
+                }
+            } else {
                SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();

                if (gamepad1.leftBumperWasPressed() &&
@@ -133,6 +186,25 @@ public class NewShooterTest extends LinearOpMode {
                                state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {

                    spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                    intakeFull = false;
+                    firstTickFull = true;
+                    shooting = true;
+                }
+
+                if (state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT){
+                    shooting = false;
+                }
+
+                if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
+                    intakeFull = true;
+                } else {
+                    intakeFull = false;
+                    firstTickFull = true;
+                }
+
+                if (intakeFull && firstTickFull){
+                    gamepad1.rumble(100);
+                    firstTickFull = false;
                }

                if (gamepad1.right_trigger > 0.5 &&
@@ -143,8 +215,17 @@ public class NewShooterTest extends LinearOpMode {
                            SpindexerTransferIntake.RapidMode.HOLD_BALLS
                    );
                }
+
                if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
                    robot.setIntakePower(1);
+                    robot.setTransferPower(-0.7);
+                }
+            }
+
+            if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+            } else if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
            }

            if (gamepad1.dpad_down){
@@ -153,14 +234,21 @@ public class NewShooterTest extends LinearOpMode {
                parkTilter.unpark();
            }

+            loopTimes.loop();
+//            TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
+//            TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
+//            TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
+//
            TELE.addData("Distance From Goal", commander.getDistance());
-            TELE.addData("Hood Position", commander.getHoodPredicted());
-            TELE.addData("Transfer Power", commander.getTransferPow());
+//            TELE.addData("Hood Position", commander.getHoodPredicted());
+//            TELE.addData("Transfer Power", robot.transfer.getPower());
            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
-            TELE.addData("Manuel Velocity RPM", flywheelVelo);
            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
-            TELE.addData("TX:", turret.getTX());
-
+//
+//            TELE.addData("Current Position", currentPose);
+//
+//            TELE.addData("Current LL Pipeline", turret.pipeline());
+//
            TELE.update();
        }

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
@@ -83,11 +83,15 @@ public class Flywheel {

 //        pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);

+        if (velo1 != 0){
            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+        } else {
+            robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+        }
+
    }

    private double prevF = 0;
-
    public static double voltagePIDFDifference = 1;
    double averageVoltage = 0;
    public void setF(double voltage){
@@ -95,7 +99,11 @@ public class Flywheel {
        double f = shooterPIDF_F / voltage;
        if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) {
            shooterPIDF.f = f;
+            if (velo1 != 0){
                robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+            } else {
+                robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+            }
        }
        prevF = f;
    }
@@ -134,21 +142,43 @@ public class Flywheel {
    double power;
    double prevTargetTime = 0;
    double prevTargetVelocity = 0;
+    int veloMode = 0;
    public void manageFlywheel(double commandedVelocity) {

        if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
            targetVelocity = commandedVelocity;
        }

-        robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
-        power = robot.shooter1.getPower();
-        robot.shooter2.setPower(power);
+        if (velo1 < 100){

+        }
        velo1 = TPS_to_RPM(robot.shooter1.getVelocity());

        velo2 = TPS_to_RPM(robot.shooter2.getVelocity());

+        if (velo1 < 100){
+            velo1 = 0;
+            velo = velo2;
+            robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
+            if (veloMode != 2){
+                robot.shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                veloMode = 2;
+            }
+            power = robot.shooter2.getPower();
+            robot.shooter1.setPower(power);
+        } else {
+            velo2 = 0;
            velo = velo1;
+            robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
+            if (veloMode != 1){
+                robot.shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                veloMode = 1;
+            }
+            power = robot.shooter1.getPower();
+            robot.shooter2.setPower(power);
+        }

        updateVelocityAverage(velo);

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java
@@ -15,6 +15,7 @@ import com.qualcomm.robotcore.hardware.TouchSensor;
 import com.qualcomm.robotcore.hardware.VoltageSensor;

 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;

 public class Robot {
@@ -80,6 +81,11 @@ public class Robot {
    public Limelight3A limelight;
    public Servo light;

+    // Current Limits
+    public static double intakeCurrentLimit = 6.0;
+    public static double transferCurrentLimit = 5.0;
+    public static double drivetrainCurrentLimit = 7.0;
+
    public Robot(HardwareMap hardwareMap) {

        //Define components w/ hardware map
@@ -120,7 +126,6 @@ public class Robot {

        transfer = hardwareMap.get(DcMotorEx.class, "transfer");

-
        transferServo = hardwareMap.get(Servo.class, "transferServo");

        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -138,6 +143,13 @@ public class Robot {

        revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");

+        frontLeft.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        frontRight.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        backLeft.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        backRight.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        intake.setCurrentAlert(intakeCurrentLimit, CurrentUnit.AMPS);
+        transfer.setCurrentAlert(transferCurrentLimit, CurrentUnit.AMPS);
+
        // Below is disregarded

 //        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
@@ -174,6 +186,12 @@ public class Robot {

    private double prevFrontLeftPower = -10.501;
    public void setFrontLeftPower(double pow){
+        if (frontLeft.isOverCurrent()){
+            double current = frontLeft.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
        if (pow != prevFrontLeftPower){
            frontLeft.setPower(pow);
@@ -183,6 +201,12 @@ public class Robot {

    private double prevFrontRightPower = -10.501;
    public void setFrontRightPower(double pow){
+        if (frontRight.isOverCurrent()){
+            double current = frontRight.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
        if (pow != prevFrontRightPower){
            frontRight.setPower(pow);
@@ -192,6 +216,12 @@ public class Robot {

    private double prevBackLeftPower = -10.501;
    public void setBackLeftPower(double pow){
+        if (backLeft.isOverCurrent()){
+            double current = backLeft.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
        if (pow != prevBackLeftPower){
            backLeft.setPower(pow);
@@ -201,6 +231,12 @@ public class Robot {

    private double prevBackRightPower = -10.501;
    public void setBackRightPower(double pow){
+        if (backRight.isOverCurrent()){
+            double current = backRight.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
        if (pow != prevBackRightPower){
            backRight.setPower(pow);
@@ -210,6 +246,12 @@ public class Robot {

    private double prevIntakePower = -10.501;
    public void setIntakePower(double pow){
+        if (intake.isOverCurrent()){
+            double current = intake.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * intakeCurrentLimit / current;
+            }
+        }
        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
        if (pow != prevIntakePower){
            intake.setPower(pow);
@@ -220,6 +262,12 @@ public class Robot {
    private double prevTransferPower = -10.501;
    public void setTransferPower(double pow){
        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
+//        if (transfer.isOverCurrent()){
+//            double current = transfer.getCurrent(CurrentUnit.AMPS);
+//            if (current != 0) {
+//                pow = pow * transferCurrentLimit / current;
+//            }
+//        }
        if (pow != prevTransferPower){
            transfer.setPower(pow);
        }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
@@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.pedropathing.follower.Follower;

 import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.teleop.TeleopV4;

@Config
 public class Shooter {
@@ -137,9 +138,9 @@ public class Shooter {
                );

                flywheelVelocity = commander.getPredictedRPM();
-
-                robot.setHoodPos(commander.getHoodPredicted());
-                fly.manageFlywheel(flywheelVelocity);
+                double hood1 = Math.max(0.35, Math.min(0.88, commander.getHoodPredicted() + TeleopV4.hoodOffset));
+                robot.setHoodPos(hood1);
+                fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
                fly.setF(voltage);
                break;
            case READ_OBELISK:
@@ -180,9 +181,9 @@ public class Shooter {
                );

                flywheelVelocity = commander.getPredictedRPM();
-                robot.setHoodPos(commander.getHoodPredicted());
-
-                fly.manageFlywheel(flywheelVelocity);
+                double hood2 = Math.max(0.35, Math.min(0.88, commander.getHoodPredicted() + TeleopV4.hoodOffset));
+                robot.setHoodPos(hood2);
+                fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
                break;

            case MANUAL_FLYWHEEL_TRACK_TURR:
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
@@ -53,8 +53,8 @@ public class SpindexerTransferIntake {
    }

    int[] shootOrder = {0, 1, 2};
-    final double sensorDistanceThreshold = 5.3;
-    final long pulseTime = 100; // ms
+    public static final double sensorDistanceThreshold = 5.0;//6.0;//4.85//5.35
+    final long pulseTime = 70; // ms

    private DesiredPattern desiredPattern = DesiredPattern.GPP;

@@ -179,7 +179,9 @@ public class SpindexerTransferIntake {
    public enum SpindexerMode {
        RAPID,
        SORTED,
-        SHOOT_SORTED
+        SHOOT_SORTED,
+        SPINDEXER_BACK,
+        SHOOT_BACK
    }

    public enum BallStates {
@@ -203,7 +205,7 @@ public class SpindexerTransferIntake {
    private RapidMode rapidMode = RapidMode.INTAKE;
    private SortedIntakeStates sortedIntakeStates = SortedIntakeStates.IDLE;
    private BallStates[] ballColors = {BallStates.UNKNOWN, BallStates.UNKNOWN, BallStates.UNKNOWN};
-    final double greenThresh = 0.39;
+    final double greenThresh = 0.41;
    final double spinMovementTime = 250;

    /**
@@ -241,6 +243,15 @@ public class SpindexerTransferIntake {
                SpindexerMode.SHOOT_SORTED
        );
    }
+    public void startBackShooting(){
+        setShootState(
+                SortedShootState.IDLE
+        );
+
+        setSpindexerMode(
+                SpindexerMode.SHOOT_BACK
+        );
+    }
    public void setRapidMode(RapidMode newMode) {
        if (rapidMode != newMode) {
            rapidMode = newMode;
@@ -263,6 +274,10 @@ public class SpindexerTransferIntake {
        return this.rapidMode;
    }

+    public SpindexerMode getSpindexerMode(){return this.mode;}
+
+    public SortedIntakeStates getSortedIntakeStates(){return this.sortedIntakeStates;}
+
    private long stateTime() {
        return System.currentTimeMillis() - stateStartTime;
    }
@@ -274,6 +289,7 @@ public class SpindexerTransferIntake {
    private int greenTicks = 0;
    private int ballTicks = 0;
    private int holdBallsTicker = 0;
+    public static boolean intakeFull = false;
    public void update() {

 //        TELE.addData("Sorted State", sortedIntakeStates);
@@ -310,17 +326,16 @@ public class SpindexerTransferIntake {
                        robot.setSpinPos(
                                ServoPositions.spindexer_A2
                        );
-                        robot.setTransferPower(-0.7);
+                        robot.setTransferPower(-0.8);
                        robot.setTransferServoPos(
                                ServoPositions.transferServo_out
                        );

-                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
-
+                        if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                            holdBallsTicker++;
                        }

-                        if (holdBallsTicker > 10){
+                        if (holdBallsTicker > 5){
                            setRapidMode(RapidMode.TRANSFER_OFF);
                            holdBallsTicker = 0;
                        }
@@ -332,6 +347,7 @@ public class SpindexerTransferIntake {
                        if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                            setRapidMode(RapidMode.BEFORE_PULSE_OUT);
                        }
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
                        robot.setTransferPower(-0.3);

                        break;
@@ -340,7 +356,7 @@ public class SpindexerTransferIntake {

                        robot.setIntakePower(1.0);

-                        if (stateTime() >= 300) {
+                        if (stateTime() >= 100) {
                            setRapidMode(RapidMode.PULSE_OUT);
                        }

@@ -348,7 +364,7 @@ public class SpindexerTransferIntake {

                    case PULSE_OUT:

-                        robot.setIntakePower(-0.1);
+//                        robot.setIntakePower(0);

                        if (stateTime() >= pulseTime) {
                            setRapidMode(RapidMode.PULSE_IN);
@@ -360,11 +376,10 @@ public class SpindexerTransferIntake {

                        robot.setIntakePower(1.0);

-                        if (stateTime() >= 200) {
+                        if (stateTime() >= 100) {
                            setRapidMode(RapidMode.HOLD_BALLS);
                        }

-
                        break;

                    case HOLD_BALLS:
@@ -372,18 +387,20 @@ public class SpindexerTransferIntake {
                        if (robot.insideBeam.isPressed()
                                && robot.outsideBeam.isPressed()) {

+                            intakeFull = true;
+
                            robot.setTransferPower(0);
                            robot.setIntakePower(0.1);
                            robot.setTransferServoPos(ServoPositions.transferServo_in);

                        } else {
-                            holdBallsTicker++;
-                            robot.setIntakePower(1);
+                            setRapidMode(RapidMode.INTAKE);
+                            intakeFull = false;
                        }
                        break;

                    case OPEN_GATE:
-
+                        intakeFull = false;
                        if (stateTime() >= 100) {
                            setRapidMode(RapidMode.SHOOT);
                        }
@@ -538,23 +555,132 @@ public class SpindexerTransferIntake {
                        }
                        break;
                    case REVERSE:
-                        robot.setTransferPower(-0.3);
-                        robot.setIntakePower(-0.1);
+                        if (ballTicks > 4){
+                            robot.setTransferPower(0);
+                        } else if (ballTicks > 1){
+                            robot.setTransferPower(1);
+                        } else {
+                            robot.setTransferPower(-1);
+                        }
+                        ballTicks++;
+                        robot.setIntakePower(-0.7);
+                        robot.setSpinPos(ServoPositions.spindexer_StopShooting);
+                        break;
+                }
+
+
+                break;
+            case SPINDEXER_BACK:
+
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+
+                switch (sortedIntakeStates) {
+                    case NOTHING:
+                        break;
+                    case IDLE:
+                        ballColors[0] = BallStates.UNKNOWN;
+                        ballColors[1] = BallStates.UNKNOWN;
+                        ballColors[2] = BallStates.UNKNOWN;
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Open
+                        );
+                        robot.setSpindexBlockerPos(
+                                ServoPositions.spindexBlocker_Closed
+                        );
+                        robot.setSpinPos(
+                                ServoPositions.spindexer_A1
+                        );
+                        robot.setTransferServoPos(
+                                ServoPositions.transferServo_out
+                        );
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(-0.7);
+                        if (sortedStateTime() > 200) {
+                            setSortedIntakeMode(SortedIntakeStates.INTAKE_1);
+                        }
+                        break;
+                    case INTAKE_1:
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(-0.7);
+                        if (robot.insideBeam.isPressed()) {
+                            ballTicks++;
+                            if (ballTicks > 0) {
+                                robot.setSpinPos(ServoPositions.spindexer_A2);
+                                setSortedIntakeMode(SortedIntakeStates.DELAY_1);
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
+                        }
+                        break;
+                    case DELAY_1:
+                        robot.setSpinPos(ServoPositions.spindexer_A2);
+                        if (sortedStateTime() > spinMovementTime) {
+                            setSortedIntakeMode(SortedIntakeStates.INTAKE_2);
+                        }
+                        break;
+                    case INTAKE_2:
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(-1);
+                        if (robot.insideBeam.isPressed()) {
+                            ballTicks++;
+                            if (ballTicks > 0) {
+                                robot.setSpinPos(ServoPositions.spindexer_A3);
+                                setSortedIntakeMode(SortedIntakeStates.DELAY_2);
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
+                        }
+                        break;
+                    case DELAY_2:
+
+                        robot.setSpinPos(
+                                ServoPositions.spindexer_A3
+                        );
+
+                        if (sortedStateTime() > spinMovementTime) {
+                            setSortedIntakeMode(
+                                    SortedIntakeStates.INTAKE_3
+                            );
+                        }
+
+                        break;
+                    case INTAKE_3:
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(-1);
+                        if (robot.insideBeam.isPressed()) {
+                            ballTicks++;
+                            if (ballTicks > 0) {
+                                setSortedIntakeMode(SortedIntakeStates.REVERSE);
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
+                        }
+                        break;
+                    case REVERSE:
+                        if (ballTicks > 3){
+                            robot.setTransferPower(0);
+                        } else if (ballTicks > 1){
+                            robot.setTransferPower(1);
+                        } else {
+                            robot.setTransferPower(-1);
+                        }
+                        ballTicks++;
+                        robot.setIntakePower(-0.7);
+                        robot.setSpinPos(ServoPositions.spindexer_StopShooting);
                        break;
                }


                break;
            case SHOOT_SORTED:
-
+                ballTicks = 0;
                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
                if (Shooter.manualFlywheel) {
                    robot.setTransferPower(NewShooterTest.transferPower);
                } else {
-                    robot.setTransferPower(commander.getTransferPow());
+                    robot.setTransferPower(-0.8);
                }

-
                switch (shootState) {
                    case IDLE:
                        shootOrder = buildShootOrder(
@@ -740,6 +866,203 @@ public class SpindexerTransferIntake {
                }


+                break;
+            case SHOOT_BACK:
+                ballTicks = 0;
+
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+                if (Shooter.manualFlywheel) {
+                    robot.setTransferPower(NewShooterTest.transferPower);
+                } else {
+                    robot.setTransferPower(-0.8);
+                }
+
+
+                switch (shootState) {
+                    case IDLE:
+                        shootOrder = buildShootOrder(
+                                ballColors,
+                                desiredPattern
+                        );
+
+                        setShootState(SortedShootState.MOVE_TO_1);
+                        mode = SpindexerMode.SHOOT_SORTED;
+                        break;
+                    case MOVE_TO_1:
+
+                        robot.setSpinPos(ServoPositions.spindexer_A3);
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Open
+                        );
+                        robot.setSpindexBlockerPos(
+                                ServoPositions.spindexBlocker_Closed
+                        );
+
+
+                        setShootState(
+                                SortedShootState.WAIT_FOR_1
+                        );
+
+                        break;
+                    case WAIT_FOR_1:
+
+                        if (shootStateTime() > 150) {
+
+                            setShootState(
+                                    SortedShootState.SHOOT_1
+                            );
+                        }
+
+                        break;
+
+                    case SHOOT_1:
+
+                        robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
+
+
+                        if (shootStateTime() > 300) {
+
+                            setShootState(
+                                    SortedShootState.RETRACT_1
+                            );
+                        }
+
+                        break;
+                    case RETRACT_1:
+
+                        robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
+                        robot.setTransferServoPos(ServoPositions.transferServo_out);
+
+                        if (shootStateTime() > 150) {
+
+                            setShootState(
+                                    SortedShootState.MOVE_TO_2
+                            );
+                        }
+
+                        break;
+                    case MOVE_TO_2:
+
+                        robot.setSpinPos(ServoPositions.spindexer_A2);
+
+                        setShootState(
+                                SortedShootState.WAIT_FOR_2
+                        );
+
+                        break;
+                    case WAIT_FOR_2:
+
+                        if (shootStateTime() > 150) {
+
+                            setShootState(
+                                    SortedShootState.SHOOT_2
+                            );
+                        }
+
+                        break;
+                    case SHOOT_2:
+
+                        robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
+
+                        if (shootStateTime() > 300) {
+
+                            setShootState(
+                                    SortedShootState.RETRACT_2
+                            );
+                        }
+
+                        break;
+                    case RETRACT_2:
+
+                        robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
+                        robot.setTransferServoPos(ServoPositions.transferServo_out);
+
+
+                        if (shootStateTime() > 150) {
+
+                            setShootState(
+                                    SortedShootState.MOVE_TO_3
+                            );
+                        }
+
+                        break;
+                    case MOVE_TO_3:
+
+                        robot.setSpinPos(ServoPositions.spindexer_A1);
+
+                        setShootState(
+                                SortedShootState.WAIT_FOR_3
+                        );
+
+                        break;
+                    case WAIT_FOR_3:
+
+                        if (shootStateTime() > 150) {
+
+                            setShootState(
+                                    SortedShootState.SHOOT_3
+                            );
+                        }
+
+                        break;
+                    case SHOOT_3:
+
+                        robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
+
+                        if (shootStateTime() > 300) {
+
+                            setShootState(
+                                    SortedShootState.RETRACT_3
+                            );
+                        }
+
+                        break;
+                    case RETRACT_3:
+
+                        robot.setTransferServoPos(ServoPositions.transferServo_out);
+
+                        if (shootStateTime() > 150) {
+
+                            setShootState(
+                                    SortedShootState.DONE
+                            );
+                        }
+
+                        break;
+                    case DONE:
+
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Open
+                        );
+                        robot.setSpindexBlockerPos(
+                                ServoPositions.spindexBlocker_Closed
+                        );
+                        robot.setSpinPos(
+                                ServoPositions.spindexer_A1
+                        );
+                        robot.setTransferServoPos(
+                                ServoPositions.transferServo_out
+                        );
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(-0.7);
+
+                        if (shootStateTime() > 250) {
+
+                            setSortedIntakeMode(
+                                    SortedIntakeStates.IDLE
+                            );
+
+                            mode = SpindexerMode.SPINDEXER_BACK;
+                        }
+
+                        break;
+
+                }
+
+
                break;
        }
    }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
@@ -17,16 +17,16 @@ public class Turret {

    private final double servoTicksPer180 = 0.58;
    public static double neutralPosition = 0.51;
-    private final double turretMin = 0.05;
-    private final double turretMax = 0.95;
+    private final double turretMin = 0.13;
+    private final double turretMax = 0.87;
    public static boolean limelightUsed = true;
    public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005;
    LLResult result;
    PIDController bearingPID;
    boolean bearingAligned = false;
    public int LL_COAST_TICKS = 5;
-    public static double TARGET_POSITION_TOLERANCE = 0.65;
-    public static double alphaTX = 0.5;
+    public static double TARGET_POSITION_TOLERANCE = 1.5;
+    public static double alphaTX = 0.3;
    private double targetTx = 0;
    private double currentTrackOffset = 0;
    private double llCoast = 0;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
@@ -10,8 +10,8 @@ public class VelocityCommander {
    public static double yAccK = 0.025; // TODO: Tune
    public static boolean lockFront = false;
    public static boolean lockBack = false;
-    public static int farBound = 140;
-    public static int closeBound = 110;
+    public static int farBound = 138;
+    public static int closeBound = 112;
    public static double errorHoodAdjustment = 0.0005;
    private double hoodPos = 0.88;
    private double transferPow = -1;
@@ -24,6 +24,10 @@ public class VelocityCommander {
    final double veloC = -0.0739531;
    final double veloD = 5.16759;
    final double veloE = 62.45781;
+    final double veloF = -0.0000833333;
+    final double veloG = 0.0377857;
+    final double veloH = -4.55067;
+    final double veloI = 456.97486;
    private double distToRPM (double dist){
        double currentVelo;
        if (lockFront && dist > closeBound){
@@ -33,8 +37,8 @@ public class VelocityCommander {
        }
        if (dist < 54) {
            velo = 2000;
-        } else if (dist > 181){
-            velo = 3600;
+        } else if (dist > 174){
+            velo = 3700;
        } else {
            currentVelo = veloA*Math.pow(dist, 4) +
                    veloB*Math.pow(dist, 3) +
@@ -50,6 +54,8 @@ public class VelocityCommander {
    final double hoodB = -0.0000204165;
    final double hoodC = -0.00252089;
    final double hoodD = 1.06154;
+    final double hoodE = -0.002;
+    final double hoodF = 0.918;
    private void distToHood (double dist){
        if (dist > 174){
            hoodPos = 0.48;
@@ -61,6 +67,10 @@ public class VelocityCommander {
                    hoodC*Math.pow(dist, 1) +
                    hoodD;
        }
+        if (lockBack){
+            hoodPos-=0.04;
+        }
+
        hoodPos = Math.max(0.48, Math.min(0.88, hoodPos));
    }
    public double getHoodPredicted(){
@@ -69,9 +79,9 @@ public class VelocityCommander {

    private void distToTransferPow(double dist, double voltage){
        if (dist < 140){
-            transferPow = -0.8;
+            transferPow = -0.85;
        } else {
-            transferPow = -0.5;
+            transferPow = -0.7;
        }
    }
    public double getTransferPow(){return transferPow;}
@@ -94,7 +104,7 @@ public class VelocityCommander {
        distToTransferPow(predictedDist, voltage);
        distToRPM(predictedDist);

-        hoodPos += adjustHood(predictedDist, velocity, velo);
+//        hoodPos += adjustHood(predictedDist, velocity, velo);
    }

    public double adjustHood(double dist, double currentVelocity, double targetVelocity){
Author	SHA1	Message	Date
Keshav Anand	97e0183fdd	Last commit of 23344...	2026-06-12 22:52:51 -04:00
DanTheMan-byte	ef6b0bebbb	cowtown end	2026-06-11 15:55:56 -05:00
DanTheMan-byte	e8f80ef39e	match 69	2026-06-10 10:43:02 -05:00
DanTheMan-byte	20137025c1	nighttime sleepy	2026-06-09 22:48:11 -05:00
DanTheMan-byte	12f433a221	day 1	2026-06-09 17:44:13 -05:00
DanTheMan-byte	1cd14021dd	calibration	2026-06-09 09:29:32 -05:00
DanTheMan-byte	d0ee294d26	night before	2026-06-08 22:29:36 -05:00
DanTheMan-byte	b2a1ea32d9	a lot of changes happened	2026-06-08 15:29:22 -05:00