Sorting beta workish

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

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

@@ -1,341 +1,366 @@
-//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();
-//        }
-//    }
-//}
+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/teleop/TeleopV4.java

@@ -25,24 +25,38 @@ public class TeleopV4 extends LinearOpMode {
     MultipleTelemetry TELE;
     Follower follower;
     SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+
+    ParkTilter parkTilter;
 
     @Override
     public void runOpMode() throws InterruptedException {
 
+        Robot.resetInstance();
+
         robot = Robot.getInstance(hardwareMap);
 
         TELE = new MultipleTelemetry(
                 FtcDashboard.getInstance().getTelemetry(), telemetry
         );
 
+        commander = new VelocityCommander();
         drivetrain = new Drivetrain(robot, TELE);
         follower = Constants.createFollower(hardwareMap);
         Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
         follower.setStartingPose(start);
 
-        shooter = new Shooter(robot, TELE, follower, Color.redAlliance);
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        parkTilter = new ParkTilter(robot);
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
         shooter.setState(Shooter.ShooterState.TRACK_GOAL);
-        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE);
+        shooter.setRedAlliance(Color.redAlliance);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
 
@@ -51,21 +65,22 @@ public class TeleopV4 extends LinearOpMode {
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
-
             //Drivetrain
-
             drivetrain.drive(
                     -gamepad1.right_stick_y,
                     gamepad1.right_stick_x,
                     gamepad1.left_stick_x
             );
 
-            shooter.update();
+            follower.update();
+
+
+            shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
 
             SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
 
-            if (gamepad1.xWasPressed() &&
+            if (gamepad1.leftBumperWasPressed() &&
                     (state == SpindexerTransferIntake.RapidMode.INTAKE ||
                             state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
                             state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
@@ -76,7 +91,7 @@ public class TeleopV4 extends LinearOpMode {
                 spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
             }
 
-            if (gamepad1.aWasPressed() &&
+            if (gamepad1.right_trigger > 0.5 &&
                     (state == SpindexerTransferIntake.RapidMode.INTAKE ||
                             state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
 
@@ -84,8 +99,7 @@ public class TeleopV4 extends LinearOpMode {
                         SpindexerTransferIntake.RapidMode.HOLD_BALLS
                 );
             }
-
-            if (gamepad1.yWasPressed()
+            if (gamepad1.rightBumperWasPressed()
                     && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
 
                 spindexerTransferIntake.setRapidMode(
@@ -93,6 +107,17 @@ public class TeleopV4 extends LinearOpMode {
                 );
             }
 
+            if (gamepad1.dpad_down){
+                parkTilter.park();
+            } else if (gamepad1.dpad_up) {
+                parkTilter.unpark();
+            }
+
+            TELE.addData("Distance From Goal", commander.getDistance());
+            TELE.addData("Hood Position", commander.getHoodPredicted());
+            TELE.addData("Transfer Power", robot.transfer.getPower());
+            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
 
             TELE.update();
         }

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

@@ -124,12 +124,13 @@ public class Hardware_Tester extends LinearOpMode {
 
             // Sensor Data
 
-//            TELE.addData("Beam Break 1?", robot.beam1.isPressed());
+            TELE.addData("Beam Break insideBeam?", robot.insideBeam.isPressed());
+            TELE.addData("Beam Break outsideBeam?", robot.outsideBeam.isPressed());
 //            TELE.addData("Beam Break 2?", robot.beam2.isPressed());
 //            TELE.addData("Beam Break 3?", robot.beam3.isPressed());
 
             NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
-            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM));
+            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM));
             TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green));
 
             TELE.addData("Voltage Sensor", robot.voltage.getVoltage());

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

@@ -1,139 +1,131 @@
 package org.firstinspires.ftc.teamcode.tests;
 
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
-import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
-import org.firstinspires.ftc.teamcode.utilsv2.Robot;
-import org.firstinspires.ftc.teamcode.utilsv2.Shooter;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
 
-@Config
 @TeleOp
+@Config
 public class NewShooterTest extends LinearOpMode {
-
     Robot robot;
-
+    Drivetrain drivetrain;
+    Shooter shooter;
     MultipleTelemetry TELE;
+    Follower follower;
+    SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+    ParkTilter parkTilter;
 
-    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;
+    public static int flywheelVelo = 0;
+    public static double hoodPos = 0.5;
+    public static double transferPower = -0.7;
+//    public static double turretPos = 0.51;
 
     @Override
     public void runOpMode() throws InterruptedException {
 
+        Robot.resetInstance();
+
         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
+        TELE = new MultipleTelemetry(
+                FtcDashboard.getInstance().getTelemetry(), telemetry
         );
 
-        rpmFlywheel = new Flywheel(robot);
+        commander = new VelocityCommander();
+        drivetrain = new Drivetrain(robot, TELE);
+        follower = Constants.createFollower(hardwareMap);
+        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(start);
 
-        shooter.setState(Shooter.ShooterState.MANUAL);
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        parkTilter = new ParkTilter(robot);
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+        shooter.setRedAlliance(Color.redAlliance);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
         waitForStart();
 
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
+            //Drivetrain
+            drivetrain.drive(
+                    -gamepad1.right_stick_y,
+                    gamepad1.right_stick_x,
+                    gamepad1.left_stick_x
+            );
 
+            follower.update();
+
+            shooter.setFlywheelVelocity(flywheelVelo);
             robot.setHoodPos(hoodPos);
-            shooter.setTurretPosition(turretPos);
-            shooter.setFlywheelVelocity(flywheel);
-            double voltage = robot.voltage.getVoltage();
-            rpmFlywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage);
+//            shooter.setTurretPosition(turretPos);
+            shooter.update(robot.voltage.getVoltage());
+            spindexerTransferIntake.update();
 
-            robot.setSpinPos(ServoPositions.spindexer_A2);
+            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
 
-            if (intake && !shoot) {
+            if (gamepad1.leftBumperWasPressed() &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
+                            state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
+                            state == SpindexerTransferIntake.RapidMode.PULSE_OUT ||
+                            state == SpindexerTransferIntake.RapidMode.PULSE_IN ||
+                            state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) {
 
-                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;
-                }
+                spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
             }
 
-            TELE.addData("Flywheel Velocity", (robot.shooter1.getVelocity() * 60) / 28);
-            TELE.update();
+            if (gamepad1.right_trigger > 0.5 &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
 
-            shooter.update();
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
+                );
+            }
+            if (gamepad1.rightBumperWasPressed()
+                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.INTAKE
+                );
+            }
+
+            if (gamepad1.dpad_down){
+                parkTilter.park();
+            } else if (gamepad1.dpad_up) {
+                parkTilter.unpark();
+            }
+
+            TELE.addData("Distance From Goal", commander.getDistance());
+            TELE.addData("Hood Position", commander.getHoodPredicted());
+            TELE.addData("Transfer Power", commander.getTransferPow());
+            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+
+            TELE.update();
         }
+
     }
 }

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

