Merge remote-tracking branch 'origin/danielv5' into update-teleop

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

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

@@ -22,28 +22,28 @@ 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")
+@Autonomous (preselectTeleOp = "TeleopV4")
 public class Auto12BallPedroPathing extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
-    Flywheel flywheel;
-    Targeting targeting;
-    Targeting.Settings targetingSettings;
+//    Flywheel flywheel;
+//    Targeting targeting;
+//    Targeting.Settings targetingSettings;
     Follower follower;
-    Turret turret;
-    Spindexer spindexer;
-    Servos servos;
+//    Turret turret;
+//    Spindexer spindexer;
+//    Servos servos;
     MeasuringLoopTimes loopTimes;
 
     // Wait Times
@@ -222,10 +222,10 @@ public class Auto12BallPedroPathing extends LinearOpMode {
                 driveShoot(PathState.WAIT_SHOOT3, currentTime);
                 break;
             case WAIT_SHOOT3:
-                if (spindexer.shootAllComplete()){
-                    spindexer.resetSpindexer();
-                    TELE.addLine("Done Auto");
-                }
+//                if (spindexer.shootAllComplete()){
+//                    spindexer.resetSpindexer();
+//                    TELE.addLine("Done Auto");
+//                }
                 break;
             default:
                 break;
@@ -237,21 +237,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) {
-            spindexer.resetSpindexer();
+        if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() ||
+//            spindexer.resetSpindexer();
             pathState = nextState;
             follower.followPath(nextPath, true);
-            spindexer.setIntakePower(1);
+//            spindexer.setIntakePower(1);
         }
     }
     private void drivePickup(PathState nextState, PathChain nextPath) {
@@ -286,24 +286,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);
-        targeting = new Targeting();
-        targetingSettings = new Targeting.Settings(0,0);
+//        flywheel = new Flywheel(hardwareMap);
+//        targeting = new Targeting();
+//        targetingSettings = new Targeting.Settings(0,0);
         follower = Constants.createFollower(hardwareMap);
         follower.setStartingPose(new Pose(72,72,0));
