readjusted shooter test

@KeshavAnandCode please merge
Merge branch 'update-teleop' into danielv5
2026-06-02 19:14:58 -05:00 · 2026-06-02 18:28:00 -05:00 · 2026-06-02 18:22:41 -05:00 · 2026-06-02 18:22:28 -05:00 · 2026-06-02 18:12:32 -05:00 · 2026-06-02 17:18:04 -05:00
18 changed files with 1874 additions and 100 deletions
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java
+++ b/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();
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
@@ -0,0 +1,341 @@
+//package org.firstinspires.ftc.teamcode.autonomous;
+//
+//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
+//import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
+//import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
+//import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
+//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.utils.Servos;
+//import org.firstinspires.ftc.teamcode.utils.Spindexer;
+//import org.firstinspires.ftc.teamcode.utils.Targeting;
+//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;
+//    //    Flywheel flywheel;
+////    Targeting targeting;
+////    Targeting.Settings targetingSettings;
+//    Follower follower;
+//    //    Turret turret;
+////    Spindexer spindexer;
+////    Servos servos;
+//    MeasuringLoopTimes loopTimes;
+//
+//    // Wait Times
+//    public static double shootTime = 2;
+//
+//    // 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, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
+//        DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
+//        DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
+//    }
+//    PathState pathState = PathState.DRIVE_SHOOT0;
+//
+//    // Poses
+//    public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
+//    public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
+//    public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
+//    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, shoot0,
+//            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));
+//        shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
+//        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();
+//    }
+//
+//    //Path State Machine
+//    private boolean startAuto = true;
+//    private double timeStamp = 0;
+//    private void pathStateMachine(){
+//        double currentTime = (double) System.currentTimeMillis() / 1000;
+//        switch(pathState){
+//            case DRIVE_SHOOT0:
+//                if (startAuto){
+//                    follower.followPath(startPose_shoot0, true);
+//                    startAuto = false;
+//                    shootX = shoot0X;
+//                    shootY = shoot0Y;
+//                    shootH = shoot0H;
+//                }
+//                driveShoot(PathState.WAIT_SHOOT0, currentTime);
+//                break;
+//            case WAIT_SHOOT0:
+//                waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime);
+//                break;
+//            case DRIVE_PICKUP1:
+//                drivePickup(PathState.PICKUP1, drivePickup1_pickup1);
+//                break;
+//            case PICKUP1:
+//                pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1);
+//                shootX = shoot1X;
+//                shootY = shoot1Y;
+//                shootH = shoot1H;
+//                break;
+//            case DRIVE_SHOOT1:
+//                intakePowerDown(timeStamp, currentTime);
+//                driveShoot(PathState.WAIT_SHOOT1, currentTime);
+//                break;
+//            case WAIT_SHOOT1:
+//                waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime);
+//                break;
+//            case DRIVE_PICKUP2:
+//                drivePickup(PathState.PICKUP2, drivePickup2_pickup2);
+//                break;
+//            case PICKUP2:
+//                pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2);
+//                shootX = shoot2X;
+//                shootY = shoot2Y;
+//                shootH = shoot2H;
+//                break;
+//            case DRIVE_SHOOT2:
+//                intakePowerDown(timeStamp, currentTime);
+//                driveShoot(PathState.WAIT_SHOOT2, currentTime);
+//                break;
+//            case WAIT_SHOOT2:
+//                waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime);
+//                break;
+//            case DRIVE_PICKUP3:
+//                drivePickup(PathState.PICKUP3, drivePickup3_pickup3);
+//                break;
+//            case PICKUP3:
+//                pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3);
+//                shootX = shoot3X;
+//                shootY = shoot3Y;
+//                shootH = shoot3H;
+//                break;
+//            case DRIVE_SHOOT3:
+//                intakePowerDown(timeStamp, currentTime);
+//                driveShoot(PathState.WAIT_SHOOT3, currentTime);
+//                break;
+//            case WAIT_SHOOT3:
+////                if (spindexer.shootAllComplete()){
+////                    spindexer.resetSpindexer();
+////                    TELE.addLine("Done Auto");
+////                }
+//                break;
+//            default:
+//                break;
+//        }
+//        TELE.update(); // use for debugging
+//    }
+//
+//    // Voids for State Machine
+//    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);
+//    }
+//    private void driveShoot(PathState nextState, double currentTime){
+//        if (!follower.isBusy()){
+//            pathState = nextState;
+//            timeStamp = currentTime;
+////            spindexer.prepareShootAllContinous();
+//        }
+//    }
+//    private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) {
+//        if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() ||
+////            spindexer.resetSpindexer();
+//            pathState = nextState;
+//            follower.followPath(nextPath, true);
+////            spindexer.setIntakePower(1);
+//        }
+//    }
+//    private void drivePickup(PathState nextState, PathChain nextPath) {
+//        if (!follower.isBusy()) {
+//            pathState = nextState;
+//            follower.followPath(nextPath, intakePower, false);
+//        }
+//    }
+//    private void pickup(PathState nextState, PathChain nextPath) {
+//        if (!follower.isBusy()) {
+//            pathState = nextState;
+//            follower.followPath(nextPath, true);
+//        }
+//    }
+//
+//    // 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());
+////        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);
+//        loopTimes = new MeasuringLoopTimes();
+//        loopTimes.init();
+//
+////        robot.light.setPosition(Color.LightRed);
+//
+//        boolean initializeRobot = false;
+//        while (opModeInInit()){
+//            follower.update();
+//
+//            if (gamepad1.crossWasPressed() && !initializeRobot){
+//                Color.redAlliance = !Color.redAlliance;
+////                if (Color.redAlliance){
+////                    robot.light.setPosition(Color.LightRed);
+////                } else {
+////                    robot.light.setPosition(Color.LightBlue);
+////                }
+//
+////                double[] xPoses = {startPoseX, shoot0X,
+////                        drivePickup1X, pickup1X, shoot1X,
+////                        drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
+////                        drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
+////
+////                double[] headings = {startPoseH, shoot0H,
+////                        drivePickup1H, pickup1H, 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);
+//
+////                turret.setTurret(turrDefault);
+////                servos.setSpinPos(spinStartPos);
+//            }
+//
+//            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;
+//
+////        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();}
+//
+//            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();
+//        }
+//    }
+//}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
@@ -20,6 +20,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferSe
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;

