teleop tweaks

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

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java

@@ -22,28 +22,26 @@ 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.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
-import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
-import org.firstinspires.ftc.teamcode.utils.Turret;
+import org.firstinspires.ftc.teamcode.utilsv2.Turret;
 
 import java.util.List;
 
-@Config
-@Autonomous (preselectTeleOp = "TeleopV3")
 public class Auto12BallPedroPathing extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
-    Flywheel flywheel;
+//    Flywheel flywheel;
-    Targeting targeting;
+//    Targeting targeting;
-    Targeting.Settings targetingSettings;
+//    Targeting.Settings targetingSettings;
     Follower follower;
-    Turret turret;
+//    Turret turret;
-    Spindexer spindexer;
+//    Spindexer spindexer;
-    Servos servos;
+//    Servos servos;
     MeasuringLoopTimes loopTimes;
 
     // Wait Times
@@ -222,10 +220,10 @@ public class Auto12BallPedroPathing extends LinearOpMode {
                 driveShoot(PathState.WAIT_SHOOT3, currentTime);
                 break;
             case WAIT_SHOOT3:
-                if (spindexer.shootAllComplete()){
+//                if (spindexer.shootAllComplete()){
-                    spindexer.resetSpindexer();
+//                    spindexer.resetSpindexer();
-                    TELE.addLine("Done Auto");
+//                    TELE.addLine("Done Auto");
-                }
+//                }
                 break;
             default:
                 break;
@@ -237,21 +235,21 @@ public class Auto12BallPedroPathing extends LinearOpMode {
     private void intakePowerDown(double stamp, double currentTime) {
         double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust
         if (pow < -1) {pow = 0;}
-        spindexer.setIntakePower(pow);
+//        spindexer.setIntakePower(pow);
     }
     private void driveShoot(PathState nextState, double currentTime){
         if (!follower.isBusy()){
             pathState = nextState;
             timeStamp = currentTime;
-            spindexer.prepareShootAllContinous();
+//            spindexer.prepareShootAllContinous();
         }
     }
     private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) {
-        if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime) {
+        if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() ||
-            spindexer.resetSpindexer();
+//            spindexer.resetSpindexer();
             pathState = nextState;
             follower.followPath(nextPath, true);
-            spindexer.setIntakePower(1);
+//            spindexer.setIntakePower(1);
         }
     }
     private void drivePickup(PathState nextState, PathChain nextPath) {
@@ -286,24 +284,25 @@ public class Auto12BallPedroPathing extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        robot = new Robot(hardwareMap);
+        Robot.resetInstance();
+        robot = Robot.getInstance(hardwareMap);
         List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
         for (LynxModule hub : allHubs) {
             hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
         }
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-        flywheel = new Flywheel(hardwareMap);
+//        flywheel = new Flywheel(hardwareMap);
-        targeting = new Targeting();
+//        targeting = new Targeting();
-        targetingSettings = new Targeting.Settings(0,0);
+//        targetingSettings = new Targeting.Settings(0,0);
         follower = Constants.createFollower(hardwareMap);
         follower.setStartingPose(new Pose(72,72,0));
-        turret = new Turret(robot, TELE, robot.limelight);
+//        turret = new Turret(robot, TELE, robot.limelight);
-        spindexer = new Spindexer(hardwareMap);
+//        spindexer = new Spindexer(hardwareMap);
-        servos = new Servos(hardwareMap);
+//        servos = new Servos(hardwareMap);
         loopTimes = new MeasuringLoopTimes();
         loopTimes.init();
 
-        robot.light.setPosition(Color.LightRed);
+//        robot.light.setPosition(Color.LightRed);
 
         boolean initializeRobot = false;
         while (opModeInInit()){
@@ -311,11 +310,11 @@ public class Auto12BallPedroPathing extends LinearOpMode {
 
             if (gamepad1.crossWasPressed() && !initializeRobot){
                 Color.redAlliance = !Color.redAlliance;
-                if (Color.redAlliance){
+//                if (Color.redAlliance){
-                    robot.light.setPosition(Color.LightRed);
+//                    robot.light.setPosition(Color.LightRed);
-                } else {
+//                } else {
-                    robot.light.setPosition(Color.LightBlue);
+//                    robot.light.setPosition(Color.LightBlue);
-                }
+//                }
 
                 double[] xPoses = {startPoseX, shoot0X,
                         drivePickup1X, pickup1X, shoot1X,
@@ -336,10 +335,12 @@ public class Auto12BallPedroPathing extends LinearOpMode {
                 initializePoses();
                 follower.setPose(startPose);
                 buildPaths();
+//                turret.switchPipeline(Turret.PipelineMode.OBELISK);
+                robot.limelight.start();
                 sleep(2000);
 
-                turret.setTurret(turrDefault);
+//                turret.setTurret(turrDefault);
-                servos.setSpinPos(spinStartPos);
+//                servos.setSpinPos(spinStartPos);
             }
 
             TELE.addData("Red Alliance?", Color.redAlliance);
@@ -352,27 +353,27 @@ public class Auto12BallPedroPathing extends LinearOpMode {
 
         if (isStopRequested()) return;
 
-        robot.transfer.setPower(1);
+//        robot.transfer.setPower(1);
         limelightUsed = false;
 
         while (opModeIsActive()){
             follower.update();
             pathStateMachine();
             Pose currentPose = follower.getPose();
-            teleStartPoseX = currentPose.getX();
+//            teleStartPoseX = currentPose.getX();
-            teleStartPoseY = currentPose.getY();
+//            teleStartPoseY = currentPose.getY();
-            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
+//            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
-
+//
-            turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
+//            turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
-            targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
+//            targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
-
+//
-            double voltage = robot.voltage.getVoltage();
+//            double voltage = robot.voltage.getVoltage();
-            flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
+//            flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
-            flywheel.manageFlywheel(targetingSettings.flywheelRPM);
+//            flywheel.manageFlywheel(targetingSettings.flywheelRPM);
-            servos.setHoodPos(targetingSettings.hoodAngle);
+//            servos.setHoodPos(targetingSettings.hoodAngle);
-
+//
-            if (driveToShoot()){servos.setSpinPos(spinStartPos);}
+//            if (driveToShoot()){servos.setSpinPos(spinStartPos);}
-            else {spindexer.processIntake();}
+//            else {spindexer.processIntake();}
 
             for (LynxModule hub : allHubs) {
                 hub.clearBulkCache();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java

@@ -0,0 +1,421 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+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.teleop.TeleopV4;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
+import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
+
+import java.util.List;
+
+@Config
+@Autonomous (preselectTeleOp = "TeleopV4")
+public class Auto12Ball_Back_Sorted extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    Follower follower;
+    MeasuringLoopTimes loopTimes;
+    Shooter shooter;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+    SpindexerTransferIntake spindexer;
+
+    // Wait Times
+    public static double sortedShootTime = 2.6;
+    public static double rapidWaitTime = 0.5;
+    public static double rapidShootTime = 0.8;
+    public static double openGateTime = 2.5;
+    public static double pushTime = 2;
+
+    // Extra Variables
+    public static double intakePower = 0.5;
+    double shootX, shootY, shootH;
+
+    // Initialize path state machine
+    private enum PathState {
+        PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
+        PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
+        DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
+        DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
+    }
+    PathState pathState = PathState.PUSHBOT;
+
+    // Poses
+    public static double startPoseX = 12, startPoseY = -64, startPoseH = 90;
+    public static double pushBotX = 19, pushBotY = -63, pushBotH = 100;
+    public static double shoot0ControlX = 16.29667812142038, shoot0ControlY = -19.67493699885454;
+    public static double shoot0X = 19, shoot0Y = 10, shoot0H = 0;
+    public static double pickup1X = 54, pickup1Y = 10, pickup1H = 0;
+    public static double openGateControlX = 37.184421534937, openGateControlY = 2.24455899198165;
+    public static double openGateX = 59, openGateY = 2, openGateH = 0;
+    public static double shoot1ControlX = 40, shoot1ControlY = 3;
+    public static double shoot1X = 19, shoot1Y = 10, shoot1H = 0;
+    public static double drivePickup2X = 26, drivePickup2Y = -14, drivePickup2H = 0;
+    public static double pickup2X = 61, pickup2Y = -14.5, pickup2H = 0;
+    public static double shoot2ControlX = 30, shoot2ControlY = -9;
+    public static double shoot2X = 19, shoot2Y = 10, shoot2H = 0;
+    public static double drivePickup3X = 26, drivePickup3Y = -37.5, drivePickup3H = 0;
+    public static double pickup3X = 61, pickup3Y = -37.5, pickup3H = 0;
+    public static double shoot3ControlX = 25.62371134020621, shoot3ControlY = -38.813287514318446;
+    public static double shoot3X = 12, shoot3Y = 40, shoot3H = -90;
+    double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
+            pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
+            drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
+            drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
+    double[] yPoses = {startPoseY, pushBotY, shoot0ControlY, shoot0Y,
+            pickup1Y, openGateControlY, openGateY, shoot1ControlY, shoot1Y,
+            drivePickup2Y, pickup2Y, shoot2ControlY, shoot2Y,
+            drivePickup3Y, pickup3Y, shoot3ControlY, shoot3Y};
+    double[] headings = {startPoseH, pushBotH, 0, shoot0H,
+            pickup1H, 0, openGateH, 0, shoot1H,
+            drivePickup2H, pickup2H, 0, shoot2H,
+            drivePickup3H, pickup3H, 0, shoot3H};
+    Pose startPose, pushBot, shoot0Control, shoot0,
+            pickup1, openGateControl, openGate, shoot1Control, shoot1,
+            drivePickup2, pickup2, shoot2Control, shoot2,
+            drivePickup3, pickup3, shoot3Control, shoot3;
+    private void initializePoses(){
+        startPose = new Pose(xPoses[0], yPoses[0], Math.toRadians(headings[0]));
+        pushBot = new Pose(xPoses[1], yPoses[1], Math.toRadians(headings[1]));
+        shoot0Control = new Pose(xPoses[2], yPoses[2]);
+        shoot0 = new Pose(xPoses[3], yPoses[3], Math.toRadians(headings[3]));
+        pickup1 = new Pose(xPoses[4], yPoses[4], Math.toRadians(headings[4]));
+        openGateControl = new Pose(xPoses[5], yPoses[5]);
+        openGate = new Pose(xPoses[6], yPoses[6], Math.toRadians(headings[6]));
+        shoot1Control = new Pose(xPoses[7], yPoses[7]);
+        shoot1 = new Pose(xPoses[8], yPoses[8], Math.toRadians(headings[8]));
+        drivePickup2 = new Pose(xPoses[9], yPoses[9], Math.toRadians(headings[9]));
+        pickup2 = new Pose(xPoses[10], yPoses[10], Math.toRadians(headings[10]));
+        shoot2Control = new Pose(xPoses[11], yPoses[11]);
+        shoot2 = new Pose(xPoses[12], yPoses[12], Math.toRadians(headings[12]));
+        drivePickup3 = new Pose(xPoses[13], yPoses[13], Math.toRadians(headings[13]));
+        pickup3 = new Pose(xPoses[14], yPoses[14], Math.toRadians(headings[14]));
+        shoot3Control = new Pose(xPoses[15], yPoses[15]);
+        shoot3 = new Pose(xPoses[16], yPoses[16], Math.toRadians(headings[16]));
+    }
+
+    //Building Paths
+    PathChain startPose_pushBot, pushBot_shoot0,
+            shoot0_pickup1, pickup1_openGate, openGate_shoot1,
+            shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
+            shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3;
+    private void buildPaths(){
+        startPose_pushBot = follower.pathBuilder()
+                .addPath(new BezierLine(startPose, pushBot))
+                .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading())
+                .build();
+
+        pushBot_shoot0 = follower.pathBuilder()
+                .addPath(new BezierCurve(pushBot, shoot0Control, shoot0))
+                .setLinearHeadingInterpolation(pushBot.getHeading(), shoot0.getHeading())
+                .build();
+
+        shoot0_pickup1 = follower.pathBuilder()
+                .addPath(new BezierLine(shoot0, pickup1))
+                .setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading())
+                .build();
+
+        pickup1_openGate = follower.pathBuilder()
+                .addPath(new BezierCurve(pickup1, openGateControl, openGate))
+                .setLinearHeadingInterpolation(pickup1.getHeading(), openGate.getHeading())
+                .build();
+
+        openGate_shoot1 = follower.pathBuilder()
+                .addPath(new BezierCurve(openGate, shoot1Control, shoot1))
+                .setLinearHeadingInterpolation(openGate.getHeading(), shoot1.getHeading())
+                .build();
+
+        shoot1_drivePickup2 = follower.pathBuilder()
+                .addPath(new BezierLine(shoot1, drivePickup2))
+                .setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading())
+                .build();
+
+        drivePickup2_pickup2 = follower.pathBuilder()
+                .addPath(new BezierLine(drivePickup2, pickup2))
+                .setLinearHeadingInterpolation(drivePickup2.getHeading(), pickup2.getHeading())
+                .build();
+
+        pickup2_shoot2 = follower.pathBuilder()
+                .addPath(new BezierCurve(pickup2, shoot2Control, shoot2))
+                .setLinearHeadingInterpolation(pickup2.getHeading(), shoot2.getHeading())
+                .build();
+
+        shoot2_drivePickup3 = follower.pathBuilder()
+                .addPath(new BezierLine(shoot2, drivePickup3))
+                .setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading())
+                .build();
+
+        drivePickup3_pickup3 = follower.pathBuilder()
+                .addPath(new BezierLine(drivePickup3, pickup3))
+                .setLinearHeadingInterpolation(drivePickup3.getHeading(), pickup3.getHeading())
+                .build();
+
+        pickup3_shoot3 = follower.pathBuilder()
+                .addPath(new BezierCurve(pickup3, shoot3Control, shoot3))
+                .setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading())
+                .build();
+    }
+
+    //Path State Machine
+    private boolean startAuto = true;
+    private double timeStamp = 0;
+    private void pathStateMachine(){
+        double currentTime = (double) System.currentTimeMillis() / 1000;
+        switch(pathState){
+            case PUSHBOT:
+                spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                shooter.setFlywheelVelocity(2400);
+                robot.setHoodPos(0.64);
+                if (startAuto){
+                    follower.followPath(startPose_pushBot, false);
+                    startAuto = false;
+                    timeStamp = currentTime;
+                    shootX = shoot0X;
+                    shootY = shoot0Y;
+                    shootH = shoot0H;
+                }
+                if (!follower.isBusy() || currentTime - timeStamp > pushTime){
+                    follower.followPath(pushBot_shoot0, true);
+                    pathState = PathState.DRIVE_SHOOT0;
+                }
+                break;
+            case DRIVE_SHOOT0:
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT0;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } else if (follower.isBusy()){
+                    timeStamp = currentTime;
+                }
+                break;
+            case WAIT_SHOOT0:
+                if (currentTime - timeStamp > rapidShootTime ||
+                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
+                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
+                    follower.followPath(shoot0_pickup1, intakePower, false);
+                    pathState = PathState.PICKUP1;
+                    spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SORTED);
+                }
+                break;
+            case PICKUP1:
+                if (!follower.isBusy()){
+                    follower.followPath(pickup1_openGate, false);
+                    pathState = PathState.OPENGATE;
+                    shootX = shoot1X;
+                    shootY = shoot1Y;
+                    shootH = shoot1H;
+                    timeStamp = currentTime;
+                    shooter.setFlywheelVelocity(2300);
+                    robot.setHoodPos(0.68);
+                }
+                break;
+            case OPENGATE:
+                if (currentTime - timeStamp > openGateTime){
+                    follower.followPath(openGate_shoot1, true);
+                    pathState = PathState.DRIVE_SHOOT1;
+                }
+                break;
+            case DRIVE_SHOOT1:
+                if (!follower.isBusy()){
+                    pathState = PathState.WAIT_SHOOT1;
+                    timeStamp = currentTime;
+                    spindexer.startSortedShoot();
+                }
+                break;
+            case WAIT_SHOOT1:
+                if (currentTime - timeStamp > sortedShootTime){
+                    follower.followPath(shoot1_drivePickup2, true);
+                    pathState = PathState.DRIVE_PICKUP2;
+                }
+                break;
+            case DRIVE_PICKUP2:
+                if (!follower.isBusy()) {
+                    follower.followPath(drivePickup2_pickup2, intakePower, false);
+                    pathState = PathState.PICKUP2;
+                }
+                break;
+            case PICKUP2:
+                if (!follower.isBusy()){
+                    follower.followPath(pickup2_shoot2, true);
+                    pathState = PathState.DRIVE_SHOOT2;
+                }
+                shootX = shoot2X;
+                shootY = shoot2Y;
+                shootH = shoot2H;
+                break;
+            case DRIVE_SHOOT2:
+                if (!follower.isBusy()){
+                    pathState = PathState.WAIT_SHOOT2;
+                    timeStamp = currentTime;
+                    spindexer.startSortedShoot();
+                }
+                break;
+            case WAIT_SHOOT2:
+                if (currentTime - timeStamp > sortedShootTime){
+                    follower.followPath(shoot2_drivePickup3, true);
+                    pathState = PathState.DRIVE_PICKUP3;
+                }
+                break;
+            case DRIVE_PICKUP3:
+                if (!follower.isBusy()){
+                    shooter.setFlywheelVelocity(2200);
+                    robot.setHoodPos(0.75);
+                    follower.followPath(drivePickup3_pickup3, intakePower, false);
+                    pathState = PathState.PICKUP3;
+                }
+                break;
+            case PICKUP3:
+                if (!follower.isBusy()){
+                    follower.followPath(pickup3_shoot3, true);
+                    pathState = PathState.DRIVE_SHOOT3;
+                }
+                shootX = shoot3X;
+                shootY = shoot3Y;
+                shootH = shoot3H;
+                break;
+            case DRIVE_SHOOT3:
+                if (!follower.isBusy()){
+                    pathState = PathState.WAIT_SHOOT3;
+                    spindexer.startSortedShoot();
+                }
+                break;
+            case WAIT_SHOOT3:
+                // add line here to say "done auto"
+                break;
+            default:
+                break;
+        }
+        TELE.update(); // use for debugging
+    }
+
+    // Used for changing alliance
+    private double adjustXPoseBasedOnAlliance(double pose) {return -pose;}
+    private double adjustHeadingBasedOnAlliance(double heading){
+        heading = 180 - heading;
+        while (heading > 180) {heading-=360;}
+        while (heading <= -180) {heading+=360;}
+        return heading;
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Robot.resetInstance();
+        robot = Robot.getInstance(hardwareMap);
+        List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        follower = Constants.createFollower(hardwareMap);
+        sleep(1000);
+        follower.setStartingPose(new Pose(0,0,0));
+        loopTimes = new MeasuringLoopTimes();
+        loopTimes.init();
+        turret = new Turret(robot);
+        flywheel = new Flywheel(robot);
+        commander = new VelocityCommander();
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        spindexer = new SpindexerTransferIntake(robot, TELE, commander);
+        ParkTilter park = new ParkTilter(robot);
+
+        boolean initializeRobot = false;
+        while (opModeInInit()){
+            follower.update();
+
+            if (gamepad1.squareWasPressed()){
+                robot.setSpinPos(ServoPositions.spindexer_A2);
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
+                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+            }
+
+            if (gamepad1.crossWasPressed() && !initializeRobot){
+                Color.redAlliance = !Color.redAlliance;
+                shooter.setRedAlliance(Color.redAlliance);
+            }
+
+            if (!initializeRobot){
+                if ((Color.redAlliance && xPoses[0] < 0)
+                    || (!Color.redAlliance && xPoses[0] > 0)){
+                    for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
+                    for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
+                }
+            }
+
+            if (gamepad1.triangleWasPressed()){
+                initializeRobot = true;
+                initializePoses();
+                follower.setPose(startPose);
+                buildPaths();
+                sleep(2000);
+                turret.switchPipeline(Turret.PipelineMode.OBELISK);
+                robot.limelight.start();
+                limelightUsed = true;
+                park.unpark();
+            }
+
+            if (initializeRobot){
+                //add obelisk read here
+                shooter.setState(Shooter.ShooterState.READ_OBELISK);
+                int ID = turret.getObeliskID();
+                spindexer.setDesiredPatternAuto(ID);
+                TELE.addData("ID", ID);
+                shooter.update(robot.voltage.getVoltage());
+            }
+
+            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());
+            TELE.addData("Current LL Pipeline", turret.pipeline());
+            TELE.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()){
+            robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
+            park.unpark();
+
+            shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            shooter.update(robot.voltage.getVoltage());
+
+            if (!isStopRequested()){
+                follower.update();
+            }
+            pathStateMachine();
+            TeleopV4.teleStart = follower.getPose();
+
+            spindexer.update();
+
+            for (LynxModule hub : allHubs) {
+                hub.clearBulkCache();
+            }
+            loopTimes.loop();
+
+            TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
+            TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
+            TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
+            TELE.addData("X:", TeleopV4.teleStart.getX());
+            TELE.addData("Y:", TeleopV4.teleStart.getY());
+            TELE.addData("H:", TeleopV4.teleStart.getHeading());
+            TELE.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto15Ball_Back.java

@@ -0,0 +1,361 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+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.teleop.TeleopV4;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
+import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
+
+import java.util.List;
+
+@Config
+@Autonomous (preselectTeleOp = "TeleopV4")
+public class Auto15Ball_Back extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    Follower follower;
+    MeasuringLoopTimes loopTimes;
+    Shooter shooter;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+    SpindexerTransferIntake spindexer;
+
+    // Wait Times
+    public static double rapidWaitTime = 0.5;
+    public static double rapidShootTime = 0.8;
+    public static double loadPickupTime = 3;
+
+    // Initialize path state machine
+    private enum PathState {
+        WAIT_VELOCITY, WAIT_SHOOT0,
+        PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3,
+        PICKUP_LOAD, DRIVE_SHOOT_LOAD, WAIT_SHOOT_LOAD,
+        INTAKE_GATE, DRIVE_SHOOT_GATE, WAIT_SHOOT_GATE, LEAVE
+    }
+    PathState pathState = PathState.WAIT_VELOCITY;
+
+    // Poses
+    public static double startPoseX = 12, 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 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 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 shootGateControlX = 53.8705612829324, shootGateControlY = -35.14501718213059;
+    public static double shootGateX = 16, shootGateY = -57, shootGateH = 0;
+    public static double leaveX = 40, leaveY = 14, leaveH = 0;
+    double[] xPoses = {startPoseX, pickup3ControlX, pickup3X, shoot3X,
+            pickupLoadControlX, pickupLoadX, shootLoadControlX, shootLoadX,
+            intakeGateControl1X, intakeGateControl2X, intakeGateX, shootGateControlX, shootGateX, leaveX};
+    double[] yPoses = {startPoseY, pickup3ControlY, pickup3Y, shoot3Y,
+            pickupLoadControlY, pickupLoadY, shootLoadControlY, shootLoadY,
+            intakeGateControl1Y, intakeGateControl2Y, intakeGateY, shootGateControlY, shootGateY, leaveY};
+    double[] headings = {startPoseH, 0, pickup3H, shoot3H,
+            0, pickupLoadH, 0, shootLoadH,
+            0, 0, intakeGateH, 0, shootGateH, leaveH};
+    Pose startPose, pickup3Control, pickup3, shoot3,
+            pickupLoadControl, pickupLoad, shootLoadControl, shootLoad,
+            intakeGateControl1, intakeGateControl2, intakeGate, shootGateControl, shootGate, leave;
+    private void initializePoses(){
+        startPose = new Pose(xPoses[0], yPoses[0], Math.toRadians(headings[0]));
+        pickup3Control = new Pose(xPoses[1], yPoses[1]);
+        pickup3 = new Pose(xPoses[2], yPoses[2], Math.toRadians(headings[2]));
+        shoot3 = new Pose(xPoses[3], yPoses[3], Math.toRadians(headings[3]));
+        pickupLoadControl = new Pose(xPoses[4], yPoses[4]);
+        pickupLoad = new Pose(xPoses[5], yPoses[5], Math.toRadians(headings[5]));
+        shootLoadControl = new Pose(xPoses[6], yPoses[6]);
+        shootLoad = new Pose(xPoses[7], yPoses[7], Math.toRadians(headings[7]));
+        intakeGateControl1 = new Pose(xPoses[8], yPoses[8]);
+        intakeGateControl2 = new Pose(xPoses[9], yPoses[9]);
+        intakeGate = new Pose(xPoses[10], yPoses[10], Math.toRadians(headings[10]));
+        shootGateControl = new Pose(xPoses[11], yPoses[11]);
+        shootGate = new Pose(xPoses[12], yPoses[12], Math.toRadians(headings[12]));
+        leave = new Pose(xPoses[13], yPoses[13], Math.toRadians(headings[13]));
+    }
+
+    //Building Paths
+    PathChain startPose_pickup3, pickup3_shoot3, shoot3_pickupLoad, pickupLoad_shootLoad,
+            shootLoad_intakeGate, intakeGate_shootGate, shootGate_intakeGate, shootGate_leave;
+    private void buildPaths(){
+        startPose_pickup3 = follower.pathBuilder()
+                .addPath(new BezierCurve(startPose, pickup3Control, pickup3))
+                .setTangentHeadingInterpolation()
+                .build();
+
+        pickup3_shoot3 = follower.pathBuilder()
+                .addPath(new BezierLine(pickup3, shoot3))
+                .setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading())
+                .build();
+
+        shoot3_pickupLoad = follower.pathBuilder()
+                .addPath(new BezierCurve(shoot3, pickupLoadControl, pickupLoad))
+                .setLinearHeadingInterpolation(shoot3.getHeading(), pickupLoad.getHeading())
+                .build();
+
+        pickupLoad_shootLoad = follower.pathBuilder()
+                .addPath(new BezierCurve(pickupLoad, shootLoadControl, shootLoad))
+                .setLinearHeadingInterpolation(pickupLoad.getHeading(), shootLoad.getHeading())
+                .build();
+
+        shootLoad_intakeGate = follower.pathBuilder()
+                .addPath(new BezierCurve(shootLoad, intakeGateControl1, intakeGateControl2, intakeGate))
+                .setTangentHeadingInterpolation()
+                .build();
+
+        intakeGate_shootGate = follower.pathBuilder()
+                .addPath(new BezierCurve(intakeGate, shootGateControl, shootGate))
+                .setLinearHeadingInterpolation(intakeGate.getHeading(), shootGate.getHeading())
+                .build();
+
+        shootGate_intakeGate = follower.pathBuilder()
+                .addPath(new BezierCurve(shootGate, intakeGateControl1, intakeGateControl2, intakeGate))
+                .setTangentHeadingInterpolation()
+                .build();
+
+        shootGate_leave = follower.pathBuilder()
+                .addPath(new BezierLine(shootGate, leave))
+                .setLinearHeadingInterpolation(shootGate.getHeading(), leave.getHeading())
+                .build();
+    }
+
+    //Path State Machine
+    private int startAuto = 0;
+    private double timeStamp = 0;
+    private void pathStateMachine(){
+        double currentTime = (double) System.currentTimeMillis() / 1000;
+        switch(pathState){
+            case WAIT_VELOCITY:
+                spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                shooter.setFlywheelVelocity(2400);
+                robot.setHoodPos(0.64);
+                if (flywheel.getSteady()){
+                    startAuto++;
+                }
+                if (startAuto > 5){
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT0;
+                }
+                break;
+            case WAIT_SHOOT0:
+                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);
+                    pathState = PathState.DRIVE_SHOOT3;
+                    timeStamp = currentTime;
+                }
+                break;
+            case DRIVE_SHOOT3:
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT3;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } 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)){
+                    follower.followPath(shoot3_pickupLoad, false);
+                    pathState = PathState.PICKUP_LOAD;
+                    timeStamp = currentTime;
+                }
+                break;
+            case PICKUP_LOAD:
+                if (currentTime - timeStamp > loadPickupTime){
+                    follower.followPath(pickupLoad_shootLoad, false);
+                    pathState = PathState.DRIVE_SHOOT_LOAD;
+                    timeStamp = currentTime;
+                }
+                break;
+            case DRIVE_SHOOT_LOAD:
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT_LOAD;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } 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)){
+                    follower.followPath(shootLoad_intakeGate, false);
+                    pathState = PathState.INTAKE_GATE;
+                    timeStamp = currentTime;
+                }
+                break;
+            case INTAKE_GATE:
+                if (!follower.isBusy()){
+                    follower.followPath(intakeGate_shootGate, false);
+                    pathState = PathState.DRIVE_SHOOT_GATE;
+                    timeStamp = currentTime;
+                }
+                break;
+            case DRIVE_SHOOT_GATE:
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT_GATE;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } 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;
+                    timeStamp = currentTime;
+                    // TODO: add logic for leave
+                }
+                break;
+            case LEAVE:
+                // add line here to say "done auto"
+                break;
+            default:
+                break;
+        }
+        TELE.update(); // use for debugging
+    }
+
+    // Used for changing alliance
+    private double adjustXPoseBasedOnAlliance(double pose) {return -pose;}
+    private double adjustHeadingBasedOnAlliance(double heading){
+        heading = 180 - heading;
+        while (heading > 180) {heading-=360;}
+        while (heading <= -180) {heading+=360;}
+        return heading;
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Robot.resetInstance();
+        robot = Robot.getInstance(hardwareMap);
+        List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        follower = Constants.createFollower(hardwareMap);
+        sleep(1000);
+        follower.setStartingPose(new Pose(0,0,0));
+        loopTimes = new MeasuringLoopTimes();
+        loopTimes.init();
+        turret = new Turret(robot);
+        flywheel = new Flywheel(robot);
+        commander = new VelocityCommander();
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        spindexer = new SpindexerTransferIntake(robot, TELE, commander);
+        ParkTilter park = new ParkTilter(robot);
+
+        boolean initializeRobot = false;
+        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);
+            }
+
+            if (gamepad1.crossWasPressed() && !initializeRobot){
+                Color.redAlliance = !Color.redAlliance;
+                shooter.setRedAlliance(Color.redAlliance);
+            }
+
+            if (!initializeRobot){
+                if ((Color.redAlliance && xPoses[0] < 0)
+                        || (!Color.redAlliance && xPoses[0] > 0)){
+                    for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
+                    for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
+                }
+            }
+
+            if (gamepad1.triangleWasPressed()){
+                initializeRobot = true;
+                initializePoses();
+                follower.setPose(startPose);
+                buildPaths();
+                sleep(2000);
+                turret.switchPipeline(Turret.PipelineMode.TRACKING);
+                robot.limelight.start();
+                limelightUsed = true;
+                park.unpark();
+            }
+
+            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());
+            TELE.addData("Current LL Pipeline", turret.pipeline());
+            TELE.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()){
+            shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            shooter.update(robot.voltage.getVoltage());
+
+            if (!isStopRequested()){
+                follower.update();
+            }
+
+            pathStateMachine();
+            TeleopV4.teleStart = follower.getPose();
+
+            spindexer.update();
+
+            for (LynxModule hub : allHubs) {
+                hub.clearBulkCache();
+            }
+            loopTimes.loop();
+
+            TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
+            TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
+            TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
+            TELE.addData("X:", TeleopV4.teleStart.getX());
+            TELE.addData("Y:", TeleopV4.teleStart.getY());
+            TELE.addData("H:", TeleopV4.teleStart.getHeading());
+            TELE.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java

@@ -0,0 +1,462 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+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.teleop.TeleopV4;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
+import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
+
+import java.util.List;
+
+@Config
+@Autonomous (preselectTeleOp = "TeleopV4")
+public class Auto21Ball_Front_Gate extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    Follower follower;
+    MeasuringLoopTimes loopTimes;
+    Shooter shooter;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+    SpindexerTransferIntake spindexer;
+    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 openGateWaitTimeMax = 3;
+    public static double openGateWaitTimeMin = 1.75;
+    public static int maxLoopCycles = 3;
+
+    // Initialize path state machine
+    private enum PathState {
+        DRIVE_SHOOT0, WAIT_SHOOT0,
+        PICKUP1, OPENGATE1, DRIVE_SHOOT1, WAIT_SHOOT1,
+        PICKUP2, OPENGATE2, DRIVE_SHOOT2, WAIT_SHOOT2,
+        INTAKE_GATE, DRIVE_SHOOT_GATE, WAIT_SHOOT_GATE, DRIVE_SHOOT_LEAVE, WAIT_SHOOT_LEAVE
+    }
+    PathState pathState = PathState.DRIVE_SHOOT0;
+
+    // Poses
+    public static double startPoseX = 55, startPoseY = 39, startPoseH = 0;
+    public static double shoot0X = 25, shoot0Y = 18, shoot0H = 0;
+    public static double pickup1ControlX = 29.53321878579611, pickup1ControlY = 9.84077892325314;
+    public static double pickup1X = 50, pickup1Y = 10, pickup1H = 0;
+    public static double openGate1ControlX = 43.82989690721648, openGate1ControlY = 3.86540664375714;
+    public static double openGate1X = 59, openGate1Y = 2, openGate1H = 0;
+    public static double shoot1ControlX = 40, shoot1ControlY = 3;
+    public static double shoot1X = 25, shoot1Y = 11, shoot1H = -30;
+    public static double pickup2ControlX = 18, pickup2ControlY = -18;
+    public static double pickup2X = 61, pickup2Y = -14.5, pickup2H = 0;
+    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 shootLeaveControlX = 56, shootLeaveControlY = -10;
+    public static double shootLeaveX = 16, shootLeaveY = 36, shootLeaveH = -50;
+    public static double leaveX = 45, leaveY = 10, leaveH = 0;
+    public static double awayFromGateX = 40, awayFromGateY = -12, awayFromGateH = 0;
+    double[] xPoses = {startPoseX, shoot0X,
+            pickup1ControlX, pickup1X, openGate1ControlX, openGate1X, shoot1ControlX, shoot1X,
+            pickup2ControlX, pickup2X, openGate2ControlX, openGate2X, shoot2ControlX, shoot2X,
+            intakeGateControlX, intakeGateX, shootGateControlX, shootGateX,
+            shootLeaveControlX, shootLeaveX, leaveX, awayFromGateX};
+    double[] yPoses = {startPoseY, shoot0Y,
+            pickup1ControlY, pickup1Y, openGate1ControlY, openGate1Y, shoot1ControlY, shoot1Y,
+            pickup2ControlY, pickup2Y, openGate2ControlY, openGate2Y, shoot2ControlY, shoot2Y,
+            intakeGateControlY, intakeGateY, shootGateControlY, shootGateY,
+            shootLeaveControlY, shootLeaveY, leaveY, awayFromGateY};
+    double[] headings = {startPoseH, shoot0H,
+            0, pickup1H, 0, openGate1H, 0, shoot1H,
+            0, pickup2H, 0, openGate2H, 0, shoot2H,
+            0, intakeGateH, 0, shootGateH,
+            0, shootLeaveH, leaveH, awayFromGateH};
+    Pose startPose, shoot0,
+            pickup1Control, pickup1, openGate1Control, openGate1, shoot1Control, shoot1,
+            pickup2Control, pickup2, openGate2Control, openGate2, shoot2Control, shoot2,
+            intakeGateControl, intakeGate, shootGateControl, shootGate,
+            shootLeaveControl, shootLeave, leave, awayFromGate;
+    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]));
+        pickup1Control = new Pose(xPoses[2], yPoses[2]);
+        pickup1 = new Pose(xPoses[3], yPoses[3], Math.toRadians(headings[3]));
+        openGate1Control = new Pose(xPoses[4], yPoses[4]);
+        openGate1 = new Pose(xPoses[5], yPoses[5], Math.toRadians(headings[5]));
+        shoot1Control = new Pose(xPoses[6], yPoses[6]);
+        shoot1 = new Pose(xPoses[7], yPoses[7], Math.toRadians(headings[7]));
+        pickup2Control = new Pose(xPoses[8], yPoses[8]);
+        pickup2 = new Pose(xPoses[9], yPoses[9], Math.toRadians(headings[9]));
+        openGate2Control = new Pose(xPoses[10], yPoses[10]);
+        openGate2 = new Pose(xPoses[11], yPoses[11], Math.toRadians(headings[11]));
+        shoot2Control = new Pose(xPoses[12], yPoses[12]);
+        shoot2 = new Pose(xPoses[13], yPoses[13], Math.toRadians(headings[13]));
+        intakeGateControl = new Pose(xPoses[14], yPoses[14]);
+        intakeGate = new Pose(xPoses[15], yPoses[15], Math.toRadians(headings[15]));
+        shootGateControl = new Pose(xPoses[16], yPoses[16]);
+        shootGate = new Pose(xPoses[17], yPoses[17], Math.toRadians(headings[17]));
+        shootLeaveControl = new Pose(xPoses[18], yPoses[18]);
+        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]));
+    }
+
+    //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;
+    private void buildPaths(){
+        startPose_shoot0 = follower.pathBuilder()
+                .addPath(new BezierLine(startPose, shoot0))
+                .setLinearHeadingInterpolation(startPose.getHeading(), shoot0.getHeading())
+                .build();
+
+        shoot0_pickup1 = follower.pathBuilder()
+                .addPath(new BezierCurve(shoot0, pickup1Control, pickup1))
+                .setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading())
+                .build();
+
+        pickup1_openGate1 = follower.pathBuilder()
+                .addPath(new BezierCurve(pickup1, openGate1Control, openGate1))
+                .setLinearHeadingInterpolation(pickup1.getHeading(), openGate1.getHeading())
+                .build();
+
+        openGate1_shoot1 = follower.pathBuilder()
+                .addPath(new BezierCurve(openGate1, shoot1Control, shoot1))
+                .setLinearHeadingInterpolation(openGate1.getHeading(), shoot1.getHeading())
+                .build();
+
+        shoot1_pickup2 = follower.pathBuilder()
+                .addPath(new BezierCurve(shoot1, pickup2Control, pickup2))
+                .setLinearHeadingInterpolation(shoot1.getHeading(), pickup2.getHeading())
+                .build();
+
+        pickup2_openGate2 = follower.pathBuilder()
+                .addPath(new BezierCurve(pickup2, openGate2Control, openGate2))
+                .setLinearHeadingInterpolation(pickup2.getHeading(), openGate2.getHeading())
+                .build();
+
+        openGate2_shoot2 = follower.pathBuilder()
+                .addPath(new BezierCurve(openGate2, shoot2Control, shoot2))
+                .setLinearHeadingInterpolation(openGate2.getHeading(), shoot2.getHeading())
+                .build();
+
+        shoot2_intakeGate = follower.pathBuilder()
+                .addPath(new BezierCurve(shoot2, intakeGateControl, intakeGate))
+                .setLinearHeadingInterpolation(shoot2.getHeading(), intakeGate.getHeading())
+                .build();
+
+        intakeGate_shootGate = follower.pathBuilder()
+                .addPath(new BezierCurve(intakeGate, shootGateControl, shootGate))
+                .setLinearHeadingInterpolation(intakeGate.getHeading(), shootGate.getHeading())
+                .build();
+
+        shootGate_intakeGate = follower.pathBuilder()
+                .addPath(new BezierCurve(shootGate, intakeGateControl, intakeGate))
+                .setLinearHeadingInterpolation(shootGate.getHeading(), intakeGate.getHeading())
+                .build();
+
+        intakeGate_shootLeave = follower.pathBuilder()
+                .addPath(new BezierCurve(intakeGate, shootLeaveControl, shootLeave))
+                .setLinearHeadingInterpolation(intakeGate.getHeading(), shootLeave.getHeading())
+                .build();
+
+        shootGate_leave = follower.pathBuilder()
+                .addPath(new BezierLine(shootGate, leave))
+                .setLinearHeadingInterpolation(shootGate.getHeading(), leave.getHeading())
+                .build();
+
+        intakeGate_awayFromGate = follower.pathBuilder()
+                .addPath(new BezierLine(intakeGate, awayFromGate))
+                .setLinearHeadingInterpolation(intakeGate.getHeading(), awayFromGate.getHeading())
+                .build();
+    }
+
+    //Path State Machine
+    private boolean startAuto = true;
+    private double timeStamp = 0;
+    private int cycle = 0;
+    private void pathStateMachine(){
+        double currentTime = (double) System.currentTimeMillis() / 1000;
+        switch(pathState){
+            case DRIVE_SHOOT0:
+                spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                shooter.setFlywheelVelocity(2300);
+                robot.setHoodPos(0.68);
+                if (startAuto){
+                    follower.followPath(startPose_shoot0, false);
+                    startAuto = false;
+                }
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT0;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } else if (follower.isBusy()){
+                    timeStamp = currentTime;
+                }
+                break;
+            case WAIT_SHOOT0:
+                if (currentTime - timeStamp > rapidShootTime ||
+                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
+                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
+                    follower.followPath(shoot0_pickup1, false);
+                    pathState = PathState.PICKUP1;
+                }
+                break;
+            case PICKUP1:
+                if (!follower.isBusy()){
+                    follower.followPath(pickup1_openGate1, false);
+                    pathState = PathState.OPENGATE1;
+                    timeStamp = currentTime;
+                }
+                break;
+            case OPENGATE1:
+                if (currentTime - timeStamp > openGate1Time){
+                    follower.followPath(openGate1_shoot1, false);
+                    pathState = PathState.DRIVE_SHOOT1;
+                    timeStamp = currentTime;
+                }
+                break;
+            case DRIVE_SHOOT1:
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT1;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } else if (follower.isBusy()){
+                    timeStamp = currentTime;
+                }
+                break;
+            case WAIT_SHOOT1:
+                if (currentTime - timeStamp > rapidShootTime ||
+                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
+                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
+                    follower.followPath(shoot1_pickup2, false);
+                    pathState = PathState.PICKUP2;
+                }
+                break;
+            case PICKUP2:
+                if (!follower.isBusy()){
+                    follower.followPath(pickup2_openGate2, false);
+                    pathState = PathState.OPENGATE2;
+                    timeStamp = currentTime;
+                }
+                break;
+            case OPENGATE2:
+                if (currentTime - timeStamp > openGate2Time){
+                    follower.followPath(openGate2_shoot2, false);
+                    pathState = PathState.DRIVE_SHOOT2;
+                    timeStamp = currentTime;
+                }
+                break;
+            case DRIVE_SHOOT2:
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT2;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } else if (follower.isBusy()){
+                    timeStamp = currentTime;
+                }
+                break;
+            case WAIT_SHOOT2:
+                if (currentTime - timeStamp > rapidShootTime ||
+                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
+                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
+                    follower.followPath(shoot2_intakeGate, false);
+                    pathState = PathState.INTAKE_GATE;
+                    timeStamp = currentTime;
+                }
+                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){
+                        follower.followPath(intakeGate_shootLeave, true);
+                        pathState = PathState.DRIVE_SHOOT_LEAVE;
+                    } else {
+                        follower.followPath(intakeGate_shootGate, false);
+                        pathState = PathState.DRIVE_SHOOT_GATE;
+                    }
+                    timeStamp = currentTime;
+                }
+                // TODO: add logic to shoot gate
+                break;
+            case DRIVE_SHOOT_GATE:
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT_GATE;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } 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)){
+                    cycle++;
+                    if (getRuntime() - runtime > 24 || cycle >= maxLoopCycles){
+                        follower.followPath(shootGate_leave, true);
+                        pathState = PathState.WAIT_SHOOT_LEAVE;
+                    } else {
+                        follower.followPath(shootGate_intakeGate, false);
+                        pathState = PathState.INTAKE_GATE;
+                    }
+                    timeStamp = currentTime;
+                }
+                break;
+            case DRIVE_SHOOT_LEAVE:
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT_LEAVE;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                } else if (follower.isBusy()){
+                    timeStamp = currentTime;
+                }
+                break;
+            case WAIT_SHOOT_LEAVE:
+                // add line here to say "done auto"
+                break;
+            default:
+                break;
+        }
+        TELE.addData("Moving?", follower.isBusy());
+    }
+
+    // Used for changing alliance
+    private double adjustXPoseBasedOnAlliance(double pose) {return -pose;}
+    private double adjustHeadingBasedOnAlliance(double heading){
+        heading = 180 - heading;
+        while (heading > 180) {heading-=360;}
+        while (heading <= -180) {heading+=360;}
+        return heading;
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Robot.resetInstance();
+        robot = Robot.getInstance(hardwareMap);
+        List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        follower = Constants.createFollower(hardwareMap);
+        sleep(1000);
+        follower.setStartingPose(new Pose(0,0,0));
+        loopTimes = new MeasuringLoopTimes();
+        loopTimes.init();
+        turret = new Turret(robot);
+        flywheel = new Flywheel(robot);
+        commander = new VelocityCommander();
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        spindexer = new SpindexerTransferIntake(robot, TELE, commander);
+        ParkTilter park = new ParkTilter(robot);
+        boolean startAuto = true;
+
+        boolean initializeRobot = false;
+        while (opModeInInit()){
+            follower.update();
+
+            if (gamepad1.squareWasPressed()){
+                robot.setSpinPos(ServoPositions.spindexer_A2);
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
+                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+            }
+
+            if (gamepad1.crossWasPressed() && !initializeRobot){
+                Color.redAlliance = !Color.redAlliance;
+                shooter.setRedAlliance(Color.redAlliance);
+            }
+
+            if (!initializeRobot){
+                if ((Color.redAlliance && xPoses[0] < 0)
+                        || (!Color.redAlliance && xPoses[0] > 0)){
+                    for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
+                    for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
+                }
+            }
+
+            if (gamepad1.triangleWasPressed()){
+                initializeRobot = true;
+                initializePoses();
+                follower.setPose(startPose);
+                buildPaths();
+                sleep(2000);
+                turret.switchPipeline(Turret.PipelineMode.TRACKING);
+                robot.limelight.start();
+                limelightUsed = true;
+                park.unpark();
+            }
+
+            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());
+            TELE.addData("Current LL Pipeline", turret.pipeline());
+            TELE.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()){
+            if (startAuto){
+                runtime = getRuntime();
+                startAuto = false;
+            }
+
+            park.unpark();
+            robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+
+            shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            shooter.update(robot.voltage.getVoltage());
+
+            if (!isStopRequested()){
+                follower.update();
+            }
+
+            pathStateMachine();
+            TeleopV4.teleStart = follower.getPose();
+
+            spindexer.update();
+
+            for (LynxModule hub : allHubs) {
+                hub.clearBulkCache();
+            }
+            loopTimes.loop();
+
+            TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
+            TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
+            TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
+            TELE.addData("X:", TeleopV4.teleStart.getX());
+            TELE.addData("Y:", TeleopV4.teleStart.getY());
+            TELE.addData("H:", TeleopV4.teleStart.getHeading());
+            TELE.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java

@@ -51,8 +51,7 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
-@Config
+
-@Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Close extends LinearOpMode {
     public static double shoot0Vel = 2300, shoot0Hood = 0.93;
     public static double velGate0Start = 2700, hoodGate0Start = 0.6;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java

@@ -43,8 +43,7 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
-@Config
+
-@Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far extends LinearOpMode {
     public static double shoot0Vel = 3300, shoot0Hood = 0.48;
     double xLeave, yLeave, hLeave;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java

@@ -37,7 +37,6 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 
 import java.util.Objects;
 
-@Config
 public class AutoActions {
     Robot robot;
     MultipleTelemetry TELE;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java

@@ -85,9 +85,7 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
-@Disabled
+
-@Config
-@Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Close_12Ball extends LinearOpMode {
     public static double shoot0Vel = 2300, shoot0Hood = 0.93;
     public static double autoSpinStartPos = 0.2;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java

@@ -93,9 +93,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 
 import java.util.Objects;
 
-@Disabled
+
-@Config
-@Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Close_GateOpen extends LinearOpMode {
     public static double shoot0Vel = 2300, shoot0Hood = 0.93;
     public static double autoSpinStartPos = 0.2;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java

@@ -82,9 +82,7 @@ import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 
 import java.util.List;
-@Disabled
+
-@Config
-@Autonomous(preselectTeleOp = "TeleopV3")
 public class ProtoAutoClose_V3 extends LinearOpMode {
     public static double intake1Time = 2.7;
     public static double intake2Time = 3.0;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double rapidFireBlocker_Closed = 0.35;
+    public static double rapidFireBlocker_Closed = 0.32;
     public static double rapidFireBlocker_Open = 0.5;
 
     public static double spindexBlocker_Closed = 0.31;
@@ -34,9 +34,9 @@ public class ServoPositions {
 
     public static double shootAllSpindexerSpeedIncrease = 0.01;
 
-    public static double transferServo_out = 0.57;
+    public static double transferServo_out = 0.28;
 
-    public static double transferServo_in = 0.77;
+    public static double transferServo_in = 0.54;
 
     public static double hoodAuto = 0.27;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java

@@ -5,7 +5,7 @@ import com.pedropathing.control.FilteredPIDFCoefficients;
 import com.pedropathing.control.PIDFCoefficients;
 import com.pedropathing.follower.Follower;
 import com.pedropathing.follower.FollowerConstants;
-import com.pedropathing.ftc.FollowerBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.FollowerBuilder;
 import com.pedropathing.ftc.drivetrains.MecanumConstants;
 import com.pedropathing.ftc.localization.constants.PinpointConstants;
 import com.pedropathing.paths.PathConstraints;
@@ -18,9 +18,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 public class Constants {
     public static FollowerConstants followerConstants = new FollowerConstants()
             .mass(14.37888)
-            .forwardZeroPowerAcceleration(-29.512)
+            .forwardZeroPowerAcceleration(-30.322)
-            .lateralZeroPowerAcceleration(-72.872)
+            .lateralZeroPowerAcceleration(-60.876)
-            .centripetalScaling(0);
+            .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02))
+            .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02))
+            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.013, 0, 0.00001, 0.6, 0.015))
+            .centripetalScaling(0.0005);
 
     public static MecanumConstants driveConstants = new MecanumConstants()
             .maxPower(1)
@@ -32,15 +35,15 @@ public class Constants {
             .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
             .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
             .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
-            .xVelocity(64.675)
+            .xVelocity(84.376)
-            .yVelocity(49.583);
+            .yVelocity(64.052);
 
     public static double breakingStrength = 1;
     public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1);
 
     public static PinpointConstants localizerConstants = new PinpointConstants()
-            .forwardPodY(-3.676)
+            .forwardPodY(-3.7795)
-            .strafePodX(3.780)
+            .strafePodX(-3.769)
             .distanceUnit(DistanceUnit.INCH)
             .hardwareMapName("pinpoint")
             .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)
@@ -48,10 +51,16 @@ public class Constants {
             .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD);
 
     public static Follower createFollower(HardwareMap hardwareMap) {
-        return new FollowerBuilder(followerConstants, hardwareMap)
+        FollowerBuilder followerBuilder;
+        followerBuilder = new FollowerBuilder(followerConstants, hardwareMap)
                 .pathConstraints(pathConstraints)
                 .mecanumDrivetrain(driveConstants)
-                .pinpointLocalizer(localizerConstants)
+                .pinpointLocalizer(localizerConstants);
-                .build();
+
+        followerBuilder.resetPinpoint();
+        followerBuilder.recalibrateIMU();
+
+        return followerBuilder.build();
+
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/FollowerBuilder.java

@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.pedroPathing;
+import com.pedropathing.drivetrain.Drivetrain;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.ftc.drivetrains.*;
+import com.pedropathing.ftc.localization.constants.DriveEncoderConstants;
+import com.pedropathing.ftc.localization.constants.OTOSConstants;
+import com.pedropathing.ftc.localization.constants.PinpointConstants;
+import com.pedropathing.ftc.localization.constants.ThreeWheelConstants;
+import com.pedropathing.ftc.localization.constants.ThreeWheelIMUConstants;
+import com.pedropathing.ftc.localization.constants.TwoWheelConstants;
+import com.pedropathing.ftc.localization.localizers.DriveEncoderLocalizer;
+import com.pedropathing.ftc.localization.localizers.OTOSLocalizer;
+import com.pedropathing.ftc.localization.localizers.PinpointLocalizer;
+import com.pedropathing.ftc.localization.localizers.ThreeWheelIMULocalizer;
+import com.pedropathing.ftc.localization.localizers.ThreeWheelLocalizer;
+import com.pedropathing.ftc.localization.localizers.TwoWheelLocalizer;
+import com.pedropathing.localization.Localizer;
+import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+/** This is the FollowerBuilder.
+ * It is used to create Followers with a specific drivetrain + localizer without having to use a full constructor
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ */
+public class FollowerBuilder {
+    private final FollowerConstants constants;
+    private PathConstraints constraints;
+    private final HardwareMap hardwareMap;
+    private Localizer localizer;
+    private Drivetrain drivetrain;
+
+    public FollowerBuilder(FollowerConstants constants, HardwareMap hardwareMap) {
+        this.constants = constants;
+        this.hardwareMap = hardwareMap;
+        constraints = PathConstraints.defaultConstraints;
+    }
+
+    public FollowerBuilder setLocalizer(Localizer localizer) {
+        this.localizer = localizer;
+        return this;
+    }
+
+    public FollowerBuilder driveEncoderLocalizer(DriveEncoderConstants lConstants) {
+        return setLocalizer(new DriveEncoderLocalizer(hardwareMap, lConstants));
+    }
+
+    public FollowerBuilder OTOSLocalizer(OTOSConstants lConstants) {
+        return setLocalizer(new OTOSLocalizer(hardwareMap, lConstants));
+    }
+
+    PinpointLocalizer pinpointLocalizer;
+    public FollowerBuilder pinpointLocalizer(PinpointConstants lConstants) {
+        pinpointLocalizer = new PinpointLocalizer(hardwareMap, lConstants);
+        return setLocalizer(pinpointLocalizer);
+    }
+
+    public void resetPinpoint(){
+        pinpointLocalizer.resetIMU();
+    }
+
+    public void recalibrateIMU(){
+        pinpointLocalizer.recalibrate();
+    }
+
+    public FollowerBuilder threeWheelIMULocalizer(ThreeWheelIMUConstants lConstants) {
+        return setLocalizer(new ThreeWheelIMULocalizer(hardwareMap, lConstants));
+    }
+
+    public FollowerBuilder threeWheelLocalizer(ThreeWheelConstants lConstants) {
+        return setLocalizer(new ThreeWheelLocalizer(hardwareMap, lConstants));
+    }
+
+    public FollowerBuilder twoWheelLocalizer(TwoWheelConstants lConstants) {
+        return setLocalizer(new TwoWheelLocalizer(hardwareMap, lConstants));
+    }
+
+    public FollowerBuilder setDrivetrain(Drivetrain drivetrain) {
+        this.drivetrain = drivetrain;
+        return this;
+    }
+
+    public FollowerBuilder mecanumDrivetrain(MecanumConstants mecanumConstants) {
+        return setDrivetrain(new Mecanum(hardwareMap, mecanumConstants));
+    }
+
+    @Deprecated
+    public FollowerBuilder mecanumExDrivetrain(MecanumConstants mecanumConstants) {
+        return setDrivetrain(new MecanumEx(hardwareMap, mecanumConstants));
+    }
+
+    public FollowerBuilder swerveDrivetrain(SwerveConstants swerveConstants, SwervePod... pods) {
+        return setDrivetrain(new Swerve(hardwareMap, swerveConstants, pods));
+    }
+
+    public FollowerBuilder pathConstraints(PathConstraints pathConstraints) {
+        this.constraints = pathConstraints;
+        PathConstraints.setDefaultConstraints(pathConstraints);
+        return this;
+    }
+
+    public Follower build() {
+        return new Follower(constants, localizer, drivetrain, constraints);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java

@@ -148,7 +148,7 @@ class LocalizationTest extends OpMode {
 
     @Override
     public void init() {
-        follower.setStartingPose(new Pose(72,72, 0));
+        follower.setStartingPose(new Pose(0,0, 0));
     }
 
     /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */
@@ -416,7 +416,7 @@ class ForwardVelocityTuner extends OpMode {
 
 
         if (!end) {
-            if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) {
+            if (Math.abs(follower.getPose().getX()) > (DISTANCE)) {
                 end = true;
                 stopRobot();
             } else {
@@ -524,7 +524,7 @@ class LateralVelocityTuner extends OpMode {
         drawCurrentAndHistory();
 
         if (!end) {
-            if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) {
+            if (Math.abs(follower.getPose().getY()) > (DISTANCE)) {
                 end = true;
                 stopRobot();
             } else {
@@ -1272,7 +1272,7 @@ class CentripetalTuner extends OpMode {
     public void start() {
         follower.activateAllPIDFs();
         forwards = new Path(new BezierCurve(new Pose(0,0), new Pose(Math.abs(DISTANCE) ,0), new Pose(Math.abs(DISTANCE) ,DISTANCE )));
-        backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) ,0), new Pose(0,0)));
+        backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 0,DISTANCE + 0), new Pose(Math.abs(DISTANCE) ,0), new Pose(0,0)));
 
         backwards.setTangentHeadingInterpolation();
         backwards.reverseHeadingInterpolation();
@@ -1382,14 +1382,14 @@ class Circle extends OpMode {
 
     public void start() {
         circle = follower.pathBuilder()
-                .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72)))
+                .addPath(new BezierCurve(new Pose(0, 0), new Pose(RADIUS + 0, 0), new Pose(RADIUS + 0, RADIUS + 0)))
-                .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
+                .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0))
-                .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72)))
+                .addPath(new BezierCurve(new Pose(RADIUS + 0, RADIUS + 0), new Pose(RADIUS + 0, (2 * RADIUS) + 0), new Pose(0, (2 * RADIUS) + 0)))
-                .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
+                .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0))
-                .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72)))
+                .addPath(new BezierCurve(new Pose(0, (2 * RADIUS) + 0), new Pose(-RADIUS + 0, (2 * RADIUS) + 0), new Pose(-RADIUS + 0, RADIUS + 0)))
-                .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
+                .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0))
-                .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72)))
+                .addPath(new BezierCurve(new Pose(-RADIUS + 0, RADIUS + 0), new Pose(-RADIUS + 0, 0), new Pose(0, 0)))
-                .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
+                .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS + 0))
                 .build();
         follower.followPath(circle);
     }