-        turret = new Turret(robot, TELE, robot.limelight);
-        spindexer = new Spindexer(hardwareMap);
-        servos = new Servos(hardwareMap);
+//        turret = new Turret(robot, TELE, robot.limelight);
+//        spindexer = new Spindexer(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 +312,11 @@ public class Auto12BallPedroPathing extends LinearOpMode {
 
             if (gamepad1.crossWasPressed() && !initializeRobot){
                 Color.redAlliance = !Color.redAlliance;
-                if (Color.redAlliance){
-                    robot.light.setPosition(Color.LightRed);
-                } else {
-                    robot.light.setPosition(Color.LightBlue);
-                }
+//                if (Color.redAlliance){
+//                    robot.light.setPosition(Color.LightRed);
+//                } else {
+//                    robot.light.setPosition(Color.LightBlue);
+//                }
 
                 double[] xPoses = {startPoseX, shoot0X,
                         drivePickup1X, pickup1X, shoot1X,
@@ -338,8 +339,8 @@ public class Auto12BallPedroPathing extends LinearOpMode {
                 buildPaths();
                 sleep(2000);
 
-                turret.setTurret(turrDefault);
-                servos.setSpinPos(spinStartPos);
+//                turret.setTurret(turrDefault);
+//                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();
-            teleStartPoseY = currentPose.getY();
-            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
-
-            turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
-            targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
-
-            double voltage = robot.voltage.getVoltage();
-            flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
-            flywheel.manageFlywheel(targetingSettings.flywheelRPM);
-            servos.setHoodPos(targetingSettings.hoodAngle);
-
-            if (driveToShoot()){servos.setSpinPos(spinStartPos);}
-            else {spindexer.processIntake();}
+//            teleStartPoseX = currentPose.getX();
+//            teleStartPoseY = currentPose.getY();
+//            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
+//
+//            turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH)));
+//            targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate);
+//
+//            double voltage = robot.voltage.getVoltage();
+//            flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
+//            flywheel.manageFlywheel(targetingSettings.flywheelRPM);
+//            servos.setHoodPos(targetingSettings.hoodAngle);
+//
+//            if (driveToShoot()){servos.setSpinPos(spinStartPos);}
+//            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,366 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+
+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.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Turret;
+
+import java.util.List;
+
+@Config
+@Autonomous (preselectTeleOp = "TeleopV4")
+public class Auto12Ball_Back_Sorted extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+    Follower follower;
+    MeasuringLoopTimes loopTimes;
+
+    // Wait Times
+    public static double shootTime = 2;
+    public static double openGateTime = 1.5;
+
+    // Extra Variables
+    public static double intakePower = 0.3;
+    double shootX, shootY, shootH;
+
+    // Initialize path state machine
+    private enum PathState {
+        PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
+        PICKUP1, DRIVE_OPENGATE, 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 = 84, startPoseY = 7, startPoseH = 90;
+    public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
+    public static double shoot0ControlX = 88.29667812142038, shoot0ControlY = 52.03493699885454;
+    public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
+    public static double pickup1ControlX = 109.29381443298968, pickup1ControlY = 82.70618556701031;
+    public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
+    public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
+    public static double openGateX = 129, openGateY = 74, openGateH = 0;
+    public static double shoot1ControlX = 112, shoot1ControlY = 75;
+    public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12;
+    public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
+    public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
+    public static double shoot2ControlX = 102, shoot2ControlY = 63;
+    public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50;
+    public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
+    public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
+    public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
+    public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80;
+    Pose startPose, pushBot, shoot0Control, shoot0,
+            pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1,
+            drivePickup2, pickup2, shoot2Control, shoot2,
+            drivePickup3, pickup3, shoot3Control, shoot3;
+    private void initializePoses(){
+        startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
+        pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH));
+        shoot0Control = new Pose(shoot0ControlX, shoot0ControlY);
+        shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
+        pickup1Control = new Pose(pickup1ControlX, pickup1ControlY);
+        pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
+        openGateControl = new Pose(openGateControlX, openGateControlY);
+        openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH));
+        shoot1Control = new Pose(shoot1ControlX, shoot1ControlY);
+        shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H));
+        drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H));
+        pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H));
+        shoot2Control = new Pose(shoot2ControlX, shoot2ControlY);
+        shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H));
+        drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H));
+        pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H));
+        shoot3Control = new Pose(shoot3ControlX, shoot3ControlY);
+        shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H));
+    }
+
+    //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 BezierCurve(shoot0, pickup1Control, 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:
+                if (startAuto){
+                    follower.followPath(startPose_pushBot, true);
+                    startAuto = false;
+                    shootX = shoot0X;
+                    shootY = shoot0Y;
+                    shootH = shoot0H;
+                }
+                if (!follower.isBusy()){
+                    follower.followPath(pushBot_shoot0, true);
+                    pathState = PathState.DRIVE_SHOOT0;
+                }
+                break;
+            case DRIVE_SHOOT0:
+                if (!follower.isBusy()){
+                    timeStamp = currentTime;
+                    pathState = PathState.WAIT_SHOOT0;
+                }
+                break;
+            case WAIT_SHOOT0:
+                if (currentTime - timeStamp > shootTime){
+                    follower.followPath(shoot0_pickup1, intakePower, false);
+                    pathState = PathState.PICKUP1;
+                }
+                break;
+            case PICKUP1:
+                if (!follower.isBusy()){
+                    follower.followPath(pickup1_openGate, true);
+                    pathState = PathState.OPENGATE;
+                    shootX = shoot1X;
+                    shootY = shoot1Y;
+                    shootH = shoot1H;
+                }
+                break;
+            case DRIVE_OPENGATE:
+                if (!follower.isBusy()){
+                    pathState = PathState.OPENGATE;
+                    timeStamp = currentTime;
+                }
+                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;
+                }
+                break;
+            case WAIT_SHOOT1:
+                if (currentTime - timeStamp > shootTime){
+                    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;
+                }
+                break;
+            case WAIT_SHOOT2:
+                if (currentTime - timeStamp > shootTime){
+                    follower.followPath(shoot2_drivePickup3, true);
+                    pathState = PathState.DRIVE_PICKUP3;
+                }
+                break;
+            case DRIVE_PICKUP3:
+                if (!follower.isBusy()){
+                    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;
+                }
+                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 (144-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);
+        follower.setStartingPose(new Pose(72,72,0));
+        loopTimes = new MeasuringLoopTimes();
+        loopTimes.init();
+
+        boolean initializeRobot = false;
+        while (opModeInInit()){
+            follower.update();
+
+            if (gamepad1.crossWasPressed() && !initializeRobot){
+                Color.redAlliance = !Color.redAlliance;
+
+                double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
+                        pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
+                        drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
+                        drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
+
+                double[] headings = {startPoseH, pushBotH, shoot0H,
+                        pickup1H, openGateH, shoot1H,
+                        drivePickup2H, pickup2H, shoot2H,
+                        drivePickup3H, pickup3H, shoot3H};
+
+                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);
+            }
+
+            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.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        limelightUsed = false;
+
+        while (opModeIsActive()){
+            follower.update();
+            pathStateMachine();
+            Pose currentPose = follower.getPose();
+            teleStartPoseX = currentPose.getX();
+            teleStartPoseY = currentPose.getY();
+            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
+
+            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:", currentPose.getX());
+            TELE.addData("Y:", currentPose.getY());
+            TELE.addData("H:", currentPose.getHeading());
+            TELE.update();
+        }
+    }
+}

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

