tweaks were made and hardware changes are needed

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

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

@@ -337,6 +337,8 @@ public class Auto12BallPedroPathing extends LinearOpMode {
                 initializePoses();
                 follower.setPose(startPose);
                 buildPaths();
+//                turret.switchPipeline(Turret.PipelineMode.OBELISK);
+                robot.limelight.start();
                 sleep(2000);
 
 //                turret.setTurret(turrDefault);

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

@@ -326,6 +326,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 initializePoses();
                 follower.setPose(startPose);
                 buildPaths();
+//                turret.switchPipeline(Turret.PipelineMode.OBELISK);
+                robot.limelight.start();
                 sleep(2000);
             }
 

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -16,6 +16,8 @@ import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 
+import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
+
 @TeleOp
 @Config
 public class TeleopV4 extends LinearOpMode {
@@ -28,9 +30,10 @@ public class TeleopV4 extends LinearOpMode {
     Turret turret;
     Flywheel flywheel;
     VelocityCommander commander;
-
     ParkTilter parkTilter;
 
+    public static Pose relocalizePose = new Pose(128, 83, 0);
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -52,6 +55,7 @@ public class TeleopV4 extends LinearOpMode {
         turret = new Turret(robot);
 
         parkTilter = new ParkTilter(robot);
+        parkTilter.unpark();
 
         shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
         shooter.setState(Shooter.ShooterState.TRACK_GOAL);
@@ -59,6 +63,10 @@ public class TeleopV4 extends LinearOpMode {
         spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
+        turret.switchPipeline(Turret.PipelineMode.TRACKING);
+        robot.limelight.start();
+
+        limelightUsed = true;
 
         waitForStart();
 
@@ -72,8 +80,26 @@ public class TeleopV4 extends LinearOpMode {
                     gamepad1.left_stick_x
             );
 
+            if (gamepad1.crossWasPressed()){
+                follower.setPose(relocalizePose);
+                gamepad1.rumble(100);
+            }
+
             follower.update();
 
+            if (gamepad1.dpadLeftWasPressed()){
+                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            }
+
+            if (gamepad1.dpadRightWasPressed()){
+                shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+            }
+
+            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
+                shooter.setFlywheelVelocity(2500);
+                robot.setHoodPos(0.6);
+                robot.setTransferPower(-0.8);
+            }
 
             shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
@@ -99,12 +125,15 @@ public class TeleopV4 extends LinearOpMode {
                         SpindexerTransferIntake.RapidMode.HOLD_BALLS
                 );
             }
-            if (gamepad1.rightBumperWasPressed()
-                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
 
-                spindexerTransferIntake.setRapidMode(
+            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
-                        SpindexerTransferIntake.RapidMode.INTAKE
+                robot.setIntakePower(1);
-                );
+            }
+
+            if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+            } else if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
             }
 
             if (gamepad1.dpad_down){

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

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

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

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

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

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

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

@@ -3,6 +3,7 @@ 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 static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -14,6 +15,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.teleop.TeleopV4;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 
 @TeleOp
@@ -56,6 +58,7 @@ public class NewShooterTest extends LinearOpMode {
         turret = new Turret(robot);
 
         parkTilter = new ParkTilter(robot);
+        parkTilter.unpark();
 
         shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
         shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
@@ -63,6 +66,11 @@ public class NewShooterTest extends LinearOpMode {
         spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander);
         spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
 
+        turret.switchPipeline(Turret.PipelineMode.TRACKING);
+        robot.limelight.start();
+
+        limelightUsed = true;
+
         waitForStart();
 
         if (isStopRequested()) return;
@@ -75,14 +83,38 @@ public class NewShooterTest extends LinearOpMode {
                     gamepad1.left_stick_x
             );
 
+            if (gamepad1.crossWasPressed()){
+                follower.setPose(TeleopV4.relocalizePose);
+                gamepad1.rumble(100);
+            }
+
             follower.update();
 
-            shooter.setFlywheelVelocity(flywheelVelo);
+            if (gamepad1.dpadLeftWasPressed()){
-            robot.setHoodPos(hoodPos);
+                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            }
+
+            if (gamepad1.dpadRightWasPressed()){
+                shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+            }
+
+            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
+                shooter.setFlywheelVelocity(flywheelVelo);
+                robot.setHoodPos(hoodPos);
+                robot.setTransferPower(transferPower);
+            }
+
+
 //            shooter.setTurretPosition(turretPos);
             shooter.update(robot.voltage.getVoltage());
             spindexerTransferIntake.update();
 
+            if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+            } else if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
+            }
+
             SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
 
             if (gamepad1.leftBumperWasPressed() &&
@@ -104,12 +136,8 @@ public class NewShooterTest extends LinearOpMode {
                         SpindexerTransferIntake.RapidMode.HOLD_BALLS
                 );
             }
