sorting auto is done

small tweaks: added current pose telemetry and reduced coasting from limelight track
small tweaks: added loop times measurement
2026-06-05 18:25:12 -05:00 · 2026-06-05 14:29:21 -05:00 · 2026-06-05 14:19:39 -05:00 · 2026-06-05 14:18:47 -05:00 · 2026-06-04 22:13:49 -05:00 · 2026-06-04 20:57:19 -05:00
29 changed files with 459 additions and 154 deletions
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java
@@ -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);
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
@@ -18,11 +18,10 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

 import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
-import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
+import org.firstinspires.ftc.teamcode.utilsv2.*;
 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;

@@ -33,45 +32,52 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
    MultipleTelemetry TELE;
    Follower follower;
    MeasuringLoopTimes loopTimes;
+    Shooter shooter;
+    Turret turret;
+    Flywheel flywheel;
+    VelocityCommander commander;
+    SpindexerTransferIntake spindexer;

    // Wait Times
-    public static double shootTime = 2;
-    public static double openGateTime = 1.5;
+    public static double sortedShootTime = 2;
+    public static double rapidWaitTime = 0.25;
+    public static double rapidShootTime = 1;
+    public static double openGateTime = 2.5;
+    public static double pushTime = 2;

    // Extra Variables
-    public static double intakePower = 0.3;
+    public static double intakePower = 0.5;
    double shootX, shootY, shootH;

    // Initialize path state machine
    private enum PathState {
        PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
-        PICKUP1, DRIVE_OPENGATE, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
+        PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
        DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
        DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
    }
    PathState pathState = PathState.PUSHBOT;

    // Poses
-    public static double startPoseX = 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 startPoseX = 91.5, startPoseY = 15, startPoseH = 90;
+    public static double pushBotX = 97, pushBotY = 18, pushBotH = 100;
+    public static double shoot0ControlX = 94.29667812142038, shoot0ControlY = 55.03493699885454;
+    public static double shoot0X = 95, shoot0Y = 83, 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 openGateX = 131, 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 shoot1X = 95, shoot1Y = 83, shoot1H = 0;
+    public static double drivePickup2X = 98, 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 shoot2X = 95, shoot2Y = 83, shoot2H = 0;
+    public static double drivePickup3X = 98, 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;
+    public static double shoot3X = 84, shoot3Y = 120, shoot3H = -90;
    Pose startPose, pushBot, shoot0Control, shoot0,
-            pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1,
+            pickup1, openGateControl, openGate, shoot1Control, shoot1,
            drivePickup2, pickup2, shoot2Control, shoot2,
            drivePickup3, pickup3, shoot3Control, shoot3;
    private void initializePoses(){
@@ -79,7 +85,6 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
        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));
@@ -112,7 +117,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                .build();

        shoot0_pickup1 = follower.pathBuilder()
-                .addPath(new BezierCurve(shoot0, pickup1Control, pickup1))
+                .addPath(new BezierLine(shoot0, pickup1))
                .setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading())
                .build();

@@ -164,42 +169,46 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
        double currentTime = (double) System.currentTimeMillis() / 1000;
        switch(pathState){
            case PUSHBOT:
+                spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID);
+                shooter.setFlywheelVelocity(2400);
+                robot.setHoodPos(0.64);
                if (startAuto){
-                    follower.followPath(startPose_pushBot, true);
+                    follower.followPath(startPose_pushBot, false);
                    startAuto = false;
+                    timeStamp = currentTime;
                    shootX = shoot0X;
                    shootY = shoot0Y;
                    shootH = shoot0H;
                }