@@ -1406,7 +1406,7 @@ class Circle extends OpMode {
 
     @Override
     public void init() {
-        follower.setStartingPose(new Pose(72, 72));
+        follower.setStartingPose(new Pose(0, 0));
     }
 
     /**
@@ -1639,8 +1639,8 @@ class OffsetsTuner extends OpMode {
         telemetry.addLine("Total Angle: " + follower.getTotalHeading());
 
         telemetry.addLine("The following values are the offsets in inches that should be applied to your localizer.");
-        telemetry.addLine("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0));
+        telemetry.addLine("strafeX: " + ((0.0-follower.getPose().getX()) / 2.0));
-        telemetry.addLine("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0));
+        telemetry.addLine("forwardY: " + ((0.0-follower.getPose().getY()) / 2.0));
         telemetry.update();
 
         drawCurrentAndHistory();

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

@@ -1,9 +1,5 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
-import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
-
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -14,8 +10,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.utils.MeasuringLoopTimes;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 
+import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
+
 @TeleOp
 @Config
 public class TeleopV4 extends LinearOpMode {
@@ -25,47 +24,146 @@ public class TeleopV4 extends LinearOpMode {
     MultipleTelemetry TELE;
     Follower follower;
     SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+    ParkTilter parkTilter;
+    MeasuringLoopTimes loopTimes;
+
+    public static Pose relocalizePose = new Pose(56, 11, 0);
+    public static Pose teleStart = new Pose(0,0,0);
+
+    private boolean firstTickFull = true;
+    private boolean intakeFull = true;
+    private boolean shooting = false;
 
     @Override
     public void runOpMode() throws InterruptedException {
 
+        Robot.resetInstance();
+
         robot = Robot.getInstance(hardwareMap);
 
         TELE = new MultipleTelemetry(
                 FtcDashboard.getInstance().getTelemetry(), telemetry
         );
 
+        commander = new VelocityCommander();
         drivetrain = new Drivetrain(robot, TELE);
         follower = Constants.createFollower(hardwareMap);
-        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(teleStart);
-        follower.setStartingPose(start);
+        sleep(500);
+        follower.setPose(teleStart);
+        follower.update();
 
-        shooter = new Shooter(robot, TELE, follower, Color.redAlliance);
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        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.TRACK_GOAL);
-        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
+        shooter.setRedAlliance(Color.redAlliance);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
+        turret.switchPipeline(Turret.PipelineMode.TRACKING);
+        robot.limelight.start();
+
+        limelightUsed = true;
+
+        TELE.addLine("Initialization is done");
+        TELE.addData("Starting Position", follower.getPose());
+        TELE.addData("TELE START", teleStart);
+        TELE.update();
+
+        while (opModeInInit()){
+            if (gamepad1.triangleWasPressed()){
+                VelocityCommander.lockFront = true;
+                VelocityCommander.lockBack = false;
+            } else if (gamepad1.squareWasPressed()){
+                VelocityCommander.lockBack = true;
+                VelocityCommander.lockFront = false;
+            } else if (gamepad1.circleWasPressed()){
+                VelocityCommander.lockBack = false;
+                VelocityCommander.lockFront = false;
+            }
+            TELE.addLine("Initialization is done");
+            TELE.addData("Starting Position", follower.getPose());
+            TELE.addData("TELE START", teleStart);
+            TELE.addData("Front?:", VelocityCommander.lockFront);
+            TELE.addData("Back?:", VelocityCommander.lockBack);
+            TELE.update();
+        }
 
         waitForStart();
 
         if (isStopRequested()) return;
 
+        int emptyTicks = 0;
         while (opModeIsActive()) {
-
             //Drivetrain
-
             drivetrain.drive(
                     -gamepad1.right_stick_y,
                     gamepad1.right_stick_x,
-                    gamepad1.left_stick_x
+                    gamepad1.left_stick_x,
+                    gamepad1.left_trigger
             );
 
-            shooter.update();
+            if (gamepad1.crossWasPressed()){
+                if (Color.redAlliance){
+                    relocalizePose = new Pose(57.5, 5, 0);
+                } else {
+                    relocalizePose = new Pose(-57.5, 5, Math.toRadians(180));
+                }
+                follower.setPose(relocalizePose);
+                sleep(500);
+                gamepad1.rumble(100);
+            }
+
+            follower.update();
+            Pose currentPose = follower.getPose();
+
+            if (gamepad1.dpadLeftWasPressed()){
+                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            }
+
+            if (gamepad1.dpadRightWasPressed()){
+                shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+            }
+
+            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
+                shooter.setFlywheelVelocity(2500);
+                robot.setHoodPos(0.6);
+            }
+
+            if (gamepad1.triangleWasPressed()){
+                VelocityCommander.lockFront = true;
+                VelocityCommander.lockBack = false;
+                TELE.addData("Front?:", true);
+                TELE.addData("Back?:", false);
+            } else if (gamepad1.squareWasPressed()){
+                VelocityCommander.lockBack = true;
+                VelocityCommander.lockFront = false;
+                TELE.addData("Front?:", false);
+                TELE.addData("Back?:", true);
+            } else if (gamepad1.circleWasPressed()){
+                VelocityCommander.lockBack = false;
+                VelocityCommander.lockFront = false;
+                TELE.addData("Front?:", false);
+                TELE.addData("Back?:", true);
+            }
+
+            shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
 
             SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
 
-            if (gamepad1.xWasPressed() &&
+            if (gamepad1.leftBumperWasPressed() &&
                     (state == SpindexerTransferIntake.RapidMode.INTAKE ||
                             state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
                             state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
@@ -74,9 +172,28 @@ public class TeleopV4 extends LinearOpMode {
                             state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
 
                 spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
+                intakeFull = false;
+                firstTickFull = true;
+                shooting = true;
             }
 
-            if (gamepad1.aWasPressed() &&
+            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)) {
 
@@ -85,15 +202,38 @@ public class TeleopV4 extends LinearOpMode {
                 );
             }
 
-            if (gamepad1.yWasPressed()
+            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
-                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
+                robot.setIntakePower(1);
-
+                robot.setTransferPower(-0.7);
-                spindexerTransferIntake.setRapidMode(
-                        SpindexerTransferIntake.RapidMode.INTAKE
-                );
             }
 
+            if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+            } else if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
+            }
 
+            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("Transfer Power", robot.transfer.getPower());
+            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+//
+//            TELE.addData("Current Position", currentPose);
+//
+//            TELE.addData("Current LL Pipeline", turret.pipeline());
+//
             TELE.update();
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java

@@ -9,8 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
-@Config
+
-@TeleOp
 public class AprilTagWebcamExample extends OpMode {
 
     MultipleTelemetry TELE;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java

@@ -12,8 +12,7 @@ import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
-@Config
+
-@TeleOp
 public class ColorTest extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.tests;
 
+import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -8,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 
 @Config
 @TeleOp
@@ -50,8 +51,9 @@ public class Hardware_Tester extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        robot = new Robot(hardwareMap);
+        Robot.resetInstance();
-        TELE = new MultipleTelemetry();
+        robot = Robot.getInstance(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
         robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -122,13 +124,14 @@ public class Hardware_Tester extends LinearOpMode {
 
             // Sensor Data
 
-//            TELE.addData("Beam Break 1?", robot.beam1.isPressed());
+            TELE.addData("Beam Break insideBeam?", robot.insideBeam.isPressed());
+            TELE.addData("Beam Break outsideBeam?", robot.outsideBeam.isPressed());
 //            TELE.addData("Beam Break 2?", robot.beam2.isPressed());
 //            TELE.addData("Beam Break 3?", robot.beam3.isPressed());
 
             NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
-            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM));
+            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM));
-            TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green));
+            TELE.addData("REV Green", revColor.blue / (revColor.green + revColor.blue + revColor.red));
 
             TELE.addData("Voltage Sensor", robot.voltage.getVoltage());
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java

@@ -7,8 +7,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
-@Config
-@TeleOp
 public class MotorDirectionDebugger extends LinearOpMode {
 
     public static double flPower = 0.0;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java

@@ -1,39 +1,38 @@
 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;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
-import org.firstinspires.ftc.teamcode.utilsv2.Robot;
+import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
-import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
 
-@Config
 @TeleOp
+@Config
 public class NewShooterTest extends LinearOpMode {
-
     Robot robot;
+    Drivetrain drivetrain;
+    Shooter shooter;
+    MultipleTelemetry TELE;
+    Follower follower;
+    SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+    ParkTilter parkTilter;
 
-    public static boolean intake = true;
+    public static int flywheelVelo = 0;
-    public static boolean shoot = false;
+    public static double hoodPos = 0.5;
-    public static double intakePower = 1.0;
+    public static double transferPower = -0.8;
-    public static double transferShootPower = -1;
+    public static boolean overrideTransferPower = false;
-    public static double transferIntakePower = -1.0;
-    public static double turretPos = 0.51;
-    public static double hoodPos = 0.51;
-    public static double flywheel = 0;
-
-    private enum ShootState {
-        IDLE,
-        WAIT_GATE,
-        WAIT_PUSH
-    }
-
-    private ShootState shootState = ShootState.IDLE;
-    private long timestamp = 0;
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -42,85 +41,128 @@ public class NewShooterTest extends LinearOpMode {
 
         robot = Robot.getInstance(hardwareMap);
 
-        Shooter shooter = new Shooter(
+        TELE = new MultipleTelemetry(
-                robot,
+                FtcDashboard.getInstance().getTelemetry(), telemetry
-                new MultipleTelemetry(
-                        telemetry,
-                        FtcDashboard.getInstance().getTelemetry()
-                ),
-                Constants.createFollower(hardwareMap),
-                true
         );
 
-        shooter.setState(Shooter.ShooterState.MANUAL);
+        commander = new VelocityCommander();
+        drivetrain = new Drivetrain(robot, TELE);
+        follower = Constants.createFollower(hardwareMap);
+        follower.setStartingPose(new Pose(0,0,0));
+        sleep(500);
+        follower.setPose(new Pose(0,0,0));
+
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        parkTilter = new ParkTilter(robot);
+        parkTilter.unpark();
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+        shooter.setRedAlliance(Color.redAlliance);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+
+        turret.switchPipeline(Turret.PipelineMode.TRACKING);
+        robot.limelight.start();
+
+        limelightUsed = true;
+
+        TELE.addLine("Initialization Complete");
+        TELE.update();
 
         waitForStart();
 
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
+            //Drivetrain
+            drivetrain.drive(
+                    -gamepad1.right_stick_y,
+                    gamepad1.right_stick_x,
+                    gamepad1.left_stick_x,
+                    gamepad1.left_trigger
+            );
 
-            robot.setHoodPos(hoodPos);
+            if (gamepad1.crossWasPressed()){
-            shooter.setTurretPosition(turretPos);
+                if (Color.redAlliance){
-            shooter.setFlywheelVelocity(flywheel);
+                    TeleopV4.relocalizePose = new Pose(57.5, 5, 0);
-            robot.setSpinPos(ServoPositions.spindexer_A2);
+                } else {
-
+                    TeleopV4.relocalizePose = new Pose(-57.5, 5, Math.toRadians(180));
-            if (intake && !shoot) {
-
-                shootState = ShootState.IDLE;
-
-                robot.setRapidFireBlockerPos(
-                        ServoPositions.rapidFireBlocker_Closed);
-
-                robot.setTransferPower(transferIntakePower);
-                robot.setIntakePower(intakePower);
-                robot.setTransferServoPos(
-                        ServoPositions.transferServo_out);
-            }
-
-            else if (shoot) {
-                robot.setIntakePower(intakePower);
-
-
-                switch (shootState) {
-
-                    case IDLE:
-
-                        robot.setTransferPower(transferShootPower);
-
-                        timestamp = System.currentTimeMillis();
-                        shootState = ShootState.WAIT_GATE;
-
-                        break;
-
-                    case WAIT_GATE:
-
-                        if (System.currentTimeMillis() - timestamp >= 300) {
-
-                            robot.setRapidFireBlockerPos(
-                                    ServoPositions.rapidFireBlocker_Open);
-
-                            timestamp = System.currentTimeMillis();
-                            shootState = ShootState.WAIT_PUSH;
-                        }
-
-                        break;
-
-                    case WAIT_PUSH:
-
-                        if (System.currentTimeMillis() - timestamp >= 100) {
-
-                            robot.setTransferServoPos(
-                                    ServoPositions.transferServo_in);
-
-                            shootState = ShootState.IDLE;
-                        }
-
-                        break;
                 }
+                follower.setPose(TeleopV4.relocalizePose);
+                sleep(500);
+                gamepad1.rumble(100);
             }
 
-            shooter.update();
+            follower.update();
+
+            if (gamepad1.dpadLeftWasPressed()){
+                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            }
+
+            if (gamepad1.dpadRightWasPressed()){
+                shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+            }
+
+            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
+                shooter.setFlywheelVelocity(flywheelVelo);
+                robot.setHoodPos(hoodPos);
+            }
+
+
+//            shooter.setTurretPosition(turretPos);
+            shooter.update(robot.voltage.getVoltage());
+            spindexerTransferIntake.update();
+
+            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();
+            }
+
+            TELE.addData("Distance From Goal", commander.getDistance());
+            TELE.addData("Hood Position", commander.getHoodPredicted());
+            TELE.addData("Transfer Power", commander.getTransferPow());
+            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.update();
         }
+
     }
-}
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

@@ -27,8 +27,6 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
-@Config
-@TeleOp
 public class ShooterTest extends LinearOpMode {
     public static int mode = 1;
     public static double parameter = 0.0;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java

@@ -0,0 +1,126 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+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.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
+
+@TeleOp
+@Config
+public class SortedSpindexerTest extends LinearOpMode {
+    Robot robot;
+    Drivetrain drivetrain;
+    Shooter shooter;
+    MultipleTelemetry TELE;
+    Follower follower;
+    SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+
+    ParkTilter parkTilter;
+    public static String order = "GPP";
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        Robot.resetInstance();
+
+        robot = Robot.getInstance(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                FtcDashboard.getInstance().getTelemetry(), telemetry
+        );
+
+        commander = new VelocityCommander();
+        drivetrain = new Drivetrain(robot, TELE);
+        follower = Constants.createFollower(hardwareMap);
+        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(start);
+
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        parkTilter = new ParkTilter(robot);
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+        shooter.setRedAlliance(Color.redAlliance);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SORTED);
+
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            switch(order) {
+                case "PPG":
+                    spindexerTransferIntake.setDesiredPattern(
+                            SpindexerTransferIntake.DesiredPattern.PPG
+                    );
+                    break;
+
+                case "PGP":
+                    spindexerTransferIntake.setDesiredPattern(
+                            SpindexerTransferIntake.DesiredPattern.PGP
+                    );
+                    break;
+
+                default:
+                    spindexerTransferIntake.setDesiredPattern(
+                            SpindexerTransferIntake.DesiredPattern.GPP
+                    );
+            }
+
+
+            //Drivetrain
+            drivetrain.drive(
+                    -gamepad1.right_stick_y,
+                    gamepad1.right_stick_x,
+                    gamepad1.left_stick_x,
+                    gamepad1.left_trigger
+            );
+
+            follower.update();
+
+
+            shooter.update(robot.voltage.getVoltage());
+            spindexerTransferIntake.update();
+
+            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
+
+            if(gamepad1.leftBumperWasPressed()) {
+                spindexerTransferIntake.startSortedShoot();
+            }
+
+            if (gamepad1.dpad_down){
+                parkTilter.park();
+            } else if (gamepad1.dpad_up) {
+                parkTilter.unpark();
+            }
+
+            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.update();
+        }
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java

@@ -18,8 +18,6 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
-@Config
-@TeleOp
 public class SortingTest extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java

@@ -15,8 +15,6 @@ import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Turret;
 
-@TeleOp
-@Config
 public class TurretTest extends LinearOpMode {
     public static boolean zeroTurr = false;
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java

@@ -7,7 +7,6 @@ import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.StateEnums.LightState;
 
 
-@Config
 public final class Light {
 
     private static Light instance;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java

@@ -43,13 +43,12 @@ public class MeasuringLoopTimes {
 
     public void loop() {
         currentTime = getTimeSeconds();
-        if ((MeasurementStart + 5.0) < currentTime)
+        if ((MeasurementStart + 60) < currentTime)
         {
             minLoopTime = 9999999.0;
             maxLoopTime = 0.0;
             MeasurementStart = currentTime;
 
-            avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker;
             avgLoopTimeSum = 0.0;
             avgLoopTimeTicker = 0;
         }
@@ -59,6 +58,7 @@ public class MeasuringLoopTimes {
 
         avgLoopTimeSum += mainLoopTime;
         avgLoopTimeTicker++;
+        avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker;
         minLoopTime = Math.min(minLoopTime,mainLoopTime);
         maxLoopTime = Math.max(maxLoopTime,mainLoopTime);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

@@ -10,8 +10,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
-@TeleOp
-@Config
 public class PositionalServoProgrammer extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -16,7 +16,6 @@ import com.qualcomm.robotcore.hardware.VoltageSensor;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
 
-@Config
 public class Robot {
 
     //Initialize Public Components

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java

@@ -6,7 +6,6 @@ import com.acmerobotics.dashboard.config.Config;
 import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-@Config
 public class Servos {
     //PID constants
     // TODO: get PIDF constants

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

@@ -19,7 +19,6 @@ import org.firstinspires.ftc.teamcode.teleop.TeleopV3;
 import java.util.ArrayList;
 import java.util.List;
 
-@Config
 
 public class Turret {
 

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

@@ -37,7 +37,7 @@ public class Drivetrain {
         tele = input;
     }
 
-    public void drive(double y, double x, double rx) {
+    public void drive(double y, double x, double rx, double stop) {
 
         boolean snappedForward = false;
         boolean snappedStrafe = false;
@@ -79,18 +79,30 @@ public class Drivetrain {
         robot.setFrontRightPower(frontRightPower);
         robot.setBackRightPower(backRightPower);
 
-        if (tele) {
+        if (stop > 0.5){
-
+            robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            telemetry.addData("Forward Snap", snappedForward);
+            robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            telemetry.addData("Strafe Snap", snappedStrafe);
+            robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
+            robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            telemetry.addData("Correction RX", correctionRX);
-
-            telemetry.addData("FL", frontLeftPower);
-            telemetry.addData("BL", backLeftPower);
-            telemetry.addData("FR", frontRightPower);
-            telemetry.addData("BR", backRightPower);
 
+            robot.setFrontLeftPower(0);
+            robot.setBackLeftPower(0);
+            robot.setFrontRightPower(0);
+            robot.setBackRightPower(0);
         }
+
+//        if (tele) {
+//
+//            telemetry.addData("Forward Snap", snappedForward);
+//            telemetry.addData("Strafe Snap", snappedStrafe);
+//
+//            telemetry.addData("Correction RX", correctionRX);
+//
+//            telemetry.addData("FL", frontLeftPower);
+//            telemetry.addData("BL", backLeftPower);
+//            telemetry.addData("FR", frontRightPower);
+//            telemetry.addData("BR", backRightPower);
+//
+//        }
     }
 }

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

@@ -1,18 +1,37 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDFController;
+import com.arcrobotics.ftclib.controller.wpilibcontroller.SimpleMotorFeedforward;
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 
+import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 
 import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 
 import java.util.LinkedList;
 
+@Config
 public class Flywheel {
     Robot robot;
 
-    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
+//    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
+    public static PIDFCoefficients shooterPIDF;
+    PIDFController pidf;
+    SimpleMotorFeedforward feedforward;
+
+    public static double kS = 0.01; // Static feedforward
+    public static double kV = 0.0001935; // Velocity feedforward
+
+    public static double shooterPIDF_P = 500;
+    public static double shooterPIDF_I = 1;
+    public static double shooterPIDF_D = 0.0;
+    public static double shooterPIDF_F = 93;
+//    public static double shooterPIDF_P = 0.0001;
+//    public static double shooterPIDF_I = 0;
+//    public static double shooterPIDF_D = 0.00001;
+//    public static double shooterPIDF_F = 0;
 
     private double velo = 0.0;
     private double velo1 = 0.0;
@@ -27,10 +46,10 @@ public class Flywheel {
     private final LinkedList<Double> velocityHistory = new LinkedList<>();
 
     public Flywheel(Robot rob) {
-
         robot = rob;
-        shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
+        shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
-        shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F);
+//        pidf = new PIDFController(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);
+//        feedforward = new SimpleMotorFeedforward(kS, kV);
     }
 
     public double getVelo() {
@@ -54,30 +73,31 @@ public class Flywheel {
     }
 
     // Set the robot PIDF for the next cycle.
-    private double prevF = 0;
-
-    public static double voltagePIDFDifference = 0.8;
 
     public void setPIDF(double p, double i, double d, double f) {
 
-        shooterPIDF1.p = p;
+        shooterPIDF.p = p;
-        shooterPIDF1.i = i;
+        shooterPIDF.i = i;
-        shooterPIDF1.d = d;
+        shooterPIDF.d = d;
-        shooterPIDF1.f = f;
+        shooterPIDF.f = f;
 
-        shooterPIDF2.p = p;
+//        pidf.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, 0);
-        shooterPIDF2.i = i;
-        shooterPIDF2.d = d;
-        shooterPIDF2.f = f;
 
-        if (Math.abs(prevF - f) > voltagePIDFDifference) {
+        robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+    }
 
-            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
+    private double prevF = 0;
 
-            robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
+    public static double voltagePIDFDifference = 1;
-
+    double averageVoltage = 0;
-            prevF = f;
+    public void setF(double voltage){
+        averageVoltage = ALPHA * voltage + (1 - ALPHA) * averageVoltage;
+        double f = shooterPIDF_F / voltage;
+        if (Math.abs(prevF - f) > voltagePIDFDifference && !steady) {
+            shooterPIDF.f = f;
+            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
         }
+        prevF = f;
     }
 
     // Convert from RPM to Ticks per Second
@@ -90,24 +110,30 @@ public class Flywheel {
         return (TPS * 60.0) / 28.0;
     }
 
+    double ALPHA = 0.3;
     private void updateVelocityAverage(double newVelocity) {
 
-        velocityHistory.add(newVelocity);
+//        velocityHistory.add(newVelocity);
+//
+//        int velocityHistorySize = 5;
+//        if (velocityHistory.size() > velocityHistorySize) {
+//            velocityHistory.removeFirst();
+//        }
+//
+//        double sum = 0.0;
+//
+//        for (double v : velocityHistory) {
+//            sum += v;
+//        }
+//
+//        averageVelocity = sum / velocityHistory.size();
 
-        int velocityHistorySize = 5;
+        averageVelocity = ALPHA * newVelocity + (1 - ALPHA) * averageVelocity;
-        if (velocityHistory.size() > velocityHistorySize) {
-            velocityHistory.removeFirst();
-        }
-
-        double sum = 0.0;
-
-        for (double v : velocityHistory) {
-            sum += v;
-        }
-
-        averageVelocity = sum / velocityHistory.size();
     }
 
+    double power;
+    double prevTargetTime = 0;
+    double prevTargetVelocity = 0;
     public void manageFlywheel(double commandedVelocity) {
 
         if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
@@ -115,17 +141,19 @@ public class Flywheel {
         }
 
         robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
-
+        power = robot.shooter1.getPower();
-        robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
+        robot.shooter2.setPower(power);
 
         velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
 
         velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
 
-        velo = (velo1 + velo2) / 2.0;
+        velo = velo1;
 
         updateVelocityAverage(velo);
 
         steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
     }
+
+    public double getShooterPower(){return power;}
 }

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

@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+
+public class ParkTilter {
+    Robot robot;
+    public ParkTilter (Robot rob) {
+        this.robot = rob;
+    }
+
+    public void park() {
+        robot.setTilt1Pos(ServoPositions.tilt1_down);
+        robot.setTilt2Pos(ServoPositions.tilt2_down);
+    }
+
+    public void unpark() {
+        robot.setTilt1Pos(ServoPositions.tilt1_up);
+        robot.setTilt2Pos(ServoPositions.tilt2_up);
+    }
+}

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import android.view.View;
+
 import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.hardware.AnalogInput;
@@ -19,6 +21,7 @@ public class Robot {
     // Singleton instance
     private static Robot instance;
 
+
     /**
      * Returns the existing Robot instance or creates one if it doesn't exist.
      */
@@ -45,11 +48,6 @@ public class Robot {
     public DcMotorEx backRight;
     public DcMotorEx intake;
     public DcMotorEx transfer;
-    public PIDFCoefficients shooterPIDF;
-    public static double shooterPIDF_P = 255;
-    public static double shooterPIDF_I = 0.0;
-    public static double shooterPIDF_D = 0.0;
-    public static double shooterPIDF_F = 75;
     //    public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;
@@ -65,7 +63,6 @@ public class Robot {
     public Servo spin2;
     public TouchSensor insideBeam;
     public TouchSensor outsideBeam;
-
     public RevColorSensorV3 revSensor;
 
     public VoltageSensor voltage;
@@ -106,13 +103,10 @@ public class Robot {
         shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
 
         shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
-        shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
+//        shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
         shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+//        shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
-        shooter1.setVelocity(0);
+        shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
-        shooter2.setVelocity(0);
 
         hood = hardwareMap.get(Servo.class, "hood");
 
@@ -175,12 +169,12 @@ public class Robot {
     // Voids below are used to minimize hardware calls to minimize loop times
 
     // Used to cut off digits that are negligible
-    private final int maxDigits = 5;
+    private final int maxDigits = 3;
     private final int roundingFactor = (int) Math.pow(10, maxDigits);
 
     private double prevFrontLeftPower = -10.501;
     public void setFrontLeftPower(double pow){
-        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevFrontLeftPower){
             frontLeft.setPower(pow);
         }
@@ -189,7 +183,7 @@ public class Robot {
 
     private double prevFrontRightPower = -10.501;
     public void setFrontRightPower(double pow){
-        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevFrontRightPower){
             frontRight.setPower(pow);
         }
@@ -198,7 +192,7 @@ public class Robot {
 
     private double prevBackLeftPower = -10.501;
     public void setBackLeftPower(double pow){
-        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevBackLeftPower){
             backLeft.setPower(pow);
         }
@@ -207,7 +201,7 @@ public class Robot {
 
     private double prevBackRightPower = -10.501;
     public void setBackRightPower(double pow){
-        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevBackRightPower){
             backRight.setPower(pow);
         }
@@ -216,7 +210,7 @@ public class Robot {
 
     private double prevIntakePower = -10.501;
     public void setIntakePower(double pow){
-        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevIntakePower){
             intake.setPower(pow);
         }
@@ -225,7 +219,7 @@ public class Robot {
 
     private double prevTransferPower = -10.501;
     public void setTransferPower(double pow){
-        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevTransferPower){
             transfer.setPower(pow);
         }
@@ -236,7 +230,7 @@ public class Robot {
 
     private double prevHoodPos = -10.501;
     public void setHoodPos(double pos){
-        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        pos = (double) Math.round((float) pos * roundingFactor) / roundingFactor;
         if (pos != prevHoodPos){
             hood.setPosition(pos);
         }
@@ -245,7 +239,7 @@ public class Robot {
 
     private double prevTransferServoPos = -10.501;
     public void setTransferServoPos(double pos){
-        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        pos = (double) Math.round((float) pos * roundingFactor) / roundingFactor;
         if (pos != prevTransferServoPos){
             transferServo.setPosition(pos);
         }
@@ -254,7 +248,7 @@ public class Robot {
 
     private double prevSpinPos = -10.501;
     public void setSpinPos(double pos){
-        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        pos = (double) Math.round((float) pos * roundingFactor) / roundingFactor;
         if (pos != prevSpinPos){
             spin1.setPosition(pos);
             spin2.setPosition(pos);
@@ -264,7 +258,7 @@ public class Robot {
 
     private double prevTurretPos = -10.501;
     public void setTurretPos(double pos){
-        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        pos = (double) Math.round((float) pos * roundingFactor) / roundingFactor;
         if (pos != prevTurretPos){
             turr1.setPosition(pos);
             turr2.setPosition(pos);
@@ -274,7 +268,7 @@ public class Robot {
 
     private double prevTilt1Pos = -10.501;
     public void setTilt1Pos(double pos){
-        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        pos = (double) Math.round((float) pos * roundingFactor) / roundingFactor;
         if (pos != prevTilt1Pos){
             tilt1.setPosition(pos);
         }
@@ -283,7 +277,7 @@ public class Robot {
 
     private double prevTilt2Pos = -10.501;
     public void setTilt2Pos(double pos){
-        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        pos = (double) Math.round((float) pos * roundingFactor) / roundingFactor;
         if (pos != prevTilt2Pos){
             tilt2.setPosition(pos);
         }
@@ -292,7 +286,7 @@ public class Robot {
 
     private double prevSpindexBlockerPos = -10.501;
     public void setSpindexBlockerPos(double pos){
-        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        pos = (double) Math.round((float) pos * roundingFactor) / roundingFactor;
         if (pos != prevSpindexBlockerPos){
             spindexBlocker.setPosition(pos);
         }
@@ -301,7 +295,7 @@ public class Robot {
 
     private double prevRapidFireBlockerPos = -10.501;
     public void setRapidFireBlockerPos(double pos){
-        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        pos = (double) Math.round((float) pos * roundingFactor) / roundingFactor;
         if (pos != prevRapidFireBlockerPos){
             rapidFireBlocker.setPosition(pos);
         }

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

@@ -1,9 +1,14 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import static org.firstinspires.ftc.teamcode.utilsv2.Flywheel.*;
+
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.pedropathing.follower.Follower;
 
+import org.firstinspires.ftc.teamcode.constants.Color;
 
+@Config
 public class Shooter {
 
     Robot robot;
@@ -15,31 +20,38 @@ public class Shooter {
     double goalY = 0.0;
     double obeliskX = 72;
     double obeliskY = 144;
+    double turretGoalX = 0;
+    double turretGoalY = 0;
 
     private boolean red = true;
+    public static boolean manualFlywheel = false;
 
 
     Follower follow;
 
-    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) {
+    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance, Turret turret, Flywheel flywheel, VelocityCommander com) {
         this.robot = rob;
-        this.fly = new Flywheel(rob);
+        this.fly = flywheel;
-        this.turr = new Turret(rob);
+        this.turr = turret;
         this.follow = follower;
-        this.commander = new VelocityCommander();
+        this.commander = com;
 
         setRedAlliance(redAlliance);
 
-        if (redAlliance) {
-            goalX = 144;
-        } else {
-            goalX = 0;
-        }
-        goalY = 144;
     }
 
     public void setRedAlliance(boolean input) {
         this.red = input;
+
+        if (this.red) {
+            goalX = 72;
+            turretGoalX = 68;
+        } else {
+            goalX = -72;
+            turretGoalX = -68;
+        }
+        goalY = 72;
+        turretGoalY = 68;
     }
 
     private double flywheelVelocity = 0.0;
@@ -61,6 +73,10 @@ public class Shooter {
         this.state = shooterState;
     }
 
+    public ShooterState getState(){
+        return state;
+    }
+
     public void setTurretPosition(double input) {
         this.turretPosition = input;
     }
@@ -73,20 +89,34 @@ public class Shooter {
         return turr.getObeliskID();
     }
 
-
+    private final double shooterDistFromCenter = 1.545;
-    public void update() {
+    public void update(double voltage) {
-
+        setRedAlliance(Color.redAlliance);
         switch (state) {
             case NOTHING:
                 break;
             case MANUAL:
+                manualFlywheel = true;
+                commander.getVeloPredictive(
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent(),
+                        voltage,
+                        fly.getAverageVelocity()
+                );
+
                 fly.manageFlywheel(flywheelVelocity);
+                fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage);
                 turr.manual(turretPosition);
                 break;
             case TRACK_GOAL:
+                manualFlywheel = false;
                 turr.trackGoal(
-                        (follow.getPose().getX() - goalX),
+                        (turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
-                        (follow.getPose().getY() - goalY),
+                        (turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -95,54 +125,71 @@ public class Shooter {
                         follow.getAcceleration().getYComponent()
                 );
 
-                flywheelVelocity = commander.getVeloPredictive(
+                commander.getVeloPredictive(
-                        (follow.getPose().getX() - goalX),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
-                        (follow.getPose().getY() - goalY),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
-                        follow.getAcceleration().getYComponent()
+                        follow.getAcceleration().getYComponent(),
+                        voltage,
+                        fly.getAverageVelocity()
                 );
 
+                flywheelVelocity = commander.getPredictedRPM();
+
+                robot.setHoodPos(commander.getHoodPredicted());
                 fly.manageFlywheel(flywheelVelocity);
+                fly.setF(voltage);
                 break;
             case READ_OBELISK:
-                turr.trackObelisk(
+                manualFlywheel = false;
-                        (follow.getPose().getX() - goalX),
+                robot.setTurretPos(Turret.neutralPosition);
-                        (follow.getPose().getY() - goalY),
-                        follow.getHeading()
-                );
 
-                flywheelVelocity = commander.getVeloPredictive(
+                turr.detectObelisk();
-                        (follow.getPose().getX() - goalX),
+
-                        (follow.getPose().getY() - goalY),
+                commander.getVeloPredictive(
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
-                        follow.getAcceleration().getYComponent()
+                        follow.getAcceleration().getYComponent(),
+                        voltage,
+                        fly.getAverageVelocity()
                 );
 
-                fly.manageFlywheel(flywheelVelocity);
+                flywheelVelocity = commander.getPredictedRPM();
+
+                fly.manageFlywheel(0);
+                fly.setF(voltage);
                 break;
 
             case MANUAL_TURRET_TRACK_FLY:
+                manualFlywheel = false;
                 turr.manual(turretPosition);
-                flywheelVelocity = commander.getVeloPredictive(
+                commander.getVeloPredictive(
-                        (follow.getPose().getX() - goalX),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
-                        (follow.getPose().getY() - goalY),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
-                        follow.getAcceleration().getYComponent()
+                        follow.getAcceleration().getYComponent(),
+                        voltage,
+                        fly.getAverageVelocity()
                 );
 
+                flywheelVelocity = commander.getPredictedRPM();
+                robot.setHoodPos(commander.getHoodPredicted());
+
                 fly.manageFlywheel(flywheelVelocity);
                 break;
 
             case MANUAL_FLYWHEEL_TRACK_TURR:
+                manualFlywheel = true;
                 turr.trackGoal(
-                        (follow.getPose().getX() - goalX),
+                        (turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
-                        (follow.getPose().getY() - goalY),
+                        (turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -150,11 +197,26 @@ public class Shooter {
                         follow.getVelocity().getYComponent(),
                         follow.getAcceleration().getYComponent()
                 );
+                commander.getVeloPredictive(
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent(),
+                        voltage,
+                        fly.getAverageVelocity()
+                );
                 fly.manageFlywheel(flywheelVelocity);
+                fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage);
+                fly.setF(voltage);
                 break;
 
         }
 
     }
 
+    public double getDistance(){return commander.getDistance();}
+
+
 }

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

@@ -1,24 +1,191 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import android.health.connect.datatypes.units.Velocity;
+
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.tests.NewShooterTest;
 
+@Config
 public class SpindexerTransferIntake {
 
     private final Robot robot;
 
-    public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) {
+    VelocityCommander commander;
+
+    private MultipleTelemetry TELE;
+
+    public SpindexerTransferIntake(Robot rob, MultipleTelemetry tele, VelocityCommander com) {
         this.robot = rob;
+        this.commander = com;
+        this.TELE = tele;
     }
 
-    private final double sensorDistanceThreshold = 4.0;
+    public enum DesiredPattern {
-    private final long pulseTime = 50; // ms
+        PPG,
+        PGP,
+        GPP
+    }
+
+    public enum SortedShootState {
+        IDLE,
+
+        MOVE_TO_1,
+        WAIT_FOR_1,
+        SHOOT_1,
+        RETRACT_1,
+
+        MOVE_TO_2,
+        WAIT_FOR_2,
+        SHOOT_2,
+        RETRACT_2,
+
+        MOVE_TO_3,
+        WAIT_FOR_3,
+        SHOOT_3,
+        RETRACT_3,
+
+        DONE
+    }
+
+    int[] shootOrder = {0, 1, 2};
+    final double sensorDistanceThreshold = 5.3;
+    final long pulseTime = 100; // ms
+
+    private DesiredPattern desiredPattern = DesiredPattern.GPP;
+
+    private SortedShootState shootState = SortedShootState.IDLE;
+    private long shootStateStartTime = System.currentTimeMillis();
+
+    private void setShootState(SortedShootState newState) {
+        shootState = newState;
+        shootStateStartTime = System.currentTimeMillis();
+    }
+
+    private long shootStateTime() {
+        return System.currentTimeMillis() - shootStateStartTime;
+    }
+
+    private int[] buildShootOrder(
+            BallStates[] loaded,
+            DesiredPattern desired) {
+
+        BallStates[] target;
+
+        switch (desired) {
+            case PPG:
+                target = new BallStates[]{
+                        BallStates.PURPLE,
+                        BallStates.PURPLE,
+                        BallStates.GREEN
+                };
+                break;
+
+            case PGP:
+                target = new BallStates[]{
+                        BallStates.PURPLE,
+                        BallStates.GREEN,
+                        BallStates.PURPLE
+                };
+                break;
+
+            default: // GPP
+                target = new BallStates[]{
+                        BallStates.GREEN,
+                        BallStates.PURPLE,
+                        BallStates.PURPLE
+                };
+        }
+
+        int[] order = new int[3];
+        boolean[] used = new boolean[3];
+
+        // first pass: exact color matches
+        for (int i = 0; i < 3; i++) {
+
+            order[i] = -1;
+
+            for (int slot = 0; slot < 3; slot++) {
+
+                if (!used[slot]
+                        && loaded[slot] == target[i]) {
+
+                    order[i] = slot;
+                    used[slot] = true;
+                    break;
+                }
+            }
+        }
+
+        // second pass: fill leftovers
+        for (int i = 0; i < 3; i++) {
+
+            if (order[i] != -1)
+                continue;
+
+            for (int slot = 0; slot < 3; slot++) {
+
+                if (!used[slot]) {
+                    order[i] = slot;
+                    used[slot] = true;
+                    break;
+                }
+            }
+        }
+
+        return order;
+    }
+
+    private void moveToSlot(int slot) {
+
+        switch (slot) {
+
+            case 0:
+                robot.setSpinPos(
+                        ServoPositions.spindexer_A1
+                );
+                break;
+
+            case 1:
+                robot.setSpinPos(
+                        ServoPositions.spindexer_A2
+                );
+                break;
+
+            case 2:
+                robot.setSpinPos(
+                        ServoPositions.spindexer_A3
+                );
+                break;
+        }
+    }
+
+    public enum SortedIntakeStates {
+        NOTHING,
+        IDLE,
+        INTAKE_1,
+        DELAY_1,
+        INTAKE_2,
+        DELAY_2,
+        INTAKE_3,
+        REVERSE,
+
+    }
 
     public enum SpindexerMode {
         RAPID,
-        SORTED
+        SORTED,
+        SHOOT_SORTED
+    }
+
+    public enum BallStates {
+        GREEN,
+        PURPLE,
+        UNKNOWN
     }
 
     public enum RapidMode {
@@ -34,12 +201,46 @@ public class SpindexerTransferIntake {
 
     private SpindexerMode mode = SpindexerMode.RAPID;
     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 spinMovementTime = 250;
 
     /**
      * Time when current state was entered.
      */
     private long stateStartTime = System.currentTimeMillis();
+    private long sortedStateStartTime = System.currentTimeMillis();
 
+    public void setDesiredPattern(DesiredPattern pattern) {
+        desiredPattern = pattern;
+    }
+
+    public void setDesiredPatternAuto(int id) {
+        if (id == 22){
+            desiredPattern = DesiredPattern.PGP;
+        } else if (id == 23){
+            desiredPattern = DesiredPattern.PPG;
+        } else {
+            desiredPattern = DesiredPattern.GPP;
+        }
+    }
+
+    public void startSortedShoot() {
+
+        shootOrder = buildShootOrder(
+                ballColors,
+                desiredPattern
+        );
+
+        setShootState(
+                SortedShootState.IDLE
+        );
+
+        setSpindexerMode(
+                SpindexerMode.SHOOT_SORTED
+        );
+    }
     public void setRapidMode(RapidMode newMode) {
         if (rapidMode != newMode) {
             rapidMode = newMode;
@@ -47,11 +248,18 @@ public class SpindexerTransferIntake {
         }
     }
 
+    public void setSortedIntakeMode(SortedIntakeStates newMode) {
+        if (sortedIntakeStates != newMode) {
+            sortedIntakeStates = newMode;
+            sortedStateStartTime = System.currentTimeMillis();
+        }
+    }
+
     public void setSpindexerMode(SpindexerMode spindexerMode) {
         this.mode = spindexerMode;
     }
 
-    public RapidMode getRapidState(){
+    public RapidMode getRapidState() {
         return this.rapidMode;
     }
 
@@ -59,8 +267,30 @@ public class SpindexerTransferIntake {
         return System.currentTimeMillis() - stateStartTime;
     }
 
+    private long sortedStateTime() {
+        return System.currentTimeMillis() - sortedStateStartTime;
+    }
+
+    private int greenTicks = 0;
+    private int ballTicks = 0;
+    private int holdBallsTicker = 0;
     public void update() {
 
+//        TELE.addData("Sorted State", sortedIntakeStates);
+//        TELE.addData("Ball0", ballColors[0]);
+//        TELE.addData("Ball1", ballColors[1]);
+//        TELE.addData("Ball2", ballColors[2]);
+//
+//        TELE.addData("Shoot0", shootOrder[0]);
+//        TELE.addData("Shoot1", shootOrder[1]);
+//        TELE.addData("Shoot2", shootOrder[2]);
+//
+//        TELE.addData("Color0", ballColors[0]);
+//        TELE.addData("Color1", ballColors[1]);
+//        TELE.addData("Color2", ballColors[2]);
+//
+//        TELE.addData("Shoot State", shootState);
+        
         switch (mode) {
 
             case RAPID:
@@ -74,32 +304,35 @@ public class SpindexerTransferIntake {
                     case INTAKE:
 
                         robot.setIntakePower(1);
-                        robot.setTransferPower(1);
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Closed
                         );
                         robot.setSpinPos(
                                 ServoPositions.spindexer_A2
                         );
+                        robot.setTransferPower(-0.7);
                         robot.setTransferServoPos(
                                 ServoPositions.transferServo_out
                         );
 
                         if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
 
-                            setRapidMode(RapidMode.TRANSFER_OFF);
+                            holdBallsTicker++;
                         }
 
+                        if (holdBallsTicker > 10){
+                            setRapidMode(RapidMode.TRANSFER_OFF);
+                            holdBallsTicker = 0;
+                        }
 
                         break;
 
                     case TRANSFER_OFF:
 
-                        robot.setTransferPower(0.3);
-
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
                         }
+                        robot.setTransferPower(-0.3);
 
                         break;
 
@@ -139,44 +372,374 @@ public class SpindexerTransferIntake {
                         if (robot.insideBeam.isPressed()
                                 && robot.outsideBeam.isPressed()) {
 
+                            robot.setTransferPower(0);
                             robot.setIntakePower(0.1);
+                            robot.setTransferServoPos(ServoPositions.transferServo_in);
 
                         } else {
-
+                            holdBallsTicker++;
                             robot.setIntakePower(1);
                         }
-
-
-
                         break;
 
                     case OPEN_GATE:
 
+                        if (stateTime() >= 100) {
+                            setRapidMode(RapidMode.SHOOT);
+                        }
+
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
+
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Open
                         );
 
-                        if (stateTime() >= 100) {
+                        if (Shooter.manualFlywheel) {
-                            setRapidMode(RapidMode.SHOOT);
+                            robot.setTransferPower(NewShooterTest.transferPower);
+                            robot.setIntakePower(-NewShooterTest.transferPower);
+                        } else {
+                            robot.setTransferPower(commander.getTransferPow());
+                            robot.setIntakePower(-commander.getTransferPow());
                         }
 
                         break;
 
                     case SHOOT:
 
-                        robot.setTransferServoPos(
+                        if (stateTime() >= 500) {
-                                ServoPositions.transferServo_in
-                        );
-                        if (stateTime() >= 400) {
                             setRapidMode(RapidMode.INTAKE);
                         }
+
+                        if (Shooter.manualFlywheel) {
+                            robot.setTransferPower(NewShooterTest.transferPower);
+                            robot.setIntakePower(-NewShooterTest.transferPower);
+                        } else {
+                            robot.setTransferPower(commander.getTransferPow());
+                            robot.setIntakePower(-commander.getTransferPow());
+                        }
+
+                        holdBallsTicker = 0;
+
                         break;
                 }
                 break;
 
             case SORTED:
 
-                // Future sorted-intake logic
+                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() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+                            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
+                            if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
+                                greenTicks++;
+                            }
+                            ballTicks++;
+                            if (ballTicks > 15) {
+                                if (greenTicks > 10){
+                                    ballColors[0] = BallStates.GREEN;
+                                } else {
+                                    ballColors[0] = BallStates.PURPLE;
+                                }
+                                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() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+                            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
+                            if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
+                                greenTicks++;
+                            }
+                            ballTicks++;
+                            if (ballTicks > 15) {
+                                if (greenTicks > 10){
+                                    ballColors[1] = BallStates.GREEN;
+                                } else {
+                                    ballColors[1] = BallStates.PURPLE;
+                                }
+                                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() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+                            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
+                            if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
+                                greenTicks++;
+                            }
+                            ballTicks++;
+                            if (ballTicks > 15) {
+                                if (greenTicks > 10){
+                                    ballColors[2] = BallStates.GREEN;
+                                } else {
+                                    ballColors[2] = BallStates.PURPLE;
+                                }
+                                setSortedIntakeMode(SortedIntakeStates.REVERSE);
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
+                        }
+                        break;
+                    case REVERSE:
+                        robot.setTransferPower(-0.3);
+                        robot.setIntakePower(-0.1);
+                        break;
+                }
+
+
+                break;
+            case SHOOT_SORTED:
+
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+                if (Shooter.manualFlywheel) {
+                    robot.setTransferPower(NewShooterTest.transferPower);
+                } else {
+                    robot.setTransferPower(commander.getTransferPow());
+                }
+
+
+                switch (shootState) {
+                    case IDLE:
+                        shootOrder = buildShootOrder(
+                                ballColors,
+                                desiredPattern
+                        );
+
+                        setShootState(SortedShootState.MOVE_TO_1);
+                        mode = SpindexerMode.SHOOT_SORTED;
+                        break;
+                    case MOVE_TO_1:
+
+                        moveToSlot(shootOrder[0]);
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Open
+                        );
+                        robot.setSpindexBlockerPos(
+                                ServoPositions.spindexBlocker_Closed
+                        );
+
+
+                        setShootState(
+                                SortedShootState.WAIT_FOR_1
+                        );
+
+                        break;
+                    case WAIT_FOR_1:
+
+                        if (shootStateTime() > 400) {
+
+                            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:
+
+                        moveToSlot(shootOrder[1]);
+
+                        setShootState(
+                                SortedShootState.WAIT_FOR_2
+                        );
+
+                        break;
+                    case WAIT_FOR_2:
+
+                        if (shootStateTime() > 400) {
+
+                            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:
+
+                        moveToSlot(shootOrder[2]);
+
+                        setShootState(
+                                SortedShootState.WAIT_FOR_3
+                        );
+
+                        break;
+                    case WAIT_FOR_3:
+
+                        if (shootStateTime() > 400) {
+
+                            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.SORTED;
+                        }
+
+                        break;
+
+                }
+
+
                 break;
         }
     }

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

@@ -1,11 +1,14 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
 import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDController;
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.robotcore.util.Range;
 
 
+import org.firstinspires.ftc.teamcode.constants.Color;
+
 import java.util.List;
 
 @Config
@@ -13,9 +16,22 @@ public class Turret {
     Robot robot;
 
     private final double servoTicksPer180 = 0.58;
-    private final double neutralPosition = 0.51;
+    public static double neutralPosition = 0.51;
     private final double turretMin = 0.05;
     private final double turretMax = 0.95;
+    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;
+    private double targetTx = 0;
+    private double currentTrackOffset = 0;
+    private double llCoast = 0;
+    private double servoAngle = 0.51;
+    double tx = 0.0;
     private final double hVelK = 0; // TODO: Tune
     private final double xVelK = 0; // TODO: Tune
     private final double xAccK = 0; // TODO: Tune
@@ -28,6 +44,7 @@ public class Turret {
 
     public Turret(Robot rob) {
         this.robot = rob;
+        bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D);
     }
 
     private double wrapAngle(double angle) {
@@ -36,6 +53,43 @@ public class Turret {
         return angle;
     }
 
+    private void limelightRead() { // only for tracking purposes, not general reads
+        switchPipeline(PipelineMode.TRACKING);
+        result = robot.limelight.getLatestResult();
+        tx = 1000;
+        if (result != null) {
+            if (result.isValid()) {
+                tx = result.getTx();
+            }
+        }
+    }
+
+    public double getTX(){return tx;}
+
+    public enum PipelineMode{
+        OBELISK,
+        TRACKING
+    }
+
+    private int prevPipeline = 0;
+    public void switchPipeline(PipelineMode pipelineMode){
+        int pipeline = 0;
+        if (pipelineMode == PipelineMode.OBELISK){
+            pipeline = 1;
+        } else if (pipelineMode == PipelineMode.TRACKING){
+            if (Color.redAlliance){
+                pipeline = 4;
+            } else {
+                pipeline = 2;
+            }
+        }
+        if (pipeline != prevPipeline){
+            robot.limelight.pipelineSwitch(pipeline);
+        }
+        prevPipeline = pipeline;
+    }
+    public int pipeline(){return prevPipeline;}
+
     public void trackObelisk(double dx, double dy, double h) {
 
         double heading = wrapAngle(h);
@@ -54,7 +108,6 @@ public class Turret {
 
         robot.setTurretPos(servoAngle);
 
-
         detectObelisk();
 
     }
@@ -63,32 +116,26 @@ public class Turret {
         return obeliskID;
     }
 
-    private int detectObelisk() {
+    public void detectObelisk() {
-        robot.limelight.pipelineSwitch(1);
+        result = robot.limelight.getLatestResult();
-        LLResult result = robot.limelight.getLatestResult();
         if (result != null && result.isValid()) {
             List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
-            double prevTx  = -1000;
             for (LLResultTypes.FiducialResult fiducial : fiducials) {
-                double currentTx = fiducial.getTargetXDegrees();
+                obeliskID = fiducial.getFiducialId();
-                if (currentTx > prevTx){
-                    obeliskID = fiducial.getFiducialId();
-                }
             }
         }
-        return obeliskID;
     }
 
     public void manual (double pos) {
         robot.setTurretPos(pos);
-
     }
 
-
     public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) {
         // dx, dy, dz is target - robot
         // h is the raw heading where 0 degrees is positive x in the system of x, y
 
+        bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D); // Keep when debugging/tuning, comment out when doing teleop
+
         double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
         double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
         double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading
@@ -98,11 +145,34 @@ public class Turret {
         double fieldRelativeHeading = Math.atan2(predictedDy, predictedDx);
 
         double angleDelta = fieldRelativeHeading - predictedH;
-        angleDelta = wrapAngle(angleDelta);
+        angleDelta = wrapAngle(angleDelta) / (2.0 * Math.PI);
 
-        double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
+        double bearingOffset = 0;
+        if (limelightUsed && servoAngle > turretMin && servoAngle < turretMax){
+            limelightRead();
+            if (result.isValid() && tx < 100){
+                targetTx = (tx*alphaTX)+(targetTx*(1-alphaTX));
+                bearingAligned = Math.abs(targetTx) < TARGET_POSITION_TOLERANCE;
+                if (!bearingAligned){
+                    bearingOffset = (bearingPID.calculate(targetTx, 0.0));
+                }
+            } else {
+                targetTx = 0;
+                bearingOffset = 0;
+            }
+            currentTrackOffset += bearingOffset;
+            llCoast = LL_COAST_TICKS;
+        } else {
+            if (llCoast <= 0){
+                currentTrackOffset = 0;
+            } else {
+                llCoast--;
+            }
+        }
 
-        double servoAngle = neutralPosition + servoTicksFromNeutral;
+        double servoTicksFromNeutral = (angleDelta+currentTrackOffset) * (2.0 * servoTicksPer180);
+
+        servoAngle = neutralPosition + servoTicksFromNeutral;
 
         servoAngle = Range.clip(servoAngle, turretMin, turretMax);
 

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

@@ -1,34 +1,112 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
 public class VelocityCommander {
-    private final double goalH = 20.0; //TODO: Tune
+    public static double xVelK = 0.05; // TODO: Tune
-    private final double xVelK = 0; // TODO: Tune
+    public static double xAccK = 0.025; // TODO: Tune
-    private final double xAccK = 0; // TODO: Tune
+    public static double yVelK = 0.05; // TODO: Tune
-    private final double yVelK = 0; // TODO: Tune
+    public static double yAccK = 0.025; // TODO: Tune
-    private final double yAccK = 0; // TODO: Tune
+    public static boolean lockFront = false;
+    public static boolean lockBack = false;
+    public static int farBound = 140;
+    public static int closeBound = 110;
+    public static double errorHoodAdjustment = 0.0005;
+    private double hoodPos = 0.88;
+    private double transferPow = -1;
+    private int velo = 0;
 
-    public VelocityCommander() {
+    public VelocityCommander() {}
-
-    }
 
+    final double veloA = -0.00000133612;
+    final double veloB = 0.000542733;
+    final double veloC = -0.0739531;
+    final double veloD = 5.16759;
+    final double veloE = 62.45781;
     private double distToRPM (double dist){
-        return Math.sqrt(dist*dist + goalH*goalH);
+        double currentVelo;
-        //TODO: Add regression here using goalH
+        if (lockFront && dist > closeBound){
+            dist = closeBound;
+        } else if (lockBack && dist < farBound){
+            dist = farBound;
+        }
+        if (dist < 54) {
+            velo = 2000;
+        } else if (dist > 181){
+            velo = 3600;
+        } else {
+            currentVelo = veloA*Math.pow(dist, 4) +
+                    veloB*Math.pow(dist, 3) +
+                    veloC*Math.pow(dist, 2) +
+                    veloD*Math.pow(dist, 1) +
+                    veloE;
+            velo = 10 * Math.round((float) Math.max(200, Math.min(360, currentVelo)));
+        }
+        return velo;
     }
 
+    final double hoodA = 9.04203*Math.pow(10, -8);
+    final double hoodB = -0.0000204165;
+    final double hoodC = -0.00252089;
+    final double hoodD = 1.06154;
+    private void distToHood (double dist){
+        if (dist > 174){
+            hoodPos = 0.48;
+        } else if (dist < 54){
+            hoodPos = 0.88;
+        } else {
+            hoodPos = hoodA*Math.pow(dist, 3) +
+                    hoodB*Math.pow(dist, 2) +
+                    hoodC*Math.pow(dist, 1) +
+                    hoodD;
+        }
+        hoodPos = Math.max(0.48, Math.min(0.88, hoodPos));
+    }
+    public double getHoodPredicted(){
+        return hoodPos;
+    }
+
+    private void distToTransferPow(double dist, double voltage){
+        if (dist < 140){
+            transferPow = -0.8;
+        } else {
+            transferPow = -0.5;
+        }
+    }
+    public double getTransferPow(){return transferPow;}
+
+    // 27
     public double getVeloStationary (double distance){
         return distToRPM(distance);
     }
 
-    public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) {
+    double predictedDist = 0;
+    public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage, double velocity) {
 
         double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
         double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
 
-        double predictedDist = Math.sqrt(dx*dx + dy*dy);
+        double goalHeight = 28;
+        predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight * goalHeight);
 
-        return distToRPM(predictedDist);
+        distToHood(predictedDist);
+        distToTransferPow(predictedDist, voltage);
+        distToRPM(predictedDist);
+
+        hoodPos += adjustHood(predictedDist, velocity, velo);
     }
 
+    public double adjustHood(double dist, double currentVelocity, double targetVelocity){
+        double error = targetVelocity - currentVelocity;
+        if (dist < farBound || error < 0){
+            error = 0;
+        }
+        System.out.println("Error "+ error);
+        return error * errorHoodAdjustment;
+    }
 
+    public double getPredictedRPM(){return velo;}
+
+    public double getDistance(){return predictedDist;}
 }