@@ -5,6 +5,19 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
+    public static double rapidFireBlocker_Closed = 0.35;
+    public static double rapidFireBlocker_Open = 0.5;
+
+    public static double spindexBlocker_Closed = 0.31;
+    public static double spindexBlocker_Open = 0.5;
+
+    public static double spindexer_A1 = 0.16;
+    public static double spindexer_A2 = 0.35;
+    public static double spindexer_A3 = 0.54;
+    public static double spindexer_B1 = 0.73;
+    public static double spindexer_B2 = 0.92;
+
+
     public static double spindexer_intakePos1 = 0.18; //0.13;
 
     public static double spindexer_intakePos2 = 0.37; //0.33;//0.5;
@@ -21,13 +34,13 @@ public class ServoPositions {
 
     public static double shootAllSpindexerSpeedIncrease = 0.01;
 
-    public static double transferServo_out = 0.15;
+    public static double transferServo_out = 0.57;
 
-    public static double transferServo_in = 0.38;
+    public static double transferServo_in = 0.77;
 
     public static double hoodAuto = 0.27;
 
-    public static double hoodOffset = -0.05; // offset from 0.93 (or position at 0,0 in targeting class)
+    public static double hoodOffset = 0; // offset from 0.93 (or position at 0,0 in targeting class)
 
     public static double turret_redClose = 0;
     public static double turret_blueClose = 0;
@@ -44,4 +57,10 @@ public class ServoPositions {
     public static double redTurretShootPos = 0.05;
     public static double blueTurretShootPos = -0.05;
 
+    public static double tilt1_down = 0.6;
+    public static double tilt2_down = 0.4;
+    public static double tilt1_up = 0.08;
+    public static double tilt2_up = 0.97;
+
+
 }

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

@@ -17,12 +17,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 @Config
 public class Constants {
     public static FollowerConstants followerConstants = new FollowerConstants()
-            .mass(15.5)
-            .forwardZeroPowerAcceleration(-29.512)
-            .lateralZeroPowerAcceleration(-72.872)
-            .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012))
-            .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.025, 0.02))
-            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03))
+            .mass(14.37888)
+            .forwardZeroPowerAcceleration(-30.322)
+            .lateralZeroPowerAcceleration(-60.876)
+            .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02))
+            .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02))
+            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.00001, 0.6, 0.015))
             .centripetalScaling(0.0005);
 
     public static MecanumConstants driveConstants = new MecanumConstants()
@@ -35,15 +35,15 @@ public class Constants {
             .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
             .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
             .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
-            .xVelocity(64.675)
-            .yVelocity(49.583);
+            .xVelocity(84.376)
+            .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(-7.5)
-            .strafePodX(-3.75)
+            .forwardPodY(3.7795)
+            .strafePodX(-3.676)
             .distanceUnit(DistanceUnit.INCH)
             .hardwareMapName("pinpoint")
             .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)

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

@@ -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 {

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

@@ -1,32 +1,56 @@
 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;
+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.utilsv2.Drivetrain;
-import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
 
 @TeleOp
 @Config
 public class TeleopV4 extends LinearOpMode {
     Robot robot;
     Drivetrain drivetrain;
+    Shooter shooter;
     MultipleTelemetry TELE;
+    Follower follower;
+    SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
-        robot = new Robot(hardwareMap);
+        robot = Robot.getInstance(hardwareMap);
 
         TELE = new MultipleTelemetry(
                 FtcDashboard.getInstance().getTelemetry(), telemetry
         );
 
         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);
+
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel);
+        shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
-        drivetrain.setTelemetry(true);
 
         waitForStart();
 
@@ -42,6 +66,40 @@ public class TeleopV4 extends LinearOpMode {
                     gamepad1.left_stick_x
             );
 