-                if (!follower.isBusy()){
+                if (!follower.isBusy() || currentTime - timeStamp > pushTime){
                    follower.followPath(pushBot_shoot0, true);
                    pathState = PathState.DRIVE_SHOOT0;
+                    timeStamp = currentTime;
                }
                break;
            case DRIVE_SHOOT0:
-                if (!follower.isBusy()){
+                if (!follower.isBusy()  && currentTime - timeStamp > rapidWaitTime){
                    timeStamp = currentTime;
                    pathState = PathState.WAIT_SHOOT0;
+                    spindexer.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
                }
                break;
            case WAIT_SHOOT0:
-                if (currentTime - timeStamp > shootTime){
+                if (currentTime - timeStamp > rapidShootTime &&
+                        (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
+                                spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
                    follower.followPath(shoot0_pickup1, intakePower, false);
                    pathState = PathState.PICKUP1;
+                    spindexer.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.SORTED);
                }
                break;
            case PICKUP1:
                if (!follower.isBusy()){
-                    follower.followPath(pickup1_openGate, true);
+                    follower.followPath(pickup1_openGate, false);
                    pathState = PathState.OPENGATE;
                    shootX = shoot1X;
                    shootY = shoot1Y;
                    shootH = shoot1H;
-                }
-                break;
-            case DRIVE_OPENGATE:
-                if (!follower.isBusy()){
-                    pathState = PathState.OPENGATE;
                    timeStamp = currentTime;
                }
                break;
@@ -213,10 +222,11 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                if (!follower.isBusy()){
                    pathState = PathState.WAIT_SHOOT1;
                    timeStamp = currentTime;
+                    spindexer.startSortedShoot();
                }
                break;
            case WAIT_SHOOT1:
-                if (currentTime - timeStamp > shootTime){
+                if (currentTime - timeStamp > sortedShootTime){
                    follower.followPath(shoot1_drivePickup2, true);
                    pathState = PathState.DRIVE_PICKUP2;
                }
@@ -240,16 +250,19 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                if (!follower.isBusy()){
                    pathState = PathState.WAIT_SHOOT2;
                    timeStamp = currentTime;
+                    spindexer.startSortedShoot();
                }
                break;
            case WAIT_SHOOT2:
-                if (currentTime - timeStamp > shootTime){
+                if (currentTime - timeStamp > sortedShootTime){
                    follower.followPath(shoot2_drivePickup3, true);
                    pathState = PathState.DRIVE_PICKUP3;
                }
                break;
            case DRIVE_PICKUP3:
                if (!follower.isBusy()){
+                    shooter.setFlywheelVelocity(2300);
+                    robot.setHoodPos(0.68);
                    follower.followPath(drivePickup3_pickup3, intakePower, false);
                    pathState = PathState.PICKUP3;
                }
@@ -266,6 +279,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
            case DRIVE_SHOOT3:
                if (!follower.isBusy()){
                    pathState = PathState.WAIT_SHOOT3;
+                    spindexer.startSortedShoot();
                }
                break;
            case WAIT_SHOOT3:
@@ -296,19 +310,33 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
        }
        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
        follower = Constants.createFollower(hardwareMap);
+        sleep(1000);
        follower.setStartingPose(new Pose(72,72,0));
        loopTimes = new MeasuringLoopTimes();
        loopTimes.init();
+        turret = new Turret(robot);
+        flywheel = new Flywheel(robot);
+        commander = new VelocityCommander();
+        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
+        spindexer = new SpindexerTransferIntake(robot, TELE, commander);
+        ParkTilter park = new ParkTilter(robot);

        boolean initializeRobot = false;
        while (opModeInInit()){
            follower.update();

+            if (gamepad1.squareWasPressed()){
+                robot.setSpinPos(ServoPositions.spindexer_A2);
+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Closed);
+                robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
+            }
+
            if (gamepad1.crossWasPressed() && !initializeRobot){
                Color.redAlliance = !Color.redAlliance;
+                shooter.setRedAlliance(Color.redAlliance);

                double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
-                        pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
+                        pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
                        drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
                        drivePickup3X, pickup3X, shoot3ControlX, shoot3X};

@@ -327,11 +355,25 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                follower.setPose(startPose);
                buildPaths();
                sleep(2000);
+                turret.switchPipeline(Turret.PipelineMode.OBELISK);
+                robot.limelight.start();
+                limelightUsed = true;
+                park.unpark();
+            }
+
+            if (initializeRobot){
+                //add obelisk read here
+                shooter.setState(Shooter.ShooterState.READ_OBELISK);
+                int ID = turret.getObeliskID();
+                spindexer.setDesiredPatternAuto(ID);
+                TELE.addData("ID", ID);
+                shooter.update(robot.voltage.getVoltage());
            }

            TELE.addData("Red Alliance?", Color.redAlliance);
            TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot);
            TELE.addData("Start Pose", follower.getPose());
+            TELE.addData("Current LL Pipeline", turret.pipeline());
            TELE.update();
        }

@@ -339,9 +381,10 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {

        if (isStopRequested()) return;

-        limelightUsed = false;
-
        while (opModeIsActive()){
+            shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            shooter.update(robot.voltage.getVoltage());
+
            follower.update();
            pathStateMachine();
            Pose currentPose = follower.getPose();
@@ -349,6 +392,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
            teleStartPoseY = currentPose.getY();
            teleStartPoseH = Math.toDegrees(currentPose.getHeading());

+            spindexer.update();
+
            for (LynxModule hub : allHubs) {
                hub.clearBulkCache();
            }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
@@ -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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
@Config
 public class ServoPositions {

-    public static double rapidFireBlocker_Closed = 0.35;
+    public static double rapidFireBlocker_Closed = 0.32;
    public static double rapidFireBlocker_Open = 0.5;

    public static double spindexBlocker_Closed = 0.31;
@@ -34,9 +34,9 @@ public class ServoPositions {

    public static double shootAllSpindexerSpeedIncrease = 0.01;

-    public static double transferServo_out = 0.57;
+    public static double transferServo_out = 0.28;

-    public static double transferServo_in = 0.77;
+    public static double transferServo_in = 0.54;

    public static double hoodAuto = 0.27;

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
@@ -5,7 +5,7 @@ import com.pedropathing.control.FilteredPIDFCoefficients;
 import com.pedropathing.control.PIDFCoefficients;
 import com.pedropathing.follower.Follower;
 import com.pedropathing.follower.FollowerConstants;
-import com.pedropathing.ftc.FollowerBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.FollowerBuilder;
 import com.pedropathing.ftc.drivetrains.MecanumConstants;
 import com.pedropathing.ftc.localization.constants.PinpointConstants;
 import com.pedropathing.paths.PathConstraints;
@@ -22,7 +22,7 @@ public class Constants {
            .lateralZeroPowerAcceleration(-60.876)
            .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.015, 0.02))
            .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02))
-            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.00001, 0.6, 0.015))
+            .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.013, 0, 0.00001, 0.6, 0.015))
            .centripetalScaling(0.0005);

    public static MecanumConstants driveConstants = new MecanumConstants()