@@ -0,0 +1,141 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
+
+@TeleOp
+@Config
+public class SortedSpindexerTest extends LinearOpMode {
+    Robot robot;
+    Drivetrain drivetrain;
+    Shooter shooter;
+    MultipleTelemetry TELE;
+    Follower follower;
+    SpindexerTransferIntake spindexerTransferIntake;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+
+    ParkTilter parkTilter;
+    public static String order = "GPP";
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        Robot.resetInstance();
+
+        robot = Robot.getInstance(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                FtcDashboard.getInstance().getTelemetry(), telemetry
+        );
+
+        commander = new VelocityCommander();
+        drivetrain = new Drivetrain(robot, TELE);
+        follower = Constants.createFollower(hardwareMap);
+        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(start);
+
+        flywheel = new Flywheel(robot);
+        turret = new Turret(robot);
+
+        parkTilter = new ParkTilter(robot);
+
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+        shooter.setRedAlliance(Color.redAlliance);
+        spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
+        spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SORTED);
+
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+
+            switch(order) {
+                case "PPG":
+                    spindexerTransferIntake.setDesiredPattern(
+                            SpindexerTransferIntake.DesiredPattern.PPG
+                    );
+                    break;
+
+                case "PGP":
+                    spindexerTransferIntake.setDesiredPattern(
+                            SpindexerTransferIntake.DesiredPattern.PGP
+                    );
+                    break;
+
+                default:
+                    spindexerTransferIntake.setDesiredPattern(
+                            SpindexerTransferIntake.DesiredPattern.GPP
+                    );
+            }
+
+
+            //Drivetrain
+            drivetrain.drive(
+                    -gamepad1.right_stick_y,
+                    gamepad1.right_stick_x,
+                    gamepad1.left_stick_x
+            );
+
+            follower.update();
+
+
+            shooter.update(robot.voltage.getVoltage());
+            spindexerTransferIntake.update();
+
+            SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
+
+            if(gamepad1.leftBumperWasPressed()) {
+                spindexerTransferIntake.startSortedShoot();
+            }
+
+            if (gamepad1.right_trigger > 0.5 &&
+                    (state == SpindexerTransferIntake.RapidMode.INTAKE ||
+                            state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
+                );
+            }
+            if (gamepad1.rightBumperWasPressed()
+                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
+
+                spindexerTransferIntake.setRapidMode(
+                        SpindexerTransferIntake.RapidMode.INTAKE
+                );
+            }
+
+            if (gamepad1.dpad_down){
+                parkTilter.park();
+            } else if (gamepad1.dpad_up) {
+                parkTilter.unpark();
+            }
+
+            TELE.addData("Distance From Goal", commander.getDistance());
+            TELE.addData("Hood Position", commander.getHoodPredicted());
+            TELE.addData("Transfer Power", robot.transfer.getPower());
+            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+
+            TELE.update();
+        }
+
+    }
+}

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