+            shooter.update();
+            spindexerTransferIntake.update();
+
+            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
+
+            if (gamepad1.xWasPressed() &&
+                    (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.aWasPressed() &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
+                );
+            }
+
+            if (gamepad1.yWasPressed()
+                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.INTAKE
+                );
+            }
+
+
             TELE.update();
         }
 

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

@@ -1,13 +1,15 @@
 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;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+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
@@ -49,8 +51,14 @@ public class Hardware_Tester extends LinearOpMode {
 
     @Override
     public void runOpMode() throws InterruptedException {
-        robot = new Robot(hardwareMap);
-        TELE = new MultipleTelemetry();
+        Robot.resetInstance();
+        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);
+
+        waitForStart();
 
         if (isStopRequested()) return;
         while (opModeIsActive()){
@@ -116,9 +124,10 @@ public class Hardware_Tester extends LinearOpMode {
 
             // Sensor Data
 
-            TELE.addData("Beam Break 1?", robot.beam1.isPressed());
-            TELE.addData("Beam Break 2?", robot.beam2.isPressed());
-            TELE.addData("Beam Break 3?", robot.beam3.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));

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

@@ -0,0 +1,147 @@
+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;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
+import org.firstinspires.ftc.teamcode.utilsv2.Turret;
+
+@Config
+@TeleOp
+public class NewShooterTest extends LinearOpMode {
+
+    Robot robot;
+    Flywheel flywheel;
+    Turret turret;
+    MultipleTelemetry TELE;
+
+
+    public static boolean intake = true;
+    public static boolean shoot = false;
+    public static double intakePower = 1.0;
+    public static double transferShootPower = -1;
+    public static double transferIntakePower = -1;
+    public static double turretPos = 0.51;
+    public static double hoodPos = 0.51;
+    public static double flywheel_velo = 0;
+
+    public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75;
+
+    private enum ShootState {
+        IDLE,
+        WAIT_GATE,
+        WAIT_PUSH
+    }
+
+    private ShootState shootState = ShootState.IDLE;
+    private long timestamp = 0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        Robot.resetInstance();
+
+        robot = Robot.getInstance(hardwareMap);
+
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        Shooter shooter = new Shooter(
+                robot,
+                TELE,
+                Constants.createFollower(hardwareMap),
+                true,
+                turret,
+                flywheel
+        );
+
+        shooter.setState(Shooter.ShooterState.MANUAL);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            robot.setHoodPos(hoodPos);
+            shooter.setTurretPosition(turretPos);
+            shooter.setFlywheelVelocity(flywheel_velo);
+            double voltage = robot.voltage.getVoltage();
+            flywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
+
+            robot.setSpinPos(ServoPositions.spindexer_A2);
+
+            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;
+                }
+            }
+
+            TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28);
+            TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28);
+            TELE.addData("Flywheel Averag Velocity", flywheel.getAverageVelocity());
+            TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF);
+            TELE.addData("Power", flywheel.getShooterPower());
+            TELE.update();
+
+            shooter.update();
+        }
+    }
+}

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