@@ -51,10 +51,16 @@ public class Constants {
            .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD);

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

 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
 import org.firstinspires.ftc.teamcode.utilsv2.*;

+import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed;
+
@TeleOp
@Config
 public class TeleopV4 extends LinearOpMode {
@@ -28,8 +31,10 @@ public class TeleopV4 extends LinearOpMode {
    Turret turret;
    Flywheel flywheel;
    VelocityCommander commander;
-
    ParkTilter parkTilter;
+    MeasuringLoopTimes loopTimes;
+
+    public static Pose relocalizePose = new Pose(128, 83, 0);

    @Override
    public void runOpMode() throws InterruptedException {
@@ -52,6 +57,10 @@ public class TeleopV4 extends LinearOpMode {
        turret = new Turret(robot);

        parkTilter = new ParkTilter(robot);
+        parkTilter.unpark();
+
+        loopTimes = new MeasuringLoopTimes();
+        loopTimes.init();

        shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander);
        shooter.setState(Shooter.ShooterState.TRACK_GOAL);
@@ -59,6 +68,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 +85,27 @@ public class TeleopV4 extends LinearOpMode {
                    gamepad1.left_stick_x
            );

-            follower.update();
+            if (gamepad1.crossWasPressed()){
+                follower.setPose(relocalizePose);
+                gamepad1.rumble(100);
+            }

+            follower.update();
+            Pose currentPose = follower.getPose();
+
+            if (gamepad1.dpadLeftWasPressed()){
+                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            }
+
+            if (gamepad1.dpadRightWasPressed()){
+                shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+            }
+
+            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
+                shooter.setFlywheelVelocity(2500);
+                robot.setHoodPos(0.6);
+                robot.setTransferPower(-0.8);
+            }

            shooter.update(robot.voltage.getVoltage());
            spindexerTransferIntake.update();
@@ -99,12 +131,15 @@ public class TeleopV4 extends LinearOpMode {
                        SpindexerTransferIntake.RapidMode.HOLD_BALLS
                );
            }
-            if (gamepad1.rightBumperWasPressed()
-                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {

-                spindexerTransferIntake.setRapidMode(
-                        SpindexerTransferIntake.RapidMode.INTAKE
-                );
+            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
+                robot.setIntakePower(1);
+            }
+
+            if (gamepad2.leftBumperWasPressed()){
+                limelightUsed = false;
+            } else if (gamepad2.rightBumperWasPressed()){
+                limelightUsed = true;
            }

            if (gamepad1.dpad_down){
@@ -113,12 +148,21 @@ public class TeleopV4 extends LinearOpMode {
                parkTilter.unpark();
            }

+            loopTimes.loop();
+            TELE.addData("Loop Time Average", loopTimes.getAvgLoopTime());
+            TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
+            TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
+
            TELE.addData("Distance From Goal", commander.getDistance());
            TELE.addData("Hood Position", commander.getHoodPredicted());
            TELE.addData("Transfer Power", robot.transfer.getPower());
            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());

+            TELE.addData("Current Position", currentPose);
+
+            TELE.addData("Current LL Pipeline", turret.pipeline());
+
            TELE.update();
        }

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/MotorDirectionDebugger.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java
+++ b/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
@@ -32,7 +34,7 @@ public class NewShooterTest extends LinearOpMode {

    public static int flywheelVelo = 0;
    public static double hoodPos = 0.5;
-    public static double transferPower = -0.7;
+    public static double transferPower = -0.8;
 //    public static double turretPos = 0.51;

    @Override
@@ -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();

+            if (gamepad1.dpadLeftWasPressed()){
+                shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR);
+            }
+
+            if (gamepad1.dpadRightWasPressed()){
+                shooter.setState(Shooter.ShooterState.TRACK_GOAL);
+            }
+
+            if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){
                shooter.setFlywheelVelocity(flywheelVelo);
                robot.setHoodPos(hoodPos);
+                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()
-                    && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
-
-                spindexerTransferIntake.setRapidMode(
-                        SpindexerTransferIntake.RapidMode.INTAKE
-                );
+            if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) {
+                robot.setIntakePower(1);
            }

            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();
        }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortedSpindexerTest.java