-            if (gamepad1.rightBumperWasPressed()
+            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
-                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
+                robot.setIntakePower(1);
-
-                spindexerTransferIntake.setRapidMode(
-                        SpindexerTransferIntake.RapidMode.INTAKE
-                );
             }
 
             if (gamepad1.dpad_down){
@@ -123,6 +151,7 @@ public class NewShooterTest extends LinearOpMode {
             TELE.addData("Transfer Power", commander.getTransferPow());
             TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
             TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+            TELE.addData("TX:", turret.getTX());
 
             TELE.update();
         }

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

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

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

@@ -0,0 +1,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/tests/SortingTest.java

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -43,13 +43,13 @@ public class Shooter {
 
         if (this.red) {
             goalX = 144;
-            turretGoalX = 136;
+            turretGoalX = 140;
         } else {
             goalX = 0;
             turretGoalX = 8;
         }
         goalY = 144;
-        turretGoalY = 136;
+        turretGoalY = 132;
     }
 
     private double flywheelVelocity = 0.0;
@@ -71,6 +71,10 @@ public class Shooter {
         this.state = shooterState;
     }
 
+    public ShooterState getState(){
+        return state;
+    }
+
     public void setTurretPosition(double input) {
         this.turretPosition = input;
     }

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

@@ -4,6 +4,7 @@ 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;
@@ -16,17 +17,175 @@ public class SpindexerTransferIntake {
 
     VelocityCommander commander;
 
-    public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE, VelocityCommander com) {
+    private MultipleTelemetry TELE;
+
+    public SpindexerTransferIntake(Robot rob, MultipleTelemetry tele, VelocityCommander com) {
         this.robot = rob;
         this.commander = com;
+        this.TELE = tele;
     }
 
+    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 {
@@ -42,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;
@@ -55,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;
     }
 
@@ -67,8 +257,29 @@ public class SpindexerTransferIntake {
         return System.currentTimeMillis() - stateStartTime;
     }
 
+    private long sortedStateTime() {
+        return System.currentTimeMillis() - sortedStateStartTime;
+    }
+
+    private int greenTicks = 0;
+    private int ballTicks = 0;
     public void update() {
 
+        TELE.addData("Sorted State", sortedIntakeStates);
+        TELE.addData("Ball0", ballColors[0]);
+        TELE.addData("Ball1", ballColors[1]);
+        TELE.addData("Ball2", ballColors[2]);
+
+        TELE.addData("Shoot0", shootOrder[0]);
+        TELE.addData("Shoot1", shootOrder[1]);
+        TELE.addData("Shoot2", shootOrder[2]);
+
+        TELE.addData("Color0", ballColors[0]);
+        TELE.addData("Color1", ballColors[1]);
+        TELE.addData("Color2", ballColors[2]);
+
+        TELE.addData("Shoot State", shootState);
+        
         switch (mode) {
 
             case RAPID:
@@ -164,7 +375,7 @@ public class SpindexerTransferIntake {
                             setRapidMode(RapidMode.SHOOT);
                         }
 
-                        if (Shooter.manualFlywheel){
+                        if (Shooter.manualFlywheel) {
                             robot.setTransferPower(NewShooterTest.transferPower);
                         } else {
                             robot.setTransferPower(commander.getTransferPow());
@@ -181,7 +392,7 @@ public class SpindexerTransferIntake {
                             setRapidMode(RapidMode.INTAKE);
                         }
 
-                        if (Shooter.manualFlywheel){
+                        if (Shooter.manualFlywheel) {
                             robot.setTransferPower(NewShooterTest.transferPower);
                         } else {
                             robot.setTransferPower(commander.getTransferPow());
@@ -193,7 +404,315 @@ public class SpindexerTransferIntake {
 
             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) {
+                            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
+                            if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
+                                greenTicks++;
+                            }
+                            ballTicks++;
+                            if (ballTicks > 15) {
+                                if (greenTicks > 10){
+                                    ballColors[0] = BallStates.GREEN;
+                                } else {
+                                    ballColors[0] = BallStates.PURPLE;
+                                }
+                                robot.setSpinPos(ServoPositions.spindexer_A2);
+                                setSortedIntakeMode(SortedIntakeStates.DELAY_1);
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
+                        }
+                        break;
+                    case DELAY_1:
+                        robot.setSpinPos(ServoPositions.spindexer_A2);
+                        if (sortedStateTime() > spinMovementTime) {
+                            setSortedIntakeMode(SortedIntakeStates.INTAKE_2);
+                        }
+                        break;
+                    case INTAKE_2:
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(-1);
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(-1);
+                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+                            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
+                            if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
+                                greenTicks++;
+                            }
+                            ballTicks++;
+                            if (ballTicks > 15) {
+                                if (greenTicks > 10){
+                                    ballColors[0] = BallStates.GREEN;
+                                } else {
+                                    ballColors[0] = BallStates.PURPLE;
+                                }
+                                robot.setSpinPos(ServoPositions.spindexer_A3);
+                                setSortedIntakeMode(SortedIntakeStates.DELAY_2);
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
+                        }
+                        break;
+                    case DELAY_2:
+
+                        robot.setSpinPos(
+                                ServoPositions.spindexer_A3
+                        );
+
+                        if (sortedStateTime() > spinMovementTime) {
+                            setSortedIntakeMode(
+                                    SortedIntakeStates.INTAKE_3
+                            );
+                        }
+
+                        break;
+                    case INTAKE_3:
+                        robot.setIntakePower(1);
+                        robot.setTransferPower(-1);
+                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+                            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
+                            if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
+                                greenTicks++;
+                            }
+                            ballTicks++;
+                            if (ballTicks > 15) {
+                                if (greenTicks > 10){
+                                    ballColors[0] = BallStates.GREEN;
+                                } else {
+                                    ballColors[0] = BallStates.PURPLE;
+                                }
+                                setSortedIntakeMode(SortedIntakeStates.REVERSE);
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
+                        }
+                        break;
+                    case REVERSE:
+                        robot.setTransferPower(-0.3);
+                        robot.setIntakePower(-0.1);
+                        break;
+                }
+
+
+                break;
+            case SHOOT_SORTED:
+
+                robot.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

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