@@ -83,6 +83,7 @@ public class Robot {
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
 
         intake = hardwareMap.get(DcMotorEx.class, "intake");
+        intake.setDirection(DcMotorSimple.Direction.REVERSE);
 
         shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
 
@@ -103,11 +104,12 @@ public class Robot {
 
         turr2 = hardwareMap.get(Servo.class, "turr2");
 
-        spin1 = hardwareMap.get(Servo.class, "spin2");
+        spin1 = hardwareMap.get(Servo.class, "spin1");
 
-        spin2 = hardwareMap.get(Servo.class, "spin1");
+        spin2 = hardwareMap.get(Servo.class, "spin2");
 
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
+        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
@@ -121,27 +123,27 @@ public class Robot {
         tilt1 = hardwareMap.get(Servo.class, "tilt1");
         tilt2 = hardwareMap.get(Servo.class, "tilt2");
 
-        beam1 = hardwareMap.get(TouchSensor.class, "beam1");
-        beam2 = hardwareMap.get(TouchSensor.class, "beam2");
-        beam3 = hardwareMap.get(TouchSensor.class, "beam3");
+//        beam1 = hardwareMap.get(TouchSensor.class, "beam1");
+//        beam2 = hardwareMap.get(TouchSensor.class, "beam2");
+//        beam3 = hardwareMap.get(TouchSensor.class, "beam3");
 
         revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");
 
         // Below is disregarded
 
-        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
-
-        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
-
-        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
-
-        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
-
-        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
-
-        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
-
-        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
+//        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
+//
+//        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
+//
+//        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+//
+//        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
+//
+//        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
+//
+//        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
+//
+//        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
 
         if (usingLimelight) {
             limelight = hardwareMap.get(Limelight3A.class, "limelight");
@@ -150,7 +152,8 @@ public class Robot {
             aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
         }
 
-        light = hardwareMap.get(Servo.class, "light");
+//        light = hardwareMap.get(Servo.class, "light");
+
         voltage = hardwareMap.voltageSensor.iterator().next();
     }
 

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

@@ -25,10 +25,10 @@ public class Turret {
 
     public static double turretTolerance = 0.02;
     public static double turrPosScalar = 0.00011264432;
-    public static double turret180Range = 0.55;
-    public static double turrDefault = 0.35;
-    public static double turrMin = 0;
-    public static double turrMax = 0.69;
+    public static double turret180Range = 0.58;
+    public static double turrDefault = 0.51;
+    public static double turrMin = 0.05;
+    public static double turrMax = 0.95;
     public static boolean limelightUsed = true;
     public static double limelightPosOffset = 5;
     public static double manualOffset = 0.0;

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

@@ -4,8 +4,6 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
 @Config
 public class Drivetrain {
 
@@ -18,8 +16,8 @@ public class Drivetrain {
 
     private static final double STRAFE_MULTIPLIER = 1.2;
 
-    public static double FORWARD_ROTATION_CORRECTION = 0.03;
-    public static double STRAFE_ROTATION_CORRECTION = -0.03;
+    public static double FORWARD_ROTATION_CORRECTION = 0;
+    public static double STRAFE_ROTATION_CORRECTION = -0;
 
     private boolean tele = false;
 

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

@@ -1,18 +1,23 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
 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.utils.Robot;
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
 
 import java.util.LinkedList;
 
 public class Flywheel {
     Robot robot;
 
-    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
+//    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
+    public static 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;
 
     private double velo = 0.0;
     private double velo1 = 0.0;
@@ -27,10 +32,8 @@ 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);
-        shooterPIDF2 = 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);
     }
 
     public double getVelo() {
@@ -54,28 +57,24 @@ 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;
-        shooterPIDF1.i = i;
-        shooterPIDF1.d = d;
-        shooterPIDF1.f = f;
+        shooterPIDF.p = p;
+        shooterPIDF.i = i;
+        shooterPIDF.d = d;
+        shooterPIDF.f = f;
 
-        shooterPIDF2.p = p;
-        shooterPIDF2.i = i;
-        shooterPIDF2.d = d;
-        shooterPIDF2.f = f;
+        robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+    }
 
+    private double prevF = 0;
+
+    public static double voltagePIDFDifference = 0.8;
+    public void setF(double f){
         if (Math.abs(prevF - f) > voltagePIDFDifference) {
-
-            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
-
-            robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
-
+            shooterPIDF.f = f;
+            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
             prevF = f;
         }
     }
@@ -108,6 +107,7 @@ public class Flywheel {
         averageVelocity = sum / velocityHistory.size();
     }
 
+    double power;
     public void manageFlywheel(double commandedVelocity) {
 
         if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
@@ -115,8 +115,8 @@ public class Flywheel {
         }
 
         robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
-
-        robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
+        power = robot.shooter1.getPower();
+        robot.shooter2.setPower(power);
 
         velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
 
@@ -128,4 +128,5 @@ public class Flywheel {
 
         steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
     }
+    public double getShooterPower(){return power;}
 }

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

@@ -0,0 +1,305 @@
+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;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.TouchSensor;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+public class Robot {
+    // Singleton instance
+    private static Robot instance;
+
+
+    /**
+     * Returns the existing Robot instance or creates one if it doesn't exist.
+     */
+    public static Robot getInstance(HardwareMap hardwareMap) {
+        if (instance == null) {
+            instance = new Robot(hardwareMap);
+        }
+        return instance;
+    }
+
+    /**
+     * Optional: clears the singleton.
+     * Useful when switching OpModes.
+     */
+    public static void resetInstance() {
+        instance = null;
+    }
+
+    public static boolean usingLimelight = true;
+    public static boolean usingCamera = false;
+    public DcMotorEx frontLeft;
+    public DcMotorEx frontRight;
+    public DcMotorEx backLeft;
+    public DcMotorEx backRight;
+    public DcMotorEx intake;
+    public DcMotorEx transfer;
+    //    public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
+    public DcMotorEx shooter1;
+    public DcMotorEx shooter2;
+    public Servo hood;
+    public Servo transferServo;
+    public Servo spindexBlocker;
+    public Servo rapidFireBlocker;
+    public Servo tilt1;
+    public Servo tilt2;
+    public Servo turr1;
+    public Servo turr2;
+    public Servo spin1;
+    public Servo spin2;
+    public TouchSensor insideBeam;
+    public TouchSensor outsideBeam;
+    public RevColorSensorV3 revSensor;
+
+    public VoltageSensor voltage;
+
+    // Below is disregarded
+    public AnalogInput spin1Pos;
+    public AnalogInput spin2Pos;
+    public AnalogInput turr1Pos;
+    public AnalogInput transferServoPos;
+    public AprilTagProcessor aprilTagProcessor;
+    public WebcamName webcam;
+    public RevColorSensorV3 color1;
+    public RevColorSensorV3 color2;
+    public RevColorSensorV3 color3;
+    public Limelight3A limelight;
+    public Servo light;
+
+    public Robot(HardwareMap hardwareMap) {
+
+        //Define components w/ hardware map
+        frontLeft = hardwareMap.get(DcMotorEx.class, "fl");
+        frontRight = hardwareMap.get(DcMotorEx.class, "fr");
+        backLeft = hardwareMap.get(DcMotorEx.class, "bl");
+        backRight = hardwareMap.get(DcMotorEx.class, "br");
+        frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+        intake = hardwareMap.get(DcMotorEx.class, "intake");
+        intake.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1");
+
+        shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
+
+        shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
+//        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);
+        shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        hood = hardwareMap.get(Servo.class, "hood");
+
+        turr1 = hardwareMap.get(Servo.class, "turr1");
+
+        turr2 = hardwareMap.get(Servo.class, "turr2");
+
+        spin1 = hardwareMap.get(Servo.class, "spin1");
+
+        spin2 = hardwareMap.get(Servo.class, "spin2");
+
+        transfer = hardwareMap.get(DcMotorEx.class, "transfer");
+
+
+        transferServo = hardwareMap.get(Servo.class, "transferServo");
+
+        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
+        transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        spindexBlocker = hardwareMap.get(Servo.class, "spinB");
+
+        rapidFireBlocker = hardwareMap.get(Servo.class, "rapidB");
+
+        tilt1 = hardwareMap.get(Servo.class, "tilt1");
+        tilt2 = hardwareMap.get(Servo.class, "tilt2");
+
+        insideBeam = hardwareMap.get(TouchSensor.class, "beam1");
+        outsideBeam = hardwareMap.get(TouchSensor.class, "beam2");
+
+        revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");
+
+        // Below is disregarded
+
+//        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
+//
+//        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
+//
+//        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+//
+//        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
+//
+//        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
+//
+//        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
+//
+//        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
+
+        if (usingLimelight) {
+            limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        } else if (usingCamera) {
+            webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
+            aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
+        }
+
+//        light = hardwareMap.get(Servo.class, "light");
+
+        voltage = hardwareMap.voltageSensor.iterator().next();
+    }
+
+    // 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 roundingFactor = (int) Math.pow(10, maxDigits);
+
+    private double prevFrontLeftPower = -10.501;
+    public void setFrontLeftPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevFrontLeftPower){
+            frontLeft.setPower(pow);
+        }
+        prevFrontLeftPower = pow;
+    }
+
+    private double prevFrontRightPower = -10.501;
+    public void setFrontRightPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevFrontRightPower){
+            frontRight.setPower(pow);
+        }
+        prevFrontRightPower = pow;
+    }
+
+    private double prevBackLeftPower = -10.501;
+    public void setBackLeftPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevBackLeftPower){
+            backLeft.setPower(pow);
+        }
+        prevBackLeftPower = pow;
+    }
+
+    private double prevBackRightPower = -10.501;
+    public void setBackRightPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevBackRightPower){
+            backRight.setPower(pow);
+        }
+        prevBackRightPower = pow;
+    }
+
+    private double prevIntakePower = -10.501;
+    public void setIntakePower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevIntakePower){
+            intake.setPower(pow);
+        }
+        prevIntakePower = pow;
+    }
+
+    private double prevTransferPower = -10.501;
+    public void setTransferPower(double pow){
+        pow = (double) Math.round(pow * roundingFactor) / roundingFactor;
+        if (pow != prevTransferPower){
+            transfer.setPower(pow);
+        }
+        prevTransferPower = pow;
+    }
+
+    // shooter motors are done in separate class
+
+    private double prevHoodPos = -10.501;
+    public void setHoodPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevHoodPos){
+            hood.setPosition(pos);
+        }
+        prevHoodPos = pos;
+    }
+
+    private double prevTransferServoPos = -10.501;
+    public void setTransferServoPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTransferServoPos){
+            transferServo.setPosition(pos);
+        }
+        prevTransferServoPos = pos;
+    }
+
+    private double prevSpinPos = -10.501;
+    public void setSpinPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevSpinPos){
+            spin1.setPosition(pos);
+            spin2.setPosition(pos);
+        }
+        prevSpinPos = pos;
+    }
+
+    private double prevTurretPos = -10.501;
+    public void setTurretPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTurretPos){
+            turr1.setPosition(pos);
+            turr2.setPosition(pos);
+        }
+        prevTurretPos = pos;
+    }
+
+    private double prevTilt1Pos = -10.501;
+    public void setTilt1Pos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTilt1Pos){
+            tilt1.setPosition(pos);
+        }
+        prevTilt1Pos = pos;
+    }
+
+    private double prevTilt2Pos = -10.501;
+    public void setTilt2Pos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevTilt2Pos){
+            tilt2.setPosition(pos);
+        }
+        prevTilt2Pos = pos;
+    }
+
+    private double prevSpindexBlockerPos = -10.501;
+    public void setSpindexBlockerPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevSpindexBlockerPos){
+            spindexBlocker.setPosition(pos);
+        }
+        prevSpindexBlockerPos = pos;
+    }
+
+    private double prevRapidFireBlockerPos = -10.501;
+    public void setRapidFireBlockerPos(double pos){
+        pos = (double) Math.round(pos * roundingFactor) / roundingFactor;
+        if (pos != prevRapidFireBlockerPos){
+            rapidFireBlocker.setPosition(pos);
+        }
+        prevRapidFireBlockerPos = pos;
+    }
+
+}

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

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.utilsv2;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.pedropathing.follower.Follower;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
 
 public class Shooter {
 
@@ -22,32 +21,32 @@ public class Shooter {
 
     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) {
         this.robot = rob;
-        this.fly = new Flywheel(rob);
-        this.turr = new Turret(rob);
+        this.fly = flywheel;
+        this.turr = turret;
         this.follow = follower;
         this.commander = new VelocityCommander();
 
         setRedAlliance(redAlliance);
 
-        if (redAlliance) {
-            goalX = 144;
-            goalY = 144;
-        } else {
-            goalX = 0;
-            goalY = 144;
-        }
     }
 
     public void setRedAlliance(boolean input) {
         this.red = input;
+
+        if (this.red) {
+            goalX = 144;
+        } else {
+            goalX = 0;
+        }
+        goalY = 144;
     }
 
     private double flywheelVelocity = 0.0;
     private double turretPosition = 0.5;
 
-    enum ShooterState {
+    public enum ShooterState {
         READ_OBELISK,
         TRACK_GOAL,
         MANUAL_FLYWHEEL_TRACK_TURR,

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

@@ -0,0 +1,183 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+
+public class SpindexerTransferIntake {
+
+    private final Robot robot;
+
+    public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) {
+        this.robot = rob;
+    }
+
+    private final double sensorDistanceThreshold = 4.0;
+    private final long pulseTime = 50; // ms
+
+    public enum SpindexerMode {
+        RAPID,
+        SORTED
+    }
+
+    public enum RapidMode {
+        INTAKE,
+        TRANSFER_OFF,
+        BEFORE_PULSE_OUT,
+        PULSE_OUT,
+        PULSE_IN,
+        HOLD_BALLS,
+        OPEN_GATE,
+        SHOOT
+    }
+
+    private SpindexerMode mode = SpindexerMode.RAPID;
+    private RapidMode rapidMode = RapidMode.INTAKE;
+
+    /**
+     * Time when current state was entered.
+     */
+    private long stateStartTime = System.currentTimeMillis();
+
+    public void setRapidMode(RapidMode newMode) {
+        if (rapidMode != newMode) {
+            rapidMode = newMode;
+            stateStartTime = System.currentTimeMillis();
+        }
+    }
+
+    public void setSpindexerMode(SpindexerMode spindexerMode) {
+        this.mode = spindexerMode;
+    }
+
+    public RapidMode getRapidState(){
+        return this.rapidMode;
+    }
+
+    private long stateTime() {
+        return System.currentTimeMillis() - stateStartTime;
+    }
+
+    public void update() {
+
+        switch (mode) {
+
+            case RAPID:
+
+                robot.setSpindexBlockerPos(
+                        ServoPositions.spindexBlocker_Open
+                );
+
+                switch (rapidMode) {
+
+                    case INTAKE:
+
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(1);
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Closed
+                        );
+                        robot.setSpinPos(
+                                ServoPositions.spindexer_A2
+                        );
+                        robot.setTransferServoPos(
+                                ServoPositions.transferServo_out
+                        );
+
+                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+
+                            setRapidMode(RapidMode.TRANSFER_OFF);
+                        }
+
+
+                        break;
+
+                    case TRANSFER_OFF:
+
+                        robot.setTransferPower(0.3);
+
+                        if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
+                            setRapidMode(RapidMode.BEFORE_PULSE_OUT);
+                        }
+
+                        break;
+
+                    case BEFORE_PULSE_OUT:
+
+                        robot.setIntakePower(1.0);
+
+                        if (stateTime() >= 300) {
+                            setRapidMode(RapidMode.PULSE_OUT);
+                        }
+
+                        break;
+
+                    case PULSE_OUT:
+
+                        robot.setIntakePower(-0.1);
+
+                        if (stateTime() >= pulseTime) {
+                            setRapidMode(RapidMode.PULSE_IN);
+                        }
+
+                        break;
+
+                    case PULSE_IN:
+
+                        robot.setIntakePower(1.0);
+
+                        if (stateTime() >= 200) {
+                            setRapidMode(RapidMode.HOLD_BALLS);
+                        }
+
+
+                        break;
+
+                    case HOLD_BALLS:
+
+                        if (robot.insideBeam.isPressed()
+                                && robot.outsideBeam.isPressed()) {
+
+                            robot.setIntakePower(0.1);
+
+                        } else {
+
+                            robot.setIntakePower(1);
+                        }
+
+
+
+                        break;
+
+                    case OPEN_GATE:
+
+                        robot.setRapidFireBlockerPos(
+                                ServoPositions.rapidFireBlocker_Open
+                        );
+
+                        if (stateTime() >= 100) {
+                            setRapidMode(RapidMode.SHOOT);
+                        }
+
+                        break;
+
+                    case SHOOT:
+
+                        robot.setTransferServoPos(
+                                ServoPositions.transferServo_in
+                        );
+                        if (stateTime() >= 400) {
+                            setRapidMode(RapidMode.INTAKE);
+                        }
+                        break;
+                }
+                break;
+
+            case SORTED:
+
+                // Future sorted-intake logic
+                break;
+        }
+    }
+}

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