+
 import androidx.annotation.NonNull;

 import com.acmerobotics.dashboard.FtcDashboard;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
+++ b/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;
+
+
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
+++ b/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)
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
+++ b/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 {
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
@@ -1,38 +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;
+
    @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);
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance);
+        shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);

-        drivetrain.setTelemetry(true);

        waitForStart();

        if (isStopRequested()) return;

-        while (opModeIsActive()){
+        while (opModeIsActive()) {

            //Drivetrain

@@ -42,6 +60,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();
        }

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/Hardware_Tester.java
@@ -0,0 +1,140 @@
+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.utilsv2.Robot;
+
+@Config
+@TeleOp
+public class Hardware_Tester extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+
+    public static boolean subsystemMode = true;
+
+    // Bare Motor Powers
+    public static double flPow = 0;
+    public static double frPow = 0;
+    public static double blPow = 0;
+    public static double brPow = 0;
+    public static double intakePow = 0;
+    public static double transferPow = 0;
+    public static double shooter1Pow = 0;
+    public static double shooter2Pow = 0;
+
+    // Subsystem Motor Powers
+    public static double drivetrainPow = 0;
+    public static double shooterPow = 0;
+
+    // Bare Servo Positions
+    public static double spin1Pos = 0.501;
+    public static double spin2Pos = 0.501;
+    public static double turr1Pos = 0.501;
+    public static double turr2Pos = 0.501;
+    public static double transferServosPos = 0.501;
+    public static double hoodPos = 0.501;
+    public static double spindexBlockerPos = 0.501;
+    public static double rapidFireBlockerPos = 0.501;
+    public static double tilt1Pos = 0.501;
+    public static double tilt2Pos = 0.501;
+
+    // Subsystem Servo Positions
+    public static double spinPos = 0.501;
+    public static double turrPos = 0.501;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        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()){
+            // Non-subsystem based components
+            robot.setIntakePower(intakePow);
+            robot.setTransferPower(transferPow);
+
+            if (transferServosPos != 0.501){
+                robot.setTransferServoPos(transferServosPos);
+            }
+            if (hoodPos != 0.501){
+                robot.setHoodPos(hoodPos);
+            }
+            if (rapidFireBlockerPos != 0.501){
+                robot.setRapidFireBlockerPos(rapidFireBlockerPos);
+            }
+            if (spindexBlockerPos != 0.501){
+                robot.setSpindexBlockerPos(spindexBlockerPos);
+            }
+            if (tilt1Pos != 0.501){
+                robot.setTilt1Pos(tilt1Pos);
+            }
+            if (tilt2Pos != 0.501){
+                robot.setTilt2Pos(tilt2Pos);
+            }
+
+            // Subsystem based components
+            if (subsystemMode){
+                robot.setFrontLeftPower(drivetrainPow);
+                robot.setFrontRightPower(drivetrainPow);
+                robot.setBackLeftPower(drivetrainPow);
+                robot.setBackRightPower(drivetrainPow);
+                robot.shooter1.setPower(shooterPow);
+                robot.shooter2.setPower(shooterPow);
+
+                if (spinPos != 0.501){
+                    robot.setSpinPos(spinPos);
+                }
+                if (turrPos != 0.501){
+                    robot.setTurretPos(turrPos);
+                }
+            } else {
+                robot.setFrontLeftPower(flPow);
+                robot.setFrontRightPower(frPow);
+                robot.setBackLeftPower(blPow);
+                robot.setBackRightPower(brPow);
+                robot.shooter1.setPower(shooter1Pow);
+                robot.shooter2.setPower(shooter2Pow);
+
+                if (spin1Pos != 0.501){
+                    robot.spin1.setPosition(spin1Pos);
+                }
+                if (spin2Pos != 0.501){
+                    robot.spin2.setPosition(spin2Pos);
+                }
+                if (turr1Pos != 0.501){
+                    robot.turr1.setPosition(turr1Pos);
+                }
+                if (turr2Pos != 0.501){
+                    robot.turr2.setPosition(turr2Pos);
+                }
+            }
+
+            // 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());
+
+            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
+            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM));
+            TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green));
+
+            TELE.addData("Voltage Sensor", robot.voltage.getVoltage());
+
+            TELE.update();
+        }
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
@@ -0,0 +1,139 @@
+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;
+
+@Config
+@TeleOp
+public class NewShooterTest extends LinearOpMode {
+
+    Robot robot;
+
+    MultipleTelemetry TELE;
+
+    Flywheel rpmFlywheel;
+
+    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.0;
+    public static double turretPos = 0.51;
+    public static double hoodPos = 0.51;
+    public static double flywheel = 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 = Robot.getInstance(hardwareMap);
+
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        Shooter shooter = new Shooter(
+                robot,
+                new MultipleTelemetry(
+                        telemetry,
+                        FtcDashboard.getInstance().getTelemetry()
+                ),
+                Constants.createFollower(hardwareMap),
+                true
+        );
+
+        rpmFlywheel = new Flywheel(robot);
+
+        shooter.setState(Shooter.ShooterState.MANUAL);
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            robot.setHoodPos(hoodPos);
+            shooter.setTurretPosition(turretPos);
+            shooter.setFlywheelVelocity(flywheel);
+            double voltage = robot.voltage.getVoltage();
+            rpmFlywheel.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 Velocity", (robot.shooter1.getVelocity() * 60) / 28);
+            TELE.update();
+
+            shooter.update();
+        }
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
@@ -10,6 +10,7 @@ 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;
@@ -38,12 +39,22 @@ public class Robot {
    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 beam1;
+    public TouchSensor beam2;
+    public TouchSensor beam3;
+    public RevColorSensorV3 revSensor;
+
+    public VoltageSensor voltage;
+
+    // Below is disregarded
    public AnalogInput spin1Pos;
    public AnalogInput spin2Pos;
    public AnalogInput turr1Pos;
@@ -55,7 +66,6 @@ public class Robot {
    public RevColorSensorV3 color3;
    public Limelight3A limelight;
    public Servo light;
-    public VoltageSensor voltage;

    public Robot(HardwareMap hardwareMap) {

@@ -73,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");

@@ -89,34 +100,50 @@ public class Robot {

        hood = hardwareMap.get(Servo.class, "hood");

-        turr1 = hardwareMap.get(Servo.class, "t1");
+        turr1 = hardwareMap.get(Servo.class, "turr1");

-        turr2 = hardwareMap.get(Servo.class, "t2");
+        turr2 = hardwareMap.get(Servo.class, "turr2");

-        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
+        spin1 = hardwareMap.get(Servo.class, "spin1");

-        spin1 = hardwareMap.get(Servo.class, "spin2");
-
-        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
-
-        spin2 = hardwareMap.get(Servo.class, "spin1");
-
-        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+        spin2 = hardwareMap.get(Servo.class, "spin2");

        transfer = hardwareMap.get(DcMotorEx.class, "transfer");
+        transfer.setDirection(DcMotorSimple.Direction.REVERSE);

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

-        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
-
        transfer.setDirection(DcMotorSimple.Direction.REVERSE);
        transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

-        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
+        spindexBlocker = hardwareMap.get(Servo.class, "spinB");

-        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
+        rapidFireBlocker = hardwareMap.get(Servo.class, "rapidB");

-        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
+        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");
+
+        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");
@@ -125,7 +152,144 @@ public class Robot {
            aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
        }

-        light = hardwareMap.get(Servo.class, "light");
+//        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;
+    }
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java
+++ b/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;

@@ -76,10 +74,10 @@ public class Drivetrain {
        double frontRightPower = (y - x - rx) / denominator;
        double backRightPower = (y + x - rx) / denominator;

-        robot.frontLeft.setPower(frontLeftPower);
-        robot.backLeft.setPower(backLeftPower);
-        robot.frontRight.setPower(frontRightPower);
-        robot.backRight.setPower(backRightPower);
+        robot.setFrontLeftPower(frontLeftPower);
+        robot.setBackLeftPower(backLeftPower);
+        robot.setFrontRightPower(frontRightPower);
+        robot.setBackRightPower(backRightPower);

        if (tele) {

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java
@@ -0,0 +1,122 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+import java.util.LinkedList;
+
+public class Flywheel {
+    Robot robot;
+
+    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
+
+    private double velo = 0.0;
+    private double velo1 = 0.0;
+    private double velo2 = 0.0;
+
+    private double averageVelocity = 0.0;
+
+    double targetVelocity = 0.0;
+
+    boolean steady = false;
+
+    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 / 12);
+    }
+
+    public double getVelo() {
+        return velo;
+    }
+
+    public double getVelo1() {
+        return velo1;
+    }
+
+    public double getVelo2() {
+        return velo2;
+    }
+
+    public double getAverageVelocity() {
+        return averageVelocity;
+    }
+
+    public boolean getSteady() {
+        return steady;
+    }
+
+    // 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;
+
+        robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
+
+//        if (Math.abs(prevF - f) > voltagePIDFDifference) {
+//
+//            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
+//
+//            prevF = f;
+//        }
+    }
+
+    // Convert from RPM to Ticks per Second
+    private double RPM_to_TPS(double RPM) {
+        return (RPM * 28.0) / 60.0;
+    }
+
+    // Convert from Ticks per Second to RPM
+    private double TPS_to_RPM(double TPS) {
+        return (TPS * 60.0) / 28.0;
+    }
+
+    private void updateVelocityAverage(double 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();
+    }
+
+    public void manageFlywheel(double commandedVelocity) {
+
+        if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
+            targetVelocity = commandedVelocity;
+        }
+
+        robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
+        double power = robot.shooter1.getPower();
+        robot.shooter2.setPower(power);
+
+        velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
+
+        velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
+
+        velo = (velo1 + velo2) / 2.0;
+
+        updateVelocityAverage(velo);
+
+        steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java
@@ -0,0 +1,309 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+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 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;
+    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);
+        shooter1.setVelocity(0);
+        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;
+    }
+
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
@@ -0,0 +1,160 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+
+
+public class Shooter {
+
+    Robot robot;
+    Flywheel fly;
+    Turret turr;
+    VelocityCommander commander;
+
+    double goalX = 0.0;
+    double goalY = 0.0;
+    double obeliskX = 72;
+    double obeliskY = 144;
+
+    private boolean red = true;
+
+
+    Follower follow;
+
+    public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) {
+        this.robot = rob;
+        this.fly = new Flywheel(rob);
+        this.turr = new Turret(rob);
+        this.follow = follower;
+        this.commander = new VelocityCommander();
+
+        setRedAlliance(redAlliance);
+
+        if (redAlliance) {
+            goalX = 144;
+        } else {
+            goalX = 0;
+        }
+        goalY = 144;
+    }
+
+    public void setRedAlliance(boolean input) {
+        this.red = input;
+    }
+
+    private double flywheelVelocity = 0.0;
+    private double turretPosition = 0.5;
+
+    public enum ShooterState {
+        READ_OBELISK,
+        TRACK_GOAL,
+        MANUAL_FLYWHEEL_TRACK_TURR,
+        MANUAL_TURRET_TRACK_FLY,
+        MANUAL,
+        NOTHING
+    }
+
+
+    private ShooterState state = ShooterState.NOTHING;
+
+    public void setState(ShooterState shooterState) {
+        this.state = shooterState;
+    }
+
+    public void setTurretPosition(double input) {
+        this.turretPosition = input;
+    }
+
+    public void setFlywheelVelocity(double input) {
+        this.flywheelVelocity = input;
+    }
+
+    public int getObeliskID() {
+        return turr.getObeliskID();
+    }
+
+
+    public void update() {
+
+        switch (state) {
+            case NOTHING:
+                break;
+            case MANUAL:
+                fly.manageFlywheel(flywheelVelocity);
+                turr.manual(turretPosition);
+                break;
+            case TRACK_GOAL:
+                turr.trackGoal(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getHeading(),
+                        follow.getAngularVelocity(),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                flywheelVelocity = commander.getVeloPredictive(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+            case READ_OBELISK:
+                turr.trackObelisk(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getHeading()
+                );
+
+                flywheelVelocity = commander.getVeloPredictive(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+
+            case MANUAL_TURRET_TRACK_FLY:
+                turr.manual(turretPosition);
+                flywheelVelocity = commander.getVeloPredictive(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+
+            case MANUAL_FLYWHEEL_TRACK_TURR:
+                turr.trackGoal(
+                        (follow.getPose().getX() - goalX),
+                        (follow.getPose().getY() - goalY),
+                        follow.getHeading(),
+                        follow.getAngularVelocity(),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+                fly.manageFlywheel(flywheelVelocity);
+                break;
+
+        }
+
+    }
+
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
+++ b/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;
+        }
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
@@ -0,0 +1,111 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.robotcore.util.Range;
+
+
+import java.util.List;
+
+@Config
+public class Turret {
+    Robot robot;
+
+    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
+    private final double yVelK = 0; // TODO: Tune
+    private final double yAccK = 0; // TODO: Tune
+
+    private int obeliskID = 0;
+
+
+
+    public Turret(Robot rob) {
+        this.robot = rob;
+    }
+
+    private double wrapAngle(double angle) {
+        while (angle > Math.PI) angle -= 2.0 * Math.PI;
+        while (angle < -Math.PI) angle += 2.0 * Math.PI;
+        return angle;
+    }
+
+    public void trackObelisk(double dx, double dy, double h) {
+
+        double heading = wrapAngle(h);
+
+        double fieldRelativeHeading = Math.atan2(dy, dx);
+
+        double desiredAngle = fieldRelativeHeading - heading;
+        double angleDelta = desiredAngle - Math.PI;
+        angleDelta = wrapAngle(angleDelta);
+
+        double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
+
+        double servoAngle = neutralPosition + servoTicksFromNeutral;
+
+        servoAngle = Range.clip(servoAngle, turretMin, turretMax);
+
+        robot.setTurretPos(servoAngle);
+
+
+        detectObelisk();
+
+    }
+
+    public int getObeliskID() {
+        return obeliskID;
+    }
+
+    private int detectObelisk() {
+        robot.limelight.pipelineSwitch(1);
+        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();
+                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
+
+        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
+
+        predictedH = wrapAngle(predictedH);
+
+        double fieldRelativeHeading = Math.atan2(predictedDy, predictedDx);
+
+        double angleDelta = fieldRelativeHeading - predictedH;
+        angleDelta = wrapAngle(angleDelta);
+
+        double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
+
+        double servoAngle = neutralPosition + servoTicksFromNeutral;
+
+        servoAngle = Range.clip(servoAngle, turretMin, turretMax);
+
+        robot.setTurretPos(servoAngle);
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java
@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+public class VelocityCommander {
+    private final double goalH = 20.0; //TODO: Tune
+    private final double xVelK = 0; // TODO: Tune
+    private final double xAccK = 0; // TODO: Tune
+    private final double yVelK = 0; // TODO: Tune
+    private final double yAccK = 0; // TODO: Tune
+
+    public VelocityCommander() {
+
+    }
+
+    private double distToRPM (double dist){
+        return Math.sqrt(dist*dist + goalH*goalH);
+        //TODO: Add regression here using goalH
+    }
+
+    public double getVeloStationary (double distance){
+        return distToRPM(distance);
+    }
+
+    public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) {
+
+        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);
+
+        return distToRPM(predictedDist);
+    }
+
+
+}
Author	SHA1	Message	Date
DanTheMan-byte	7665957c7a	readjusted shooter test @KeshavAnandCode please merge	2026-06-02 19:14:58 -05:00
DanTheMan-byte	ccc6a608fc	Merge branch 'update-teleop' into danielv5	2026-06-02 18:28:00 -05:00
DanTheMan-byte	8eba32de94	new auto in progress	2026-06-02 18:22:41 -05:00
DanTheMan-byte	5c9ebf6eac	some changes	2026-06-02 18:22:28 -05:00
Keshav Anand	a540d333f1	shoooooottteeer test	2026-06-02 18:12:32 -05:00
Keshav Anand	180e7629bf	Added spindexer to teleopv4	2026-06-02 17:18:04 -05:00
Keshav Anand	ae25df0393	Fixed spidnexer i think	2026-06-02 17:08:26 -05:00
DanTheMan-byte	946deca751	middle of tuning	2026-06-02 17:04:45 -05:00
DanTheMan-byte	75b9b7b6b1	middle of tuning	2026-06-02 17:04:28 -05:00
Keshav Anand	1a1c99791d	Made Robot Singleton	2026-06-02 16:31:33 -05:00
Keshav Anand	88cf03a230	Merge remote-tracking branch 'origin/danielv5' into update-teleop	2026-06-02 15:59:18 -05:00
DanTheMan-byte	82c8ebf941	umiddle of tuning	2026-06-02 15:58:49 -05:00
Keshav Anand	aabc746a2e	Stash update	2026-06-02 15:57:31 -05:00
DanTheMan-byte	f14dc3681a	setup robot confg	2026-06-02 15:41:01 -05:00
Keshav Anand	184ec893a4	Added shooter class to manager flywheel turret and targetter	2026-06-01 17:12:17 -05:00
Keshav Anand	f32f31a224	Merge remote-tracking branch 'origin/danielv5' into update-teleop	2026-06-01 16:25:07 -05:00
Keshav Anand	bfb37f13f8	added neutral shift	2026-06-01 16:24:11 -05:00
DanTheMan-byte	ccc0e2123a	programmer to config robot	2026-06-01 16:21:48 -05:00
DanTheMan-byte	a470b7dbc4	loop times calling	2026-06-01 16:21:31 -05:00
DanTheMan-byte	dd2890ea4a	Merge branch 'main' into danielv5	2026-06-01 14:44:08 -05:00
DanTheMan-byte	76f58308fb	IGNORE THIS NEXT PUSH	2026-05-25 22:38:26 -05:00
Keshav Anand	658e8ea1d0	Added flywheel copied basically but added average velo calculations	2026-05-25 16:35:25 -07:00
Keshav Anand	9ab69f8fbe	added obelisk code	2026-05-25 16:29:38 -07:00
Keshav Anand	ed970eaf38	changed to radians, accounted for velo and acc	2026-05-25 15:39:10 -07:00
Keshav Anand	e3105a339d	added turretv2 starter code	2026-05-25 15:19:05 -07:00
DanTheMan-byte	4567a4117c	Merge branch 'main' into danielv5	2026-05-18 19:46:41 -05:00