@@ -1,16 +1,25 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 
+import org.firstinspires.ftc.teamcode.utilsv2.Robot;
+
 import java.util.LinkedList;
 
+@Config
 public class Flywheel {
     Robot robot;
 
-    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
+//    public PIDFCoefficients shooterPIDF1, shooterPIDF2;
+    public static PIDFCoefficients shooterPIDF;
+    public static double shooterPIDF_P = 500;
+    public static double shooterPIDF_I = 1;
+    public static double shooterPIDF_D = 0.0;
+    public static double shooterPIDF_F = 93;
 
     private double velo = 0.0;
     private double velo1 = 0.0;
@@ -26,7 +35,7 @@ public class Flywheel {
 
     public Flywheel(Robot rob) {
         robot = rob;
-        shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / 12);
+        shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
     }
 
     public double getVelo() {
@@ -50,25 +59,27 @@ 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;
 
-        robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
+        robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+    }
 
-//        if (Math.abs(prevF - f) > voltagePIDFDifference) {
-//
-//            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
-//
-//            prevF = f;
-//        }
+    private double prevF = 0;
+
+    public static double voltagePIDFDifference = 1;
+    public void setF(double voltage){
+        double f = shooterPIDF_F / voltage;
+        if (Math.abs(prevF - f) > voltagePIDFDifference) {
+            shooterPIDF.f = f;
+            robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+            prevF = f;
+        }
     }
 
     // Convert from RPM to Ticks per Second
@@ -99,6 +110,7 @@ public class Flywheel {
         averageVelocity = sum / velocityHistory.size();
     }
 
+    double power;
     public void manageFlywheel(double commandedVelocity) {
 
         if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
@@ -106,7 +118,7 @@ public class Flywheel {
         }
 
         robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
-        double power = robot.shooter1.getPower();
+        power = robot.shooter1.getPower();
         robot.shooter2.setPower(power);
 
         velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
@@ -119,4 +131,7 @@ public class Flywheel {
 
         steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
     }
+
+
+    public double getShooterPower(){return power;}
 }

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

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

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import android.view.View;
+
 import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.hardware.AnalogInput;
@@ -19,6 +21,7 @@ public class Robot {
     // Singleton instance
     private static Robot instance;
 
+
     /**
      * Returns the existing Robot instance or creates one if it doesn't exist.
      */
@@ -45,11 +48,6 @@ public class Robot {
     public DcMotorEx backRight;
     public DcMotorEx intake;
     public DcMotorEx transfer;
-    public PIDFCoefficients shooterPIDF;
-    public static double shooterPIDF_P = 255;
-    public static double shooterPIDF_I = 0.0;
-    public static double shooterPIDF_D = 0.0;
-    public static double shooterPIDF_F = 75;
     //    public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;
@@ -65,7 +63,6 @@ public class Robot {
     public Servo spin2;
     public TouchSensor insideBeam;
     public TouchSensor outsideBeam;
-
     public RevColorSensorV3 revSensor;
 
     public VoltageSensor voltage;
@@ -106,10 +103,9 @@ public class Robot {
         shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
 
         shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
-        shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / 12);
+//        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);
+//        shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
         shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
         hood = hardwareMap.get(Servo.class, "hood");
@@ -173,7 +169,7 @@ public class Robot {
     // Voids below are used to minimize hardware calls to minimize loop times
 
     // Used to cut off digits that are negligible
-    private final int maxDigits = 5;
+    private final int maxDigits = 3;
     private final int roundingFactor = (int) Math.pow(10, maxDigits);
 
     private double prevFrontLeftPower = -10.501;

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

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

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

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

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

@@ -83,7 +83,7 @@ public class Turret {
         robot.setTurretPos(pos);
 
     }
-
+    // 1.545
 
     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

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

@@ -1,34 +1,117 @@
 package org.firstinspires.ftc.teamcode.utilsv2;
 
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
 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 static double xVelK = 0.05; // TODO: Tune
+    public static double xAccK = 0.025; // TODO: Tune
+    public static double yVelK = 0.05; // TODO: Tune
+    public static double yAccK = 0.025; // TODO: Tune
+    private double hoodPos = 0.88;
+    private double transferPow = -1;
+    private double velo = 0;
 
-    public VelocityCommander() {
-
-    }
+    public VelocityCommander() {}
 
+    private final double veloA = -2.703087757*Math.pow(10, -14);
+    private final double veloB = 2.904756341*Math.pow(10, -11);
+    private final double veloC = -1.381814293*Math.pow(10, -8);
+    private final double veloD = 0.000003829224585;
+    private final double veloE = -0.000684090204;
+    private final double veloF = 0.0822754689;
+    private final double veloG = -6.743119277;
+    private final double veloH = 371.7359504;
+    private final double veloI = -13189.70958;
+    private final double veloJ = 272005.7124;
+    private final double veloK = -2474581.713;
     private double distToRPM (double dist){
-        return Math.sqrt(dist*dist + goalH*goalH);
-        //TODO: Add regression here using goalH
+        if (dist < 49) {
+            velo = 2000;
+        } else if (dist > 165){
+            velo = 3760;
+        } else {
+            velo = veloA*Math.pow(dist, 10) +
+                    veloB*Math.pow(dist, 9) +
+                    veloC*Math.pow(dist, 8) +
+                    veloD*Math.pow(dist, 7) +
+                    veloE*Math.pow(dist, 6) +
+                    veloF*Math.pow(dist, 5) +
+                    veloG*Math.pow(dist, 4) +
+                    veloH*Math.pow(dist, 3) +
+                    veloI*Math.pow(dist, 2) +
+                    veloJ*Math.pow(dist, 1) +
+                    veloK;
+            velo = Math.max(2000, Math.min(3760, velo));
+        }
+        return velo;
     }
 
+    private final double hoodA = -4.3276177*Math.pow(10, -13);
+    private final double hoodB = 2.68062979*Math.pow(10, -10);
+    private final double hoodC = -7.12859632*Math.pow(10, -8);
+    private final double hoodD = 0.0000106010785;
+    private final double hoodE = -0.000960693973;
+    private final double hoodF = 0.0540375808;
+    private final double hoodG = -1.82724027;
+    private final double hoodH = 33.4797545;
+    private final double hoodI = -246.888632;
+    private void distToHood (double dist){
+        if (dist > 112){
+            hoodPos = 0.35;
+        } else if (dist < 49){
+            hoodPos = 0.88;
+        } else {
+            hoodPos = hoodA*Math.pow(dist, 8) +
+                    hoodB*Math.pow(dist, 7) +
+                    hoodC*Math.pow(dist, 6) +
+                    hoodD*Math.pow(dist, 5) +
+                    hoodE*Math.pow(dist, 4) +
+                    hoodF*Math.pow(dist, 3) +
+                    hoodG*Math.pow(dist, 2) +
+                    hoodH*Math.pow(dist, 1) +
+                    hoodI;
+
+            hoodPos = Math.max(0.35, Math.min(0.88, hoodPos));
+        }
+    }
+    public double getHoodPredicted(){
+        return hoodPos;
+    }
+
+    private void distToTransferPow(double dist, double voltage){
+        if (dist < 118){
+            transferPow = -1;
+        } else if (dist < 125){
+            transferPow = -0.7;
+        } else {
+            transferPow = -0.5;
+        }
+
+//        transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage)));
+    }
+    public double getTransferPow(){return transferPow;}
+
+    // 27
     public double getVeloStationary (double distance){
         return distToRPM(distance);
     }
 
-    public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) {
+    double predictedDist = 0;
+    public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage) {
 
         double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
         double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
 
-        double predictedDist = Math.sqrt(dx*dx + dy*dy);
+        double goalHeight = 28;
+        predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight * goalHeight);
 
-        return distToRPM(predictedDist);
+        distToHood(predictedDist);
+        distToTransferPow(predictedDist, voltage);
+        distToRPM(predictedDist);
     }
 
+    public double getPredictedRPM(){return velo;}
 
+    public double getDistance(){return predictedDist;}
 }