@@ -5,7 +5,6 @@ import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.robotcore.util.Range;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
 
 import java.util.List;
 
@@ -13,10 +12,10 @@ import java.util.List;
 public class Turret {
     Robot robot;
 
-    private final double servoTicksPer180 = 0.6; // TODO: Tune
-    private final double neutralPosition = 0.5; //TODO: Tune
-    private final double turretMin = 0.04; //TODO: Tune
-    private final double turretMax = 0.94; //TODO: Tune
+    private final double servoTicksPer180 = 0.58;
+    private final double neutralPosition = 0.51;
+    private final double turretMin = 0.05;
+    private final double turretMax = 0.95;
     private final double hVelK = 0; // TODO: Tune
     private final double xVelK = 0; // TODO: Tune
     private final double xAccK = 0; // TODO: Tune
@@ -53,8 +52,8 @@ public class Turret {
 
         servoAngle = Range.clip(servoAngle, turretMin, turretMax);
 
-        robot.turr1.setPosition(servoAngle);
-        robot.turr2.setPosition(1.0 - servoAngle);
+        robot.setTurretPos(servoAngle);
+
 
         detectObelisk();
 
@@ -81,8 +80,8 @@ public class Turret {
     }
 
     public void manual (double pos) {
-        robot.turr1.setPosition(pos);
-        robot.turr2.setPosition(pos);
+        robot.setTurretPos(pos);
+
     }
 
 
@@ -107,7 +106,6 @@ public class Turret {
 
         servoAngle = Range.clip(servoAngle, turretMin, turretMax);
 
-        robot.turr1.setPosition(servoAngle);
-        robot.turr2.setPosition(servoAngle);
+        robot.setTurretPos(servoAngle);
     }
 }