a decent working version

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware/hwMap.java

@@ -35,12 +35,10 @@ public class hwMap {
     }
 
     public static class LedHwMap {
-        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led1;
+        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led;
-        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led2;
 
         public LedHwMap(HardwareMap hardwareMap) {
-            /*led1 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led1");
+            //led = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, Constants.LEDConstants.LED);
-            led2 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led2");*/
         }
     }
 
@@ -89,7 +87,7 @@ public class hwMap {
             imu = hardwareMap.get(IMU.class, "imu");
             IMU.Parameters parameters = new IMU.Parameters(
                     new RevHubOrientationOnRobot(
-                            RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
+                            RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
                             RevHubOrientationOnRobot.UsbFacingDirection.UP
                     )
             );
@@ -250,7 +248,7 @@ public class hwMap {
         private double lastKnownDistance = 0.0;
         private ElapsedTime noTargetTimer = new ElapsedTime();
 
-        PIDFCoefficients pidfCoefficients = new PIDFCoefficients(22.5, 0, 0.001, 15.6);
+        PIDFCoefficients pidfCoefficients = new PIDFCoefficients(30, 0, 10, 15);
 
         public TurretHwMap(HardwareMap hardwareMap) {
             noTargetTimer.reset();
@@ -283,12 +281,31 @@ public class hwMap {
             initLimelight(hardwareMap);
         }
 
-        public void setPIDF(double p, double i, double d, double f) {
+        public void resetTurret() {
-            PIDFCoefficients pidf = new PIDFCoefficients(p, i, d, f);
+            turretrotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-            turretLeftMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
+            turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-            turretRightMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
         }
 
+        public void setPIDF(double p, double i, double d, double f) {
+            PIDFCoefficients PIDF = new PIDFCoefficients(p, i, d, f);
+            turretLeftMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, PIDF);
+            turretRightMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, PIDF);
+        }
+
+
+        public void enableTurretHold() {
+            turretrotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        }
+
+        public void disableTurretHold() {
+            turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        }
+
+        public void runToPos(int target) {
+            turretrotation.setTargetPosition(target);
+        }
+
+
 
         public double getTurretRotationPosition() {
             return turretrotation.getCurrentPosition();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java

@@ -517,6 +517,8 @@ public class Auton {
             hwMap.TransferHwMap transferHw = new hwMap.TransferHwMap(hardwareMap);
             hwMap.TurretHwMap turretHw = new hwMap.TurretHwMap(hardwareMap);
 
+            turretHw.setPIDF(120, 0, 10, 15);
+
             systems = new pedroStateMachine(intakeHw, transferHw, turretHw);
             systems.setAlliance(allianceTarget);
 
@@ -531,13 +533,16 @@ public class Auton {
                 telemetry.update();
             }
 
+            follower.update();
+
+            turretHw.resetTurret();
+
             waitForStart();
             systems.update(allianceTarget, follower.getPose());
             opmodeTimer.resetTimer();
             setPathState(PathState.DRIVE_TO_SCORE_PRELOAD);
 
             while (opModeIsActive()) {
-
                 for (LynxModule hub : allHubs) {
                     hub.clearBulkCache();
                 }

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

@@ -25,7 +25,7 @@ public class Constants {
             .lateralZeroPowerAcceleration(-76.57271150137258);
 
     public static MecanumConstants driveConstants = new MecanumConstants()
-            .maxPower(0.8)
+            .maxPower(0.7)
             .xVelocity(68.10320673181904)
             .yVelocity(57.52038399624941)
             .rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR)
@@ -41,15 +41,15 @@ public class Constants {
 
 
     public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants()
-            .forwardTicksToInches(0.001978956)
+            .forwardTicksToInches(0.0020041908950982315)
-            .strafeTicksToInches(0.001978956)
+            .strafeTicksToInches(0.002004094712407555)
             .turnTicksToInches(0.0022824233082645137)
             .leftPodY(3.75)
             .rightPodY(-3.25)
             .strafePodX(-6.25)
             .leftEncoder_HardwareMapName("intake")
             .rightEncoder_HardwareMapName("fl")
-            .strafeEncoder_HardwareMapName("bl")
+            .strafeEncoder_HardwareMapName("fr")
             .leftEncoderDirection(Encoder.FORWARD)
             .rightEncoderDirection(Encoder.FORWARD)
             .strafeEncoderDirection(Encoder.FORWARD);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java

@@ -1,76 +1,56 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
+import org.firstinspires.ftc.robotcore.external.Const;
 import org.firstinspires.ftc.teamcode.Hware.hwMap;
 import org.firstinspires.ftc.teamcode.Prism.Color;
 import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
 import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
 
 public class LED {
     private final hwMap.LedHwMap hardware;
 
     // Track states to prevent spamming the I2C bus
-    private int currentLed1State = -1;
+    private int currentLedState = -1;
-    private int currentLed2State = -1;
 
     // Reusable animations for efficiency
     private final PrismAnimations.Solid solidRed = new PrismAnimations.Solid(Color.RED);
     private final PrismAnimations.Solid solidOrange = new PrismAnimations.Solid(Color.ORANGE);
+    private final PrismAnimations.Solid solidYellow = new PrismAnimations.Solid(Color.YELLOW);
     private final PrismAnimations.Solid solidGreen = new PrismAnimations.Solid(Color.GREEN);
-    private final PrismAnimations.Solid solidBlue = new PrismAnimations.Solid(Color.BLUE);
-    private final PrismAnimations.Solid solidPurple = new PrismAnimations.Solid(Color.PURPLE); // or MAGENTA if PURPLE is missing
 
     public LED(hwMap.LedHwMap hardware) {
         this.hardware = hardware;
 
-        solidRed.setBrightness(50);
+        int LED_BRIGHTNESS = Constants.LEDConstants.LAYER_BRIGHTNESS;
-        solidOrange.setBrightness(50);
+        solidRed.setBrightness(LED_BRIGHTNESS);
-        solidGreen.setBrightness(50);
+        solidOrange.setBrightness(LED_BRIGHTNESS);
-        solidBlue.setBrightness(50);
+        solidYellow.setBrightness(LED_BRIGHTNESS);
-        solidPurple.setBrightness(50);
+        solidGreen.setBrightness(LED_BRIGHTNESS);
     }
 
-    public void update(boolean isScoring, boolean isTurretReady, boolean hasTarget, boolean isAnchorActive) {
+    public void update(boolean isTurretLocked, boolean isFlywheelUpToSpeed) {
-    /*
+        /*int newLedState = 0;
-        int newLed1State = 0;
+
-        if (isScoring) {
+        if (isTurretLocked && isFlywheelUpToSpeed) {
-            if (isTurretReady) {
+            newLedState = 1;
-                newLed1State = 2;
+        } else if (isTurretLocked) {
-            } else {
+            newLedState = 2;
-                newLed1State = 1;
+        } else if (isFlywheelUpToSpeed) {
-            }
+            newLedState = 3;
         }
 
-        if (newLed1State != currentLed1State) {
+        if (newLedState != currentLedState) {
-            if (newLed1State == 0) {
+            if (newLedState == 0) {
-                hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
+                hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
-            } else if (newLed1State == 1) {
+            } else if (newLedState == 1) {
-                hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
+                hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
-            } else if (newLed1State == 2) {
+            } else if (newLedState == 2) {
-                hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
+                hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
+            } else if (newLedState == 3) {
+                hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidYellow);
             }
-            currentLed1State = newLed1State;
+            currentLedState = newLedState;
-        }
-
-        int newLed2State = 0;
-        if (hasTarget && isAnchorActive) {
-            newLed2State = 3;
-        } else if (hasTarget) {
-            newLed2State = 1;
-        } else if (isAnchorActive) {
-            newLed2State = 2;
-        }
-
-        if (newLed2State != currentLed2State) {
-            if (newLed2State == 0) {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
-            } else if (newLed2State == 1) {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
-            } else if (newLed2State == 2) {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidBlue);
-            } else if (newLed2State == 3) {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidPurple);
-            }
-            currentLed2State = newLed2State;
         }*/
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystems;
 
 import com.qualcomm.robotcore.util.ElapsedTime;
 import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
 
 public class TransferSys {
     private final hwMap.TransferHwMap hardware;
@@ -9,7 +10,7 @@ public class TransferSys {
     private boolean isTurretReady = false;
     private boolean limiterClosed = true;
 
-    public double launchDuration = 2; // CHANGEABLE TODO
+    public double launchDuration = Constants.TransferConstants.launch_duration; // CHANGEABLE TODO
 
     public enum TransferState {
         LAUNCHING, // Gate OPEN
@@ -94,6 +95,13 @@ public class TransferSys {
         return limiterClosed;
     }
 
+    public double getLaunchElapsedTime() {
+        if (currentState == TransferState.LAUNCHING && isLaunchSequenceActive) {
+            return timer.seconds();
+        }
+        return 100.0;
+    }
+
     public TransferState getTransferState() {
         return currentState;
     }

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

@@ -1,8 +1,5 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
-import static org.firstinspires.ftc.teamcode.tuning.allInOne.OFFSET_GAIN;
-import static org.firstinspires.ftc.teamcode.tuning.allInOne.SERVO_MIN;
-
 import com.pedropathing.geometry.Pose;
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.robotcore.util.ElapsedTime;
@@ -33,7 +30,14 @@ public class Turret {
 
     private Pose robotPose;
 
+    public double targetTick;
+
     private final ElapsedTime loopTimer = new ElapsedTime();
+    private final ElapsedTime stateTimer = new ElapsedTime();
+
+    public boolean turretModeHold = false;
+
+    public boolean lastTurretModeHold = false;
 
     public enum TurretState {
         LAUNCH,
@@ -45,11 +49,16 @@ public class Turret {
 
     public Turret(hwMap.TurretHwMap hardware) {
         this.hardware = hardware;
+        offsetTicks = 0; // Reset manual offset on every init
+        targetCorrectionOffsetTicks = 0; // Reset vision correction
         loopTimer.reset();
+        stateTimer.reset();
     }
 
     public void setAlliance(int alliance) {
+        if (this.alliance != alliance) {
             this.alliance = alliance;
+            targetCorrectionOffsetTicks = 0; // Reset correction when alliance changes
             if (alliance == 2) {
                 hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
             } else if (alliance == 1) {
@@ -58,6 +67,7 @@ public class Turret {
                 hardware.setPipeline(0);
             }
         }
+    }
 
     public void stop() {
         if (currentState != TurretState.IDLE) {
@@ -68,7 +78,9 @@ public class Turret {
     }
 
     public void setTurretState(TurretState state) {
+        if (this.currentState != state) {
             this.currentState = state;
+            stateTimer.reset();
 
             switch (state) {
                 case IDLE:
@@ -85,6 +97,7 @@ public class Turret {
                     break;
             }
         }
+    }
 
 
     public TurretState getTurretState() {
@@ -95,7 +108,7 @@ public class Turret {
         return hardware.hasTarget();
     }
 
-    public void update(boolean manualTracking, boolean manualSAM, Pose currentPose) {
+    public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, double elapsedTime) {
         double dt = loopTimer.seconds();
         loopTimer.reset();
         if (dt < 0.001) dt = 0.001;
@@ -115,8 +128,18 @@ public class Turret {
         }
 
         if (currentState == TurretState.LAUNCH) {
-            handleLaunchLogic();
+            handleLaunchLogic(elapsedTime);
         }
+        else {
+           // idleLaunch();
+        }
+    }
+
+    public void idleLaunch() {
+        hardware.setTurretVelocity(1000);
+
+        double calculatedHoodPos = getHoodPOS(65);
+        setHoodPos(calculatedHoodPos);
     }
 
     public void updateAuton(int alliance, Pose currentPose, double offset) {
@@ -127,6 +150,11 @@ public class Turret {
 
         setAlliance(alliance);
 
+        if (currentState == TurretState.IDLE) {
+            hardware.stopTurretRotation();
+            return;
+        }
+
         double rawTicks = hardware.getTurretRotationPosition();
         double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
 
@@ -134,38 +162,63 @@ public class Turret {
 
         calculateHybridTarget(currentPose, currentDegrees, rawTicks);
         runPIDControl(rawTicks, dt);
-        handleLaunchLogic(-offset);
+        handleLaunchLogicOffset(-offset);
     }
 
-    private void handleLaunchLogic() {
+    private void handleLaunchLogic(double elapsedTime) {
         if (distance > 0) {
-            targetVelocity = getFlywheelVelocity(distance);
+            double baseVelocity = getFlywheelVelocity(distance);
+            double t = stateTimer.seconds();
+
+            /* TUNING GUIDE:
+             * 1. Adjust ARTIFACT_1_WINDOW and ARTIFACT_2_WINDOW in Constants.java to match your launch sequence timing.
+             * 2. Adjust ARTIFACT_OFFSET_LOW to lower the velocity for the 1st and 3rd artifacts.
+             * 3. ARTIFACT_OFFSET_NORMAL (usually 0) is for the 2nd artifact.
+             */
+            double compensation;
+            if (t < Constants.TurretConstants.ARTIFACT_1_WINDOW) {
+                compensation = Constants.TurretConstants.ARTIFACT_1;
+            } else if (t < Constants.TurretConstants.ARTIFACT_2_WINDOW) {
+                compensation = Constants.TurretConstants.ARTIFACT_2;
+            } else {
+                compensation = Constants.TurretConstants.ARTIFACT_3;
+            }
+
+            targetVelocity = baseVelocity + compensation;
             hardware.setTurretVelocity(targetVelocity);
 
             double calculatedHoodPos = getHoodPOS(distance);
-            double currentVel = hardware.getFlywheelVelocities()[0];
+            double currentVel = hardware.getFlywheelVelocities()[1];
-            double dropThreshold = targetVelocity * 0.95;
-            double recoilOffset = 0.0;
 
-            recoilOffset = (currentVel - targetVelocity) * 0.000004;
+            double recoilOffset = (currentVel - targetVelocity) * -0.000185;
 
-            setHoodPos(calculatedHoodPos + recoilOffset);
+            setHoodPos(calculatedHoodPos + recoilOffset - 0.03);
         } else {
             hardware.setTurretVelocity(0);
         }
     }
 
-    private void handleLaunchLogic(double offset) {
+    private void handleLaunchLogicOffset(double offset) {
         if (distance > 0) {
-            targetVelocity = getFlywheelVelocity(distance);
+            double baseVelocity = getFlywheelVelocity(distance);
+            double t = stateTimer.seconds();
+
+            double compensation;
+            if (t < Constants.TurretConstants.ARTIFACT_1_WINDOW) {
+                compensation = Constants.TurretConstants.ARTIFACT_1;
+            } else if (t < Constants.TurretConstants.ARTIFACT_2_WINDOW) {
+                compensation = Constants.TurretConstants.ARTIFACT_2;
+            } else {
+                compensation = Constants.TurretConstants.ARTIFACT_3;
+            }
+
+            targetVelocity = baseVelocity + compensation;
             hardware.setTurretVelocity(targetVelocity);
 
             double calculatedHoodPos = getHoodPOS(distance);
-            double currentVel = hardware.getFlywheelVelocities()[0];
+            double currentVel = hardware.getFlywheelVelocities()[1];
-            double dropThreshold = targetVelocity * 0.95;
-            double recoilOffset = 0.0;
 
-            recoilOffset = (currentVel - targetVelocity) * 0.000004;
+            double recoilOffset = (currentVel - targetVelocity) * -0.000185;
 
             setHoodPos(calculatedHoodPos + recoilOffset);
         } else {
@@ -178,13 +231,36 @@ public class Turret {
         LLResult result = hardware.getLimelightResult();
 
         if (result != null && result.isValid()) {
-            double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
+            // 1. Calculate Vision Truth: Where the goal sits based on the camera right now
-            targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - Constants.TurretConstants.OFFSET_SMOOTHING))
+            double tx = result.getTx();
-                    + (rawErrorTicks * Constants.TurretConstants.OFFSET_SMOOTHING);
+            double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER);
+            double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES;
+            double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE;
+
+            // 2. Calculate Current Fused Target: Where we currently think the goal is (Odo + existing Offset)
+            double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+
+            double correctionError = visionTargetTicks - currentFusedTarget;
+
+            // 4. Normalize the error for wrapping (360 degrees)
+            double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE;
+            while (correctionError > (ticksPerRev / 2))  correctionError -= ticksPerRev;
+            while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev;
+
+            // 5. Apply the error to the persistent offset (Bias-Shift)
+            targetCorrectionOffsetTicks += (correctionError * Constants.TurretConstants.OFFSET_SMOOTHING);
         }
 
         double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+
+        double fullWrapTicks = Constants.TurretConstants.TURRET_FULL_WRAP_TICKS;
+        double halfWrapTicks = fullWrapTicks / 2.0;
+
+        if (rawTarget > halfWrapTicks) {rawTarget -= fullWrapTicks;}
+        else if (rawTarget < -halfWrapTicks) {rawTarget += fullWrapTicks;}
+
         fusedTargetTicks = Range.clip(rawTarget, Constants.TurretConstants.SOFT_LIMIT_NEG, Constants.TurretConstants.SOFT_LIMIT_POS);
+        targetTick = fusedTargetTicks;
     }
 
     private double calculateOdometryTargetTicks(Pose robotPose) {
@@ -208,25 +284,34 @@ public class Turret {
         return (relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE;
     }
 
-    private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
-        double tx = result.getTx();
-        double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER);
-        double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES;
-        double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE;
-
-        double rawErrorTicks = visionTargetTicks - odoTargetTicks;
-        double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE;
-
-        while (rawErrorTicks > (ticksPerRev / 2))  rawErrorTicks -= ticksPerRev;
-        while (rawErrorTicks < -(ticksPerRev / 2)) rawErrorTicks += ticksPerRev;
-        return rawErrorTicks;
-    }
-
     private void runPIDControl(double currentTicks, double dt) {
         fusedTargetTicks = fusedTargetTicks + offsetTicks;
         double error = fusedTargetTicks - currentTicks;
         double derivative = (error - lastError) / dt;
 
+        // Hysteresis thresholds for mode switching
+        double entryThreshold = 15;  // Enter hold mode if error is within this range
+        double exitThreshold = 30;   // Exit hold mode if error exceeds this range
+
+        if (!turretModeHold && Math.abs(error) <= entryThreshold) {
+            turretModeHold = true;
+        } else if (turretModeHold && Math.abs(error) > exitThreshold) {
+            turretModeHold = false;
+        }
+
+        if (turretModeHold) {
+            if (lastTurretModeHold != turretModeHold) {
+                hardware.runToPos((int) fusedTargetTicks); // Set target first
+                hardware.enableTurretHold();               // Then set mode
+                hardware.setTurretRotationPower(Constants.TurretConstants.TURRET_HOLD_POWER); // Set a holding power
+            }
+            hardware.runToPos((int) fusedTargetTicks); // Keep target updated even in hold mode
+        }
+        else {
+            if (lastTurretModeHold != turretModeHold) {
+                hardware.disableTurretHold();
+            }
+
             if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) {
                 integralSum += (error * dt);
             } else {
@@ -239,11 +324,13 @@ public class Turret {
 
             hardware.setTurretRotationPower(output);
             turretPowerRotation = output;
+        }
+        lastTurretModeHold = turretModeHold;
         lastError = error;
     }
 
     public boolean hasReachedVelocity() {
-        double currentVel = hardware.getFlywheelVelocities()[0];
+        double currentVel = hardware.getFlywheelVelocities()[1];
 
         if (Math.abs(targetVelocity) > 1000) {
             double absCurrent = Math.abs(currentVel);
@@ -258,7 +345,7 @@ public class Turret {
     }
 
     public boolean isTurretReady() {
-        return hasReachedVelocity();
+        return hasReachedVelocity() && isTurretRotationLocked();
     }
 
 
@@ -288,11 +375,11 @@ public class Turret {
     }
 
     private double getFlywheelVelocity(double dist) {
-        return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
+        return (0.0000433916 * Math.pow((dist), 4) - 0.0090065 * Math.pow(dist, 3) + 0.588434 * Math.pow(dist, 2) + (-7.67701 * dist) + 1485.89126);
     }
 
     private double getHoodPOS(double dist) {
-        return (((1.2113 * Math.pow(10, -8)) * Math.pow(dist, 4)) - (0.00000498646 * Math.pow(dist, 3)) + (0.00068726 * Math.pow(dist, 2)) + (-0.0405783 * dist) + 1.07118);
+        return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859);
     }
 
     public void setHoodPos(double pos) {
@@ -311,7 +398,20 @@ public class Turret {
         return turretPower;
     }
 
+    public double returnFlywheelSpeed() {
+        return hardware.getFlywheelVelocities()[1];
+    }
+
     public double getTargetVelocity() {
         return targetVelocity;
     }
+
+
+        public void resetTurret() {
+            hardware.resetTurret();
+        }
+
+        public boolean isTurretRotationLocked() {
+            return turretModeHold && Math.abs(lastError) <= 10;
+        }
     }

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

@@ -5,7 +5,8 @@ import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.Servo;
-@Configurable
+
+
 public class Constants {
 
     public static class FieldConstants {
@@ -37,6 +38,13 @@ public class Constants {
         public static final double TURBO_SPEED_MULTIPLIER = 1.0;
     }
 
+    public static class LEDConstants {
+        public static final String LED = "prism";
+        public static final int LAYER_BRIGHTNESS = 50;
+
+    }
+
+
     public static class IntakeConstants {
         public static final String FRONT_INTAKE_MOTOR = "intake";
 
@@ -68,6 +76,9 @@ public class Constants {
         public static final double POWER_HOLDING = 0.1;
     }
 
+
+    @Configurable
+
     public static class TransferConstants {
         public static final String LIMITER_SERVO = "limiter";
 
@@ -82,34 +93,37 @@ public class Constants {
 
         public static final double UNLOCK_POS = 0.82;
 
+        public static double launch_duration = 0.77;
+
     }
 
+    @Configurable
     public static class TurretConstants {
-        public static final double TICKS_PER_REV_MOTOR = 384.5;
+        public static double TICKS_PER_REV_MOTOR = 145.1;
-        public static final double EXTERNAL_GEAR_RATIO = 2.68888889;
+        public static double EXTERNAL_GEAR_RATIO = 9.179875;
         public static final double TICKS_PER_DEGREE = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
 
-        public static final double KP = 0.02;
+        public static double KP = 0.0048;
-        public static final double KI = 0.0;
+        public static double KI = 0.00801;
-        public static final double KD = 0.001;
+        public static double KD = 0.00083;
-        public static final double MAX_POWER = 0.6;
+        public static final double MAX_POWER = 0.8;
         public static final double MAX_INTEGRAL = 0.5;
 
-        public static final double GOAL_RED_X = 138;
+        public static double GOAL_RED_X = 140;
-        public static final double GOAL_RED_Y = 138;
+        public static double GOAL_RED_Y = 140;
-        public static final double GOAL_BLUE_X = 6;
+        public static double GOAL_BLUE_X = 4;
-        public static final double GOAL_BLUE_Y = 138;
+        public static double GOAL_BLUE_Y = 140;
 
-        public static final double RED_TARGET_OFFSET_DEGREES = 14;
+        public static double RED_TARGET_OFFSET_DEGREES = 0;
-        public static final double BLUE_TARGET_OFFSET_DEGREES = 17;
+        public static double BLUE_TARGET_OFFSET_DEGREES = 0;
-        public static final double LL_TARGET_OFFSET_DEGREES = -2;
+        public static double LL_TARGET_OFFSET_DEGREES = 0;
-        public static final double LL_LOGIC_MULTIPLIER = 1.0;
+        public static double LL_LOGIC_MULTIPLIER = -1.0;
-        public static final double OFFSET_SMOOTHING = 0.2;
+        public static double OFFSET_SMOOTHING = 0.2;
 
         public static final DcMotorSimple.Direction TURRET_ROTATION_DIR = DcMotorSimple.Direction.FORWARD;
 
-        public static final double SOFT_LIMIT_NEG = -230;
+        public static double SOFT_LIMIT_NEG = -324;
-        public static final double SOFT_LIMIT_POS = 230;
+        public static double SOFT_LIMIT_POS = 195;
 
         public static final double HOOD_POS_CLOSE = 0.3;
         public static final double HOOD_POS_FAR = 0.7;
@@ -122,6 +136,7 @@ public class Constants {
         public static final double TURRET_POWER_MID = -0.84;
         public static final double TURRET_POWER_MAX = -1;
         public static final double TURRET_POWER_LOW = -0.7;
+        public static final double TURRET_HOLD_POWER = 0.75;
 
         public static final double EXTAKE_POWER = 0.3;
         public static final double INTAKE_POWER = -0.04;
@@ -140,12 +155,25 @@ public class Constants {
         public static final int LIMELIGHT_PIPELINE_BLUE_TARGET = 1;
         public static final int LIMELIGHT_PIPELINE_RED_TARGET = 2;
 
-        public static final double CAMERA_HEIGHT_INCHES = 12.4;
+        public static double CAMERA_HEIGHT_INCHES = 12.75;
-        public static final double GOAL_HEIGHT_INCHES = 29.5;
+        public static double GOAL_HEIGHT_INCHES = 29.5;
-        public static final double CAMERA_MOUNT_ANGLE_DEGREES = 7.9;
+        public static double CAMERA_MOUNT_ANGLE_DEGREES = 19.8;
 
         public static final double SERVO_MAX = 0.5;
         public static final double SERVO_MIN = 0;
+        public static double LL_POSE_HEALING_SMOOTHING = 0.05;
+        public static double TURRET_FULL_WRAP_TICKS = 1340.0;
+
+        // Artifact Compensation Tuning
+        // Lower (negative offset) for 1st and 3rd, Normal (0 offset) for 2
+        public static double ARTIFACT_1 = -95.0;
+        public static double ARTIFACT_2 = 20.0;
+
+        public static double ARTIFACT_3 = -20.0;
+
+        // Timing Windows (seconds)
+        public static double ARTIFACT_1_WINDOW = 0.35;
+        public static double ARTIFACT_2_WINDOW = 0.55;
     }
 
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java

@@ -14,6 +14,8 @@ import org.firstinspires.ftc.teamcode.util.AutoTransfer;
 import org.firstinspires.ftc.teamcode.util.FPSCounter;
 import com.qualcomm.hardware.lynx.LynxModule;
 import java.util.List;
+
+import com.qualcomm.robotcore.hardware.VoltageSensor;
 public class SOLOTeleOP {
 
     @TeleOp(name = "Red SOLO", group = "FINAL")
@@ -35,14 +37,17 @@ public class SOLOTeleOP {
         private Follower follower;
         private int currentAlliance;
         private final int initialAlliance;
-        private boolean isScoringActive = false;
         protected List<LynxModule> allHubs;
 
+
         private final Pose DEFAULT_RED_START = new Pose(129.25, 112, Math.toRadians(90));
         private final Pose DEFAULT_BLUE_START = new Pose(14.75, 112, Math.toRadians(90));
 
         protected FPSCounter fpsCounter = new FPSCounter();
 
+        private boolean isScoringActive = false;
+
+
         public BaseSoloTeleOp(int alliance) {
             this.initialAlliance = alliance;
             this.currentAlliance = 0;
@@ -63,6 +68,7 @@ public class SOLOTeleOP {
                 currentAlliance = initialAlliance;
                 if (currentAlliance == 2) follower.setStartingPose(DEFAULT_RED_START);
                 else follower.setStartingPose(DEFAULT_BLUE_START);
+                stateMachine.getTurret().resetTurret();
                 telemetry.addLine("MANUAL START - NO AUTON DATA");
             }
 
@@ -91,16 +97,12 @@ public class SOLOTeleOP {
                 follower.update();
                 Pose currentPose = follower.getPose();
 
-                //Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
+                stateMachine.update(false, false, currentPose);
-                //if (healingPose != null) {
+
-                   // follower.setPose(healingPose);
-                //}
 
                 handleGlobalLogic();
                 handleDriverControls();
 
-                stateMachine.update(false, false, currentPose);
-
 
                 updateTelemetry(currentPose);
             }
@@ -159,9 +161,8 @@ public class SOLOTeleOP {
             }
 
             if (stateMachine.getCurrentGameState() != GameState.SCORING) {
-                if (!stateMachine.getTurret().hasReachedVelocity()) {
+                if (!stateMachine.getTurret().hasReachedVelocity() && (gamepad1.timestamp % 100 < 10)) {
-                    int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
+                    gamepad1.rumble(50);
-                    gamepad1.rumble(rumbleDuration);
                 }
             }
 
@@ -195,14 +196,16 @@ public class SOLOTeleOP {
                 else if (stateMachine.getCurrentGameState() == GameState.SCORING) stateMachine.setGameState(GameState.IDLE);
             }
 
-            if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.06);
+            if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.2);
-            if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.06);
+            if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2);
-
 
+            if (gamepad1.aWasPressed()) {
+                stateMachine.forceLaunchSequence();
+            }
             }
 
             private void updateTelemetry(Pose p) {
-            telemetry.addData("OpMod ev", "SOLO TeleOp");
+            telemetry.addData("OpMode", "SOLO TeleOp");
             telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps());
 
             telemetry.addLine("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
@@ -212,11 +215,13 @@ public class SOLOTeleOP {
             telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE");
             telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
             telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState());
+            telemetry.addData("Flywheel RPM", stateMachine.getTurret().returnFlywheelSpeed());
 
             telemetry.addLine("▰▰▰▰▰▰▰ TRACKING ▰▰▰▰▰▰▰");
             telemetry.addData("Pose", "X:%.1f Y:%.1f H:%.1f°", p.getX(), p.getY(), Math.toDegrees(p.getHeading()));
             telemetry.addData("Distance", "%.1f in", stateMachine.getTurret().getDistance());
             telemetry.addData("Turret Ready", stateMachine.getTurret().isTurretReady());
+            telemetry.addData("Target Tick", stateMachine.getTurret().targetTick);
 
             telemetry.update();
             }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java

@@ -81,7 +81,7 @@ public class StateMachine {
     public void update(boolean manualSAM, boolean manualTracking, Pose currentPose) {
         m_driveTrain.update();
 
-        m_turret.update(manualTracking, manualSAM, currentPose);
+        m_turret.update(manualTracking, manualSAM, currentPose, m_transfer.getLaunchElapsedTime());
 
         if (currentGameState == GameState.SCORING) {
             m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
@@ -94,10 +94,8 @@ public class StateMachine {
         m_transfer.update();
 
         m_led.update(
-                currentGameState == GameState.SCORING,
+                m_turret.isTurretRotationLocked(),
-                m_turret.hasReachedVelocity(),
+                m_turret.hasReachedVelocity()
-                m_turret.hasTarget(),
-                m_driveTrain.isAnchorActive()
         );
 
         if (currentGameState == GameState.SCORING) {
@@ -108,7 +106,7 @@ public class StateMachine {
     }
 
     public void initUpdate() {
-        m_led.update(false, false, false, false);
+        m_led.update(false, false);
     }
 
     private void handleGameStateEntry(GameState newState) {
@@ -182,6 +180,13 @@ public class StateMachine {
         setGameState(GameState.IDLE);
     }
 
+    public void forceLaunchSequence() {
+        setGameState(GameState.SCORING);
+        m_transfer.updateTurretReady(true);
+        m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
+        isIntakeLaunching = true;
+    }
+
     public DriveTrain getDriveTrain() { return m_driveTrain; }
     public Intake getIntake() { return m_intake; }
     public Turret getTurret() { return m_turret; }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java

@@ -107,11 +107,6 @@ public class finalTeleOp {
                 handleOperatorInput();
 
                 stateMachine.update(manualSAM, manualTracking, currentPose);
-                //Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
-
-               // if (healingPose != null) {
-               //     follower.setPose(healingPose);
-               // }
 
                 if (gamepad2.a) {
                     stateMachine.getTransfer().openLimiter();
@@ -124,6 +119,7 @@ public class finalTeleOp {
 
 
                 updateTelemetry(currentPose);
+                sleep(fpsCounter.getSyncTime(100));
             }
 
             stateMachine.setRobotState(RobotState.ESTOP);
@@ -168,9 +164,8 @@ public class finalTeleOp {
                 stateMachine.emergencyStop();
             }
 
-            if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity()) {
+            if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity() && (gamepad2.timestamp % 100 < 10)) {
-                int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
+                gamepad2.rumble(50);
-                gamepad2.rumble(rumbleDuration);
             }
 
             if (gamepad1.psWasPressed()) {
@@ -218,8 +213,8 @@ public class finalTeleOp {
 
 
 
-            if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.6);
+            if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2);
-            if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.6);
+            if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.2);
         }
 
         private void handleOperatorInput() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java

@@ -20,15 +20,16 @@ public class GetDistanceTuning extends OpMode {
 
     private Limelight3A limelight;
     private double distance;
-    public static double CAMERA_HEIGHT_INCHES = 12.5;
+
+    public static double CAMERA_HEIGHT_INCHES = 12.75;
     public static double GOAL_HEIGHT_INCHES = 29.5;
     public static int pipeline = 1;
-    public static double CAMERA_MOUNT_ANGLE_DEGREES = 14.3;
+    public static double CAMERA_MOUNT_ANGLE_DEGREES = 19.8;
+
+    public static double TARGET_DISTANCE = 0.0;
 
     private TelemetryManager telemetryM;
 
-
-
     @Override
     public void init() {
         limelight = hardwareMap.get(Limelight3A.class, "limelight");
@@ -49,14 +50,22 @@ public class GetDistanceTuning extends OpMode {
             double targetY = result.getTy();
             double targetArea = result.getTa();
 
+            if (TARGET_DISTANCE != 0) {
+                double requiredAngleRadians = Math.atan((GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / TARGET_DISTANCE);
+                CAMERA_MOUNT_ANGLE_DEGREES = Math.toDegrees(requiredAngleRadians) - targetY;
+
+                telemetryM.debug("Mode", "Angle Auto-Tuning (Diff Mode)");
+            } else {
+                telemetryM.debug("Mode", "Standard");
+            }
 
             distance = getTrigBasedDistance(targetY);
-
             double areaDistance = getDistanceFromTag(targetArea);
 
             telemetryM.debug("Method", "Trigonometry");
             telemetryM.debug("Target Y (ty)", targetY);
             telemetryM.debug("Calculated Distance", distance);
+            telemetryM.debug("Camera Mount Angle", CAMERA_MOUNT_ANGLE_DEGREES);
 
             telemetryM.debug("Area Algo Distance", areaDistance);
 
@@ -71,13 +80,7 @@ public class GetDistanceTuning extends OpMode {
     }
 
     private double getTrigBasedDistance(double targetOffsetAngleVertical) {
-
         return (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(Math.toRadians(CAMERA_MOUNT_ANGLE_DEGREES + targetOffsetAngleVertical));
-
-        // Calculate distance
-        //double distanceFromLimelightToGoalInches = (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians);
-
-        //return distanceFromLimelightToGoalInches;
     }
 
     private double getDistanceFromTag(double x) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java

@@ -30,11 +30,11 @@ public class PIDFTuning extends OpMode {
     public void init() {
         flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret");
         flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE);
+        flywheelMotorR.setDirection(DcMotorSimple.Direction.FORWARD);
 
-        flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret");
+        flywheelMotorL = hardwareMap.get(DcMotorEx.class, "lturret");
-        flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        flywheelMotorL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        flywheelMotorR.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+        flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
 
         telemetry.addLine("Init complete");
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java

@@ -27,8 +27,8 @@ public class TurretRotationTuning extends LinearOpMode {
     public static double MAX_INTEGRAL = 0.5;
 
     // --- MECHANICAL CONSTANTS ---
-    public static double TICKS_PER_REV_MOTOR = 384.5;
+    public static double TICKS_PER_REV_MOTOR = 145.1;
-    public static double EXTERNAL_GEAR_RATIO = 2.68888889;
+    public static double EXTERNAL_GEAR_RATIO = 9.179875;
     public static boolean MOTOR_REVERSED = true;
 
     // --- CALIBRATION SETTINGS ---
@@ -117,8 +117,6 @@ public class TurretRotationTuning extends LinearOpMode {
                     break;
             }
 
-            sleep(fps.getSyncTime(targetFPS));
-            
             // --- TELEMETRY ---
             telemetryM.debug("▰▰▰▰▰ MODE ▰▰▰▰▰", currentMode.toString());
             telemetryM.debug("Gear Ratio", EXTERNAL_GEAR_RATIO);
@@ -128,6 +126,8 @@ public class TurretRotationTuning extends LinearOpMode {
             telemetryM.debug("Target Deg", targetTicks / ticksPerDegree);
             telemetry.addData("FPS", fps.getAverageFps());
             telemetryM.update(telemetry);
+
+            sleep(fps.getSyncTime(targetFPS));
         }
         limelight.stop();
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java

@@ -336,7 +336,7 @@ public class allInOne extends LinearOpMode {
             double ta = result.getTa();
             calculatedDistance = getDistanceFromTag(ta);
             calculatedVelocity = getFlywheelVelocity(calculatedDistance);
-            calculatedHoodPos = getHoodPOS(calculatedDistance, calculatedVelocity);
+            calculatedHoodPos = getHoodPOS(calculatedDistance);
 
             if (launch) {
                 leftTurret.setVelocity(calculatedVelocity);
@@ -459,7 +459,7 @@ public class allInOne extends LinearOpMode {
         return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
     }
 
-    private double getHoodPOS(double dist, double targetVelocity) {
+    private double getHoodPOS(double dist) {
-        return Range.clip(((-(5.15893 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000954719 * Math.pow(dist, 3) - (0.000349083 * Math.pow(dist, 2)) + (-0.0153896 * dist) + 1.37618 + calculateHoodOffset(targetVelocity)), SERVO_MIN, SERVO_MAX);
+        return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/anchorTuning.java

@@ -83,8 +83,6 @@ public class anchorTuning extends LinearOpMode {
                 driveTrain.teleopDrive(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.right_stick_x);
             }
 
-            sleep(fps.getSyncTime(targetFPS));
-
             telemetryM.debug("Anchor Active", driveTrain.isAnchorActive());
 
             telemetry.addData("FPS", fps.getAverageFps());
@@ -94,6 +92,8 @@ public class anchorTuning extends LinearOpMode {
             telemetryM.debug("Error TRN (Degs)", driveTrain.getLastTurnError());
 
             telemetryM.update(telemetry);
+
+            sleep(fps.getSyncTime(targetFPS));
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/autoCollection.java

@@ -223,8 +223,8 @@ public class autoCollection extends LinearOpMode {
                     break;
             }
 
-            sleep(fpsCounter.getSyncTime(targetFPS));
             updateTelemetry(currentTicks, currentDegrees);
+            sleep(fpsCounter.getSyncTime(targetFPS));
         }
         limelight.stop();
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java

@@ -5,6 +5,7 @@ import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.Servo;
@@ -12,26 +13,46 @@ import com.qualcomm.robotcore.util.Range;
 
 // Make sure your import points to your actual Constants file
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
 @Configurable
 @TeleOp
 public class flywheelTuning extends OpMode {
 
+    public enum CalibrationState {
+        IDLE,
+        SPOOLING,
+        TRACKING,
+        TUNING
+    }
 
     private Limelight3A limelight;
 
     public DcMotorEx flywheelMotorR;
     public DcMotorEx flywheelMotorL;
 
-    public Servo hoodServo;
+    public DcMotor intake;
 
+    public Servo hoodServo;
+    public Servo limiter;
+
+    // Configurable / Dashboard variables
+    public static boolean autoMode = false;
     public static double curTargetVelocity = 0;
     public static double targetPOS = 0;
     public static int pipeline = 1;
-
     public static boolean reverse = false;
 
+    // Standard runtime variables
     double distance = 0;
 
+    // State machine and tracking variables
+    CalibrationState calibState = CalibrationState.IDLE;
+    boolean lastX = false;
+
+    double maxError = 0.0;
+    double snapshotAutoHood = 0.0;
+    double calculatedGain = 0.0;
+
     @Override
     public void init() {
         flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
@@ -43,45 +64,168 @@ public class flywheelTuning extends OpMode {
         flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
 
         hoodServo = hardwareMap.servo.get("hoodservo");
-
         hoodServo.setDirection(Servo.Direction.REVERSE);
 
         limelight = hardwareMap.get(Limelight3A.class, "limelight");
         limelight.pipelineSwitch(pipeline);
         limelight.start();
 
+        intake = hardwareMap.get(DcMotor.class, "intake");
+        intake.setDirection(DcMotorSimple.Direction.FORWARD);
+        intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        limiter = hardwareMap.servo.get("limiter");
+        limiter.setDirection(Servo.Direction.FORWARD);
+
+        limiter.setPosition(1);
+
+
+
         telemetry.addLine("Init complete");
     }
 
     @Override
     public void loop() {
         limelight.pipelineSwitch(pipeline);
-
         LLResult result = limelight.getLatestResult();
 
-        if (result.isValid()) {
+        if (result != null && result.isValid()) {
             double targetY = result.getTy();
             telemetry.addData("Ty", targetY);
             distance = getTrigBasedDistance(targetY);
         }
 
-        if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
+        boolean currentX = gamepad1.x;
-        flywheelMotorL.setVelocity(curTargetVelocity);}
+        if (currentX && !lastX) {
+            switch (calibState) {
+                case IDLE:
+                    maxError = 0.0;
+                    calculatedGain = 0.0;
+                    calibState = CalibrationState.SPOOLING;
+                    break;
+                case SPOOLING:
+                    calibState = CalibrationState.TRACKING;
+                    break;
+                case TRACKING:
+                    snapshotAutoHood = getHoodPOS(distance);
+                    calibState = CalibrationState.TUNING;
+                    break;
+                case TUNING:
+                    calibState = CalibrationState.IDLE;
+                    break;
+            }
+        }
+        lastX = currentX;
 
-        hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+        double applyVel = 0;
+        double applyHood = 0;
+        double currentVel = flywheelMotorR.getVelocity();
+
+        double autoTargetVel = getFlywheelVelocity(distance);
+        double autoHoodPos = getHoodPOS(distance);
 
 
 
-        telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity());
+        if (autoMode) {
-        telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity);
+            switch (calibState) {
-        telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS);
+                case IDLE:
-        telemetry.addData("Distance", distance);
+                    applyVel = autoTargetVel;
+                    applyHood = autoHoodPos;
+                    break;
+
+                case SPOOLING:
+                    applyVel = autoTargetVel;
+                    applyHood = autoHoodPos;
+                    if (Math.abs(currentVel - autoTargetVel) <= 30) {
+                        calibState = CalibrationState.TRACKING;
+                    }
+                    break;
+
+                case TRACKING:
+                    applyVel = autoTargetVel;
+                    applyHood = autoHoodPos;
+
+                    double currentError = currentVel - autoTargetVel;
+                    if (currentError < maxError) {
+                        maxError = currentError;
+                    }
+                    break;
+
+                case TUNING:
+                    applyVel = autoTargetVel + maxError;
+
+                    applyHood = targetPOS;
+
+                    if (maxError != 0) {
+                        calculatedGain = (targetPOS - snapshotAutoHood) / maxError;
+                    } else {
+                        calculatedGain = 0;
+                    }
+                    break;
+            }
+        } else {
+            applyVel = curTargetVelocity;
+            applyHood = targetPOS;
+        }
+
+        if (!gamepad1.b) {
+            flywheelMotorR.setVelocity(applyVel);
+            flywheelMotorL.setVelocity(applyVel);
+        } else {
+            flywheelMotorR.setVelocity(0);
+            flywheelMotorL.setVelocity(0);
+        }
+
+        if (gamepad1.a) {
+            intake.setPower(1);
+            limiter.setPosition(0.82);
+        } else {
+            limiter.setPosition(1);
+            intake.setPower(0);
+        }
+
+        hoodServo.setPosition(Range.clip(applyHood, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+
+
+        // --- TELEMETRY ---
+        telemetry.addData("Operating Mode", autoMode ? "AUTOMATIC" : "MANUAL");
+        telemetry.addData("Calibration State", calibState.toString());
+        telemetry.addLine();
+
+        telemetry.addData("Current Velocity", "%.2f", currentVel);
+        telemetry.addData("Applied Target Vel", "%.2f", applyVel);
+        telemetry.addData("Applied Target Hood", "%.4f", applyHood);
+        telemetry.addData("Distance", "%.2f inches", distance);
+        telemetry.addLine();
+
+        if (autoMode) {
+            telemetry.addData("Raw Auto Target Vel", "%.2f", autoTargetVel);
+            telemetry.addData("Raw Auto Target Hood", "%.4f", autoHoodPos);
+            telemetry.addLine("--- CALIBRATION DATA ---");
+            telemetry.addData("Max RPM Drop (Error)", "%.2f", maxError);
+            telemetry.addData("Snapshot Auto Hood", "%.4f", snapshotAutoHood);
+            telemetry.addData("User Tuning Hood (targetPOS)", "%.4f", targetPOS);
+            telemetry.addData("CALCULATED GAIN", "%.8f", calculatedGain);
+            telemetry.addLine("NOTE: When done tuning, copy CALCULATED GAIN into your snippet!");
+        }
 
         telemetry.update();
     }
 
     private double getTrigBasedDistance(double targetOffsetAngleVertical) {
-        return (17.1) / Math.tan(Math.toRadians(7.9 + targetOffsetAngleVertical));
+        return (29.5-12.75) / Math.tan(Math.toRadians(19.9 + targetOffsetAngleVertical));
         //      ^ GOAL - CAMERA HEIGHT         ^ Offset Degrees
     }
+
+    private double getFlywheelVelocity(double dist) {
+        return (0.0000119972 * Math.pow(dist, 4)
+                - 0.00249603 * Math.pow(dist, 3)
+                + 0.179513 * Math.pow(dist, 2)
+                + (0.0519688 * dist)
+                + 1489.8908);
+    }
+
+    private double getHoodPOS(double dist) {
+        return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859);
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java

@@ -23,38 +23,40 @@ import java.util.List;
 @TeleOp(name = "Odo Turret Tracking", group = "Turret")
 public class odoTracking extends LinearOpMode {
 
-    public static double kP = 0.02;
+    public static double kP = 0.0040;
-    public static double kI = 0.0;
+    public static double kI = 0.002;
-    public static double kD = 0.001;
+    public static double kD = 0.0004;
     public static double MAX_POWER = 0.6;
     public static double MAX_INTEGRAL = 0.5;
 
     // ================= Hardware Constants =================
-    public static double TICKS_PER_REV_MOTOR = 384.5;
+    public static double TICKS_PER_REV_MOTOR = 145.1;
-    public static double EXTERNAL_GEAR_RATIO = 2.68888889;
+    public static double EXTERNAL_GEAR_RATIO = 9.179875;
-    public static boolean MOTOR_REVERSED = true;
+    public static boolean MOTOR_REVERSED = false;
 
     // ================= Limits & Logic =================
-    public static double LL_LOGIC_MULTIPLIER = 1.0;
+    public static double LL_LOGIC_MULTIPLIER = -1.0;
-    public static double SOFT_LIMIT_NEG = -230;
+    public static double SOFT_LIMIT_NEG = -330;
-    public static double SOFT_LIMIT_POS = 230;
+    public static double SOFT_LIMIT_POS = 350;
 
     // ================= Targeting Offsets =================
-    public static double RED_TARGET_OFFSET_DEGREES = 14;
+    public static double RED_TARGET_OFFSET_DEGREES = 0;
-    public static double BLUE_TARGET_OFFSET_DEGREES = 17;
+    public static double BLUE_TARGET_OFFSET_DEGREES = 0;
-    public static double LL_TARGET_OFFSET_DEGREES = -2;
+    public static double LL_TARGET_OFFSET_DEGREES = 0;
 
     // ================= Field Coordinates =================
-    public static double GOAL_RED_X = 138;
+    public static double GOAL_RED_X = 140;
-    public static double GOAL_RED_Y = 138;
+    public static double GOAL_RED_Y = 140;
-    public static double GOAL_BLUE_X = 6;
+    public static double GOAL_BLUE_X = 4;
-    public static double GOAL_BLUE_Y = 138;
+    public static double GOAL_BLUE_Y = 140;
 
     // ================= Sensor Fusion Settings =================
     public static double OFFSET_SMOOTHING = 0.2;
 
     public static int targetFPS = 100;
 
+    public static double TURRET_FULL_WRAP_TICKS = 1340.0;
+
     // ================= Hardware Objects =================
     private Limelight3A limelight;
     private DcMotorEx turretMotor;
@@ -68,7 +70,6 @@ public class odoTracking extends LinearOpMode {
 
     private double targetCorrectionOffsetTicks = 0;
     private double fusedTargetTicks = 0;
-
     private enum TuningMode { CALIBRATION, TRACKING }
     private TuningMode currentMode = TuningMode.CALIBRATION;
 
@@ -80,7 +81,7 @@ public class odoTracking extends LinearOpMode {
     protected FPSCounter fpsCounter = new FPSCounter();
 
     public static double start_x = 72;
-    public static double start_y = 8.5;
+    public static double start_y = 72;
     public static double start_heading = 90;
 
 
@@ -157,48 +158,65 @@ public class odoTracking extends LinearOpMode {
                     break;
             }
 
-            sleep(fpsCounter.getSyncTime(targetFPS));
             updateTelemetry(currentTicks, currentDegrees);
+            sleep(fpsCounter.getSyncTime(targetFPS));
         }
         limelight.stop();
     }
 
     private void calculateHybridTarget(double currentTicks, double currentDegrees) {
-        double odoTargetTicks = calculateOdometryTargetTicks();
+        Pose robotPose = follower.getPose();
+        double odoTargetTicks = calculateOdometryTargetTicks(robotPose);
 
         LLResult result = limelight.getLatestResult();
 
         if (result != null && result.isValid()) {
-            double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
+            // 1. Calculate Vision Truth: Where the goal sits based on the camera right now
+            double tx = result.getTx();
+            double visionTargetDegrees = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
+            double finalVisionDegrees = visionTargetDegrees + LL_TARGET_OFFSET_DEGREES;
+            double visionTargetTicks = finalVisionDegrees * ticksPerDegree;
 
-            targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - OFFSET_SMOOTHING))
+            // 2. Calculate Current Fused Target: Where we currently think the goal is (Odo + existing Offset)
-                    + (rawErrorTicks * OFFSET_SMOOTHING);
+            double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+
+            // 3. Calculate Correction Error: The delta between our current belief and camera reality
+            double correctionError = visionTargetTicks - currentFusedTarget;
+
+            // 4. Normalize the error for wrapping (360 degrees)
+            double ticksPerRev = 360.0 * ticksPerDegree;
+            while (correctionError > (ticksPerRev / 2))  correctionError -= ticksPerRev;
+            while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev;
+
+            // 5. Apply the error to the persistent offset (Bias-Shift)
+            // We use OFFSET_SMOOTHING as a gain to prevent single-frame noise from causing jitter
+            targetCorrectionOffsetTicks += (correctionError * OFFSET_SMOOTHING);
         }
 
         double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
 
-        fusedTargetTicks = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+        // 1. Apply initial soft limits
+        double preWrapTarget = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+
+        // 2. Implement wrapping logic: if target exceeds 180 degrees (670 ticks), normalize to the other side.
+        // This implies a full wrap cycle is 2 * 670 = 1340 ticks.
+        double fullWrapTicks = TURRET_FULL_WRAP_TICKS;
+        double halfWrapTicks = fullWrapTicks / 2.0;
+
+        // If target is beyond positive half-wrap threshold, wrap to negative equivalent
+        if (preWrapTarget > halfWrapTicks) {
+            preWrapTarget -= fullWrapTicks;
+        }
+        // If target is beyond negative half-wrap threshold, wrap to positive equivalent
+        else if (preWrapTarget < -halfWrapTicks) {
+            preWrapTarget += fullWrapTicks;
         }
 
-    private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
+        // 3. Apply final soft limits after wrapping
-        double tx = result.getTx();
+        fusedTargetTicks = Range.clip(preWrapTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
-
-        double visionTargetDegrees = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
-
-        double finalVisionDegrees = visionTargetDegrees + LL_TARGET_OFFSET_DEGREES;
-        double visionTargetTicks = finalVisionDegrees * ticksPerDegree;
-
-        double rawErrorTicks = visionTargetTicks - odoTargetTicks;
-
-        double ticksPerRev = 360.0 * ticksPerDegree;
-
-        while (rawErrorTicks > (ticksPerRev / 2))  rawErrorTicks -= ticksPerRev;
-        while (rawErrorTicks < -(ticksPerRev / 2)) rawErrorTicks += ticksPerRev;
-        return rawErrorTicks;
     }
 
-    private double calculateOdometryTargetTicks() {
+    private double calculateOdometryTargetTicks(Pose robotPose) {
-        Pose robotPose = follower.getPose();
         double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X;
         double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y;
 
@@ -237,6 +255,7 @@ public class odoTracking extends LinearOpMode {
 
         double output = (kP * error) + integralTerm + (kD * derivative);
         output = Range.clip(output, -MAX_POWER, MAX_POWER);
+        telemetry.addData("Power", output);
 
         turretMotor.setPower(output);
         lastError = error;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java

@@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.util;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 public class FPSCounter {
-    private ElapsedTime loopTimer = new ElapsedTime();
+    private final ElapsedTime loopTimer = new ElapsedTime();
     private double loopTimeSum = 0;
     private int loopCount = 0;
     private double currentLoopTime = 0;
@@ -13,6 +13,9 @@ public class FPSCounter {
         reset();
     }
 
+    /**
+     * Call this at the very beginning of your while(opModeIsActive) loop.
+     */
     public void startLoop() {
         double now = loopTimer.milliseconds();
         if (loopCount > 0) {
@@ -23,10 +26,17 @@ public class FPSCounter {
         loopCount++;
     }
 
+    /**
+     * Calculates the time remaining to hit the target FPS.
+     * To be accurate, call this at the very end of your loop, 
+     * AFTER telemetry.update().
+     */
     public long getSyncTime(int targetFps) {
-        double targetMs = 1000.0 / targetFps;
+        if (targetFps <= 0) return 0;
         
-        double elapsedWorkMs = loopTimer.milliseconds() - lastTime;
+        double targetMs = 1000.0 / targetFps;
+        double now = loopTimer.milliseconds();
+        double elapsedWorkMs = now - lastTime;
         double remainingMs = targetMs - elapsedWorkMs;
 
         return (remainingMs > 0) ? (long) remainingMs : 0;

gradle.properties

@@ -7,7 +7,12 @@ android.useAndroidX=true
 android.enableJetifier=false
 
 # Allow Gradle to use up to 1 GB of RAM
-org.gradle.jvmargs=-Xmx1024M
+org.gradle.jvmargs=-Xmx2048M
 
 android.nonTransitiveRClass=false
 org.gradle.configuration-cache=true
+
+org.gradle.parallel=true
+org.gradle.caching=true
+
+org.gradle.daemon=true