@@ -106,22 +106,6 @@ public class SortedSpindexerTest extends LinearOpMode {
                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) {
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
+++ b/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
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java
@@ -43,13 +43,12 @@ public class MeasuringLoopTimes {

    public void loop() {
        currentTime = getTimeSeconds();
-        if ((MeasurementStart + 5.0) < currentTime)
+        if ((MeasurementStart + 60) < currentTime)
        {
            minLoopTime = 9999999.0;
            maxLoopTime = 0.0;
            MeasurementStart = currentTime;

-            avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker;
            avgLoopTimeSum = 0.0;
            avgLoopTimeTicker = 0;
        }
@@ -59,6 +58,7 @@ public class MeasuringLoopTimes {

        avgLoopTimeSum += mainLoopTime;
        avgLoopTimeTicker++;
+        avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker;
        minLoopTime = Math.min(minLoopTime,mainLoopTime);
        maxLoopTime = Math.max(maxLoopTime,mainLoopTime);
    }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java
+++ b/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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
+++ b/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
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java
+++ b/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
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
+++ b/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 {

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java
+++ b/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;
    }
@@ -136,11 +140,9 @@ public class Shooter {
                break;
            case READ_OBELISK:
                manualFlywheel = false;
-                turr.trackObelisk(
-                        (obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
-                        (obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
-                        follow.getHeading()
-                );
+                robot.setTurretPos(Turret.neutralPosition);
+
+                turr.detectObelisk();

                commander.getVeloPredictive(
                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
@@ -154,7 +156,7 @@ public class Shooter {

                flywheelVelocity = commander.getPredictedRPM();

-                fly.manageFlywheel(flywheelVelocity);
+                fly.manageFlywheel(0);
                fly.setF(voltage);
                break;

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
@@ -216,6 +216,16 @@ public class SpindexerTransferIntake {
        desiredPattern = pattern;
    }

+    public void setDesiredPatternAuto(int id) {
+        if (id == 22){
+            desiredPattern = DesiredPattern.PGP;
+        } else if (id == 23){
+            desiredPattern = DesiredPattern.PPG;
+        } else {
+            desiredPattern = DesiredPattern.GPP;
+        }
+    }
+
    public void startSortedShoot() {

        shootOrder = buildShootOrder(
@@ -261,6 +271,8 @@ public class SpindexerTransferIntake {
        return System.currentTimeMillis() - sortedStateStartTime;
    }

+    private int greenTicks = 0;
+    private int ballTicks = 0;
    public void update() {

        TELE.addData("Sorted State", sortedIntakeStates);
@@ -402,6 +414,8 @@ public class SpindexerTransferIntake {

            case SORTED:

+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+
                switch (sortedIntakeStates) {
                    case NOTHING:
                        break;
@@ -422,24 +436,31 @@ public class SpindexerTransferIntake {
                                ServoPositions.transferServo_out
                        );
                        robot.setIntakePower(1);
-                        robot.setTransferPower(-1);
+                        robot.setTransferPower(-0.7);
                        if (sortedStateTime() > 200) {
                            setSortedIntakeMode(SortedIntakeStates.INTAKE_1);
                        }
                        break;
                    case INTAKE_1:
                        robot.setIntakePower(1);
-                        robot.setTransferPower(-1);
+                        robot.setTransferPower(-0.7);
                        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) {
+                                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:
@@ -454,12 +475,20 @@ public class SpindexerTransferIntake {
                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
                            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
                            if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
+                                greenTicks++;
+                            }
+                            ballTicks++;
+                            if (ballTicks > 15) {
+                                if (greenTicks > 10){
                                    ballColors[1] = BallStates.GREEN;
                                } else {
                                    ballColors[1] = BallStates.PURPLE;
                                }
                                robot.setSpinPos(ServoPositions.spindexer_A3);
                                setSortedIntakeMode(SortedIntakeStates.DELAY_2);
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
                        }
                        break;
                    case DELAY_2:
@@ -481,24 +510,37 @@ public class SpindexerTransferIntake {
                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
                            NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
                            if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
+                                greenTicks++;
+                            }
+                            ballTicks++;
+                            if (ballTicks > 15) {
+                                if (greenTicks > 10){
                                    ballColors[2] = BallStates.GREEN;
                                } else {
                                    ballColors[2] = BallStates.PURPLE;
                                }
                                setSortedIntakeMode(SortedIntakeStates.REVERSE);
-
+                                ballTicks = 0;
+                                greenTicks = 0;
+                            }
                        }
                        break;
                    case REVERSE:
                        robot.setTransferPower(-0.3);
                        robot.setIntakePower(-0.1);
+                        break;
                }


                break;
            case SHOOT_SORTED:

+                robot.setRapidFireBlockerPos(ServoPositions.rapidFireBlocker_Open);
+                if (Shooter.manualFlywheel) {
+                    robot.setTransferPower(NewShooterTest.transferPower);
+                } else {
                    robot.setTransferPower(commander.getTransferPow());
+                }


                switch (shootState) {
@@ -670,7 +712,7 @@ public class SpindexerTransferIntake {
                                ServoPositions.transferServo_out
                        );
                        robot.setIntakePower(1);
-                        robot.setTransferPower(-1);
+                        robot.setTransferPower(-0.7);

                        if (shootStateTime() > 250) {

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
@@ -1,11 +1,14 @@
 package org.firstinspires.ftc.teamcode.utilsv2;

 import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDController;
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.robotcore.util.Range;


+import org.firstinspires.ftc.teamcode.constants.Color;
+
 import java.util.List;

@Config
@@ -13,9 +16,22 @@ public class Turret {
    Robot robot;

    private final double servoTicksPer180 = 0.58;
-    private final double neutralPosition = 0.51;
+    public static double neutralPosition = 0.51;
    private final double turretMin = 0.05;
    private final double turretMax = 0.95;
+    public static boolean limelightUsed = true;
+    public static double B_PID_P = 0.0003, B_PID_I = 0.0, B_PID_D = 0.00003;
+    LLResult result;
+    PIDController bearingPID;
+    boolean bearingAligned = false;
+    public int LL_COAST_TICKS = 5;
+    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 +44,7 @@ public class Turret {

    public Turret(Robot rob) {
        this.robot = rob;
+        bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D);
    }

    private double wrapAngle(double angle) {
@@ -36,6 +53,43 @@ public class Turret {
        return angle;
    }

+    private void limelightRead() { // only for tracking purposes, not general reads
+        switchPipeline(PipelineMode.TRACKING);
+        result = robot.limelight.getLatestResult();
+        tx = 1000;
+        if (result != null) {
+            if (result.isValid()) {
+                tx = result.getTx();
+            }
+        }
+    }
+
+    public double getTX(){return tx;}
+
+    public enum PipelineMode{
+        OBELISK,
+        TRACKING
+    }
+
+    private int prevPipeline = 0;
+    public void switchPipeline(PipelineMode pipelineMode){
+        int pipeline = 0;
+        if (pipelineMode == PipelineMode.OBELISK){
+            pipeline = 1;
+        } else if (pipelineMode == PipelineMode.TRACKING){
+            if (Color.redAlliance){
+                pipeline = 4;
+            } else {
+                pipeline = 2;
+            }
+        }
+        if (pipeline != prevPipeline){
+            robot.limelight.pipelineSwitch(pipeline);
+        }
+        prevPipeline = pipeline;
+    }
+    public int pipeline(){return prevPipeline;}
+
    public void trackObelisk(double dx, double dy, double h) {

        double heading = wrapAngle(h);
@@ -54,7 +108,6 @@ public class Turret {

        robot.setTurretPos(servoAngle);

-
        detectObelisk();

    }
@@ -63,32 +116,26 @@ public class Turret {
        return obeliskID;
    }

-    private int detectObelisk() {
-        robot.limelight.pipelineSwitch(1);
-        LLResult result = robot.limelight.getLatestResult();
+    public void detectObelisk() {
+        result = robot.limelight.getLatestResult();
        if (result != null && result.isValid()) {
            List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
-            double prevTx  = -1000;
            for (LLResultTypes.FiducialResult fiducial : fiducials) {
-                double currentTx = fiducial.getTargetXDegrees();
-                if (currentTx > prevTx){
                obeliskID = fiducial.getFiducialId();
            }
        }
    }
-        return obeliskID;
-    }

    public void manual (double pos) {
        robot.setTurretPos(pos);
-
    }
-    // 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 +145,34 @@ public class Turret {
        double fieldRelativeHeading = Math.atan2(predictedDy, predictedDx);

        double angleDelta = fieldRelativeHeading - predictedH;
-        angleDelta = wrapAngle(angleDelta);
+        angleDelta = wrapAngle(angleDelta) / (2.0 * Math.PI);

-        double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
+        double bearingOffset = 0;
+        if (limelightUsed && servoAngle > turretMin && servoAngle < turretMax){
+            limelightRead();
+            if (result.isValid() && tx < 100){
+                targetTx = (tx*alphaTX)+(targetTx*(1-alphaTX));
+                bearingAligned = Math.abs(targetTx) < TARGET_POSITION_TOLERANCE;
+                if (!bearingAligned){
+                    bearingOffset = (bearingPID.calculate(targetTx, 0.0));
+                }
+            } else {
+                targetTx = 0;
+                bearingOffset = 0;
+            }
+            currentTrackOffset += bearingOffset;
+            llCoast = LL_COAST_TICKS;
+        } else {
+            if (llCoast <= 0){
+                currentTrackOffset = 0;
+            } else {
+                llCoast--;
+            }
+        }

-        double servoAngle = neutralPosition + servoTicksFromNeutral;
+        double servoTicksFromNeutral = (angleDelta+currentTrackOffset) * (2.0 * servoTicksPer180);
+
+        servoAngle = neutralPosition + servoTicksFromNeutral;

        servoAngle = Range.clip(servoAngle, turretMin, turretMax);
Author	SHA1	Message	Date
DanTheMan-byte	c36cac12f2	sorting auto is done	2026-06-05 18:25:12 -05:00
DanTheMan-byte	f7a9f6aaf5	small tweaks: added current pose telemetry and reduced coasting from limelight track	2026-06-05 14:29:21 -05:00
DanTheMan-byte	cc38b98d6f	small tweaks: added loop times measurement	2026-06-05 14:19:39 -05:00
DanTheMan-byte	755d74a829	small tweaks: added loop times measurement and simplified SortedSpindexerTest	2026-06-05 14:18:47 -05:00
DanTheMan-byte	9d29e0b56c	tweaks were made and hardware changes are needed	2026-06-04 22:13:49 -05:00
DanTheMan-byte	e25b372eca	added limelight	2026-06-04 20:57:19 -05:00
DanTheMan-byte	3ab905af0c	added limelight	2026-06-04 20:38:53 -05:00
DanTheMan-byte	58c11f5241	removed bunch of things to remove dash cluter	2026-06-04 20:38:45 -05:00
DanTheMan-byte	3afab333ef	added delay based on ticks	2026-06-04 19:51:57 -05:00