a lot of changes happened

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;
 
@@ -369,6 +370,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);

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 rapidWaitTime = 0.7;
-    public static double rapidShootTime = 0.8;
+    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 shootGateX = 16, shootGateY = -55, shootGateH = 0;
-    public static double leaveX = 40, leaveY = 14, leaveH = 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(3250);
-                shooter.setFlywheelVelocity(2400);
+                robot.setHoodPos(0.65);
-                robot.setHoodPos(0.64);
                 if (flywheel.getSteady()){
                     startAuto++;
                 }
-                if (startAuto > 5){
+                if (startAuto > 6){
-                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                    spindexer.startBackShooting();
                     timeStamp = currentTime;
                     pathState = PathState.WAIT_SHOOT0;
                 }
+                timeStamp = currentTime;
                 break;
             case WAIT_SHOOT0:
-                if (currentTime - timeStamp > rapidShootTime ||
+                if (currentTime - timeStamp > rapidShootTime){
-                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
-                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
                     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 ||
+                if (currentTime - timeStamp > rapidShootTime){
-                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
-                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
                     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 ||
+                if (currentTime - timeStamp > rapidShootTime){
-                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
-                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
                     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 ||
+                if (currentTime - timeStamp > rapidShootTime){
-                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
+//                    follower.followPath(shootGate_intakeGate, false);
-                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
+                    follower.followPath(shootGate_leave, true);
-                    follower.followPath(shootGate_intakeGate, false);
+                    pathState = PathState.LEAVE;
-                    pathState = PathState.INTAKE_GATE;
                     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.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
-                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_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());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.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;
 
@@ -413,6 +414,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());

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;

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

@@ -9,6 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 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.*;
@@ -36,6 +37,8 @@ public class TeleopV4 extends LinearOpMode {
     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 +88,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());
@@ -137,23 +143,29 @@ public class TeleopV4 extends LinearOpMode {
             }
 
             if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
-                shooter.setFlywheelVelocity(2500);
+                shooter.setFlywheelVelocity(2500 + flywheelOffset);
-                robot.setHoodPos(0.6);
+                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,50 +173,65 @@ public class TeleopV4 extends LinearOpMode {
             shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
 
-            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
+            if (VelocityCommander.lockBack){
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+                if (gamepad1.leftBumperWasPressed()){
+                    spindexerTransferIntake.startBackShooting();
+                    firstTickFull = true;
+                }
 
-            if (gamepad1.leftBumperWasPressed() &&
+                if (spindexerTransferIntake.getSortedIntakeStates() == SpindexerTransferIntake.SortedIntakeStates.REVERSE && firstTickFull){
-                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                    gamepad1.rumble(100);
-                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
+                    firstTickFull = false;
-                            state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
+                }
-                            state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
+                TELE.addData("Intake State", spindexerTransferIntake.getSortedIntakeStates());
-                            state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
-                            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;
+                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
-                firstTickFull = true;
+                SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
-            }
 
-            if (intakeFull && firstTickFull){
+                if (gamepad1.leftBumperWasPressed() &&
-                gamepad1.rumble(100);
+                        (state == SpindexerTransferIntake.RapidMode.INTAKE ||
-                firstTickFull = false;
+                                state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
-            }
+                                state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
+                                state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
+                                state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
+                                state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
 
-            if (gamepad1.right_trigger > 0.5 &&
+                    spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
-                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                    intakeFull = false;
-                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
+                    firstTickFull = true;
+                    shooting = true;
+                }
 
-                spindexerTransferIntake.setRapidMode(
+                if (state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT){
-                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
+                    shooting = false;
-                );
+                }
-            }
 
-            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
+                if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
-                robot.setIntakePower(1);
+                    intakeFull = true;
-                robot.setTransferPower(-0.7);
+                } else {
+                    intakeFull = false;
+                    firstTickFull = true;
+                }
+
+                if (intakeFull && firstTickFull){
+                    gamepad1.rumble(100);
+                    firstTickFull = false;
+                }
+
+                if (gamepad1.right_trigger > 0.5 &&
+                        (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                                state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
+
+                    spindexerTransferIntake.setRapidMode(
+                            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()){
@@ -219,6 +246,29 @@ 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+=100;
+            } else if (gamepad2.dpadLeftWasPressed()){
+                flywheelOffset-=100;
+            }
+
+            if (gamepad2.crossWasPressed()){
+                flywheelOffset = 0;
+                hoodOffset = 0;
+            }
+
             loopTimes.loop();
 //            TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
 //            TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());

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,56 +141,114 @@ 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 (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() &&
+                        (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                                state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
+                                state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
+                                state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
+                                state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
+                                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 &&
+                        (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                                state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
+
+                    spindexerTransferIntake.setRapidMode(
+                            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;
             }
 
-            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
-
-            if (gamepad1.leftBumperWasPressed() &&
-                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
-                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
-                            state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
-                            state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
-                            state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
-                            state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
-
-                spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
-            }
-
-            if (gamepad1.right_trigger > 0.5 &&
-                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
-                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
-
-                spindexerTransferIntake.setRapidMode(
-                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
-                );
-            }
-            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
-                robot.setIntakePower(1);
-            }
-
             if (gamepad1.dpad_down){
                 parkTilter.park();
             } else if (gamepad1.dpad_up) {
                 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("Hood Position", commander.getHoodPredicted());
-            TELE.addData("Transfer Power", commander.getTransferPow());
+//            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();
         }
 

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 {
@@ -138,8 +139,8 @@ public class Shooter {
 
                 flywheelVelocity = commander.getPredictedRPM();
 
-                robot.setHoodPos(commander.getHoodPredicted());
+                robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset);
-                fly.manageFlywheel(flywheelVelocity);
+                fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
                 fly.setF(voltage);
                 break;
             case READ_OBELISK:
@@ -180,9 +181,8 @@ public class Shooter {
                 );
 
                 flywheelVelocity = commander.getPredictedRPM();
-                robot.setHoodPos(commander.getHoodPredicted());
+                robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset);
-
+                fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
-                fly.manageFlywheel(flywheelVelocity);
                 break;
 
             case MANUAL_FLYWHEEL_TRACK_TURR:

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java

@@ -179,7 +179,9 @@ public class SpindexerTransferIntake {
     public enum SpindexerMode {
         RAPID,
         SORTED,
-        SHOOT_SORTED
+        SHOOT_SORTED,
+        SPINDEXER_BACK,
+        SHOOT_BACK
     }
 
     public enum BallStates {
@@ -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;
     }
@@ -538,20 +553,130 @@ public class SpindexerTransferIntake {
                         }
                         break;
                     case REVERSE:
-                        robot.setTransferPower(-0.3);
+                        if (ballTicks > 4){
-                        robot.setIntakePower(-0.1);
+                            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);
                 }
 
 
@@ -740,6 +865,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;
         }
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java

@@ -25,8 +25,8 @@ public class Turret {
     PIDController bearingPID;
     boolean bearingAligned = false;
     public int LL_COAST_TICKS = 5;
-    public static double TARGET_POSITION_TOLERANCE = 0.65;
+    public static double TARGET_POSITION_TOLERANCE = 1.5;
-    public static double alphaTX = 0.5;
+    public static double alphaTX = 0.3;
     private double targetTx = 0;
     private double currentTrackOffset = 0;
     private double llCoast = 0;

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 farBound = 138;
-    public static int closeBound = 110;
+    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){
+        } else if (dist > 174){
-            velo = 3600;
+            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;
@@ -69,9 +75,9 @@ public class VelocityCommander {
 
     private void distToTransferPow(double dist, double voltage){
         if (dist < 140){
-            transferPow = -0.8;
+            transferPow = -0.75;
         } else {
-            transferPow = -0.5;
+            transferPow = -0.6;
         }
     }
     public double getTransferPow(){return transferPow;}
@@ -94,7 +100,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){