pre-spring break

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

@@ -15,6 +15,10 @@ import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.hardware.CRServo;
 import com.qualcomm.robotcore.hardware.SwitchableLight;
 
+
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+import com.qualcomm.robotcore.hardware.PwmControl;
+
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
@@ -35,10 +39,24 @@ public class hwMap {
     }
 
     public static class LedHwMap {
-        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led;
+        private ServoImplEx led;
 
         public LedHwMap(HardwareMap hardwareMap) {
-            //led = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, Constants.LEDConstants.LED);
+
+            led = hardwareMap.get(ServoImplEx.class, Constants.LEDConstants.LED);
+            led.setPwmRange(new PwmControl.PwmRange(500, 2500));
+        }
+
+        public void setLedPos(double pos) {
+            led.setPosition(pos);
+        }
+
+        public void setLedOn(boolean yes) {
+            if (yes) {
+                led.setPwmEnable();
+            } else {
+                led.setPwmDisable();
+            }
         }
     }
 
@@ -248,7 +266,7 @@ public class hwMap {
         private double lastKnownDistance = 0.0;
         private ElapsedTime noTargetTimer = new ElapsedTime();
 
-        PIDFCoefficients pidfCoefficients = new PIDFCoefficients(30, 0, 10, 15);
+        PIDFCoefficients pidfCoefficients = new PIDFCoefficients(10, 0, 11, 14.3);
 
         public TurretHwMap(HardwareMap hardwareMap) {
             noTargetTimer.reset();

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

@@ -5,6 +5,7 @@ import com.pedropathing.follower.Follower;
 import com.pedropathing.geometry.BezierCurve;
 import com.pedropathing.geometry.BezierLine;
 import com.pedropathing.geometry.Pose;
+import com.pedropathing.math.Vector;
 import com.pedropathing.paths.PathChain;
 import com.pedropathing.util.Timer;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -116,7 +117,7 @@ public class Auton {
 
         @Override
         public void buildPaths() {
-            startPose = new Pose(31.5, 137, Math.toRadians(180));
+            startPose = new Pose(21, 124, Math.toRadians(143.5));
             follower.setStartingPose(startPose);
 
             // Waypoints
@@ -124,9 +125,9 @@ public class Auton {
             Pose set2DrivePose = new Pose(39, 60, Math.toRadians(180));
             Pose set2PickPose = new Pose(14, 60, Math.toRadians(180)); // tangential
             Pose set2ScorePose = new Pose(54, 90, Math.toRadians(144));
-            Pose gate1PickPose = new Pose(10.652, 61.079, Math.toRadians(140));
+            Pose gate1PickPose = new Pose(9, 60, Math.toRadians(143.5));
             Pose gate1ScorePose = new Pose(54, 90, Math.toRadians(144));
-            Pose gate2PickPose = new Pose(10.652, 61.079, Math.toRadians(140));
+            Pose gate2PickPose = new Pose(9, 60, Math.toRadians(143.5));
             Pose gate2ScorePose = new Pose(54, 90, Math.toRadians(144));
             Pose set3DrivePose = new Pose(38.515, 47.156, Math.toRadians(220));
             Pose set3PickPose = new Pose(16.419, 35.945, Math.toRadians(185));
@@ -137,7 +138,6 @@ public class Auton {
             Pose parkTargetPose = new Pose(36.219, 93.170, Math.toRadians(148));
 
             // Controls
-            Pose preloadControl = new Pose(27.400, 115.541);
             Pose set2ScoreControl = new Pose(50.386, 78.923);
             Pose gate1PickControl = new Pose(40.119, 55.595);
             Pose gate1ScoreControl = new Pose(41.918, 62.187);
@@ -148,7 +148,7 @@ public class Auton {
             Pose set1PickControl = new Pose(14.863, 77.692);
 
             scorePreloadPath = follower.pathBuilder()
-                    .addPath(new BezierCurve(startPose, preloadControl, preloadScorePose))
+                    .addPath(new BezierLine(startPose, preloadScorePose))
                     .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading())
                     .build();
 
@@ -319,7 +319,7 @@ public class Auton {
 
         @Override
         public void buildPaths() {
-            startPose = new Pose(112.5, 137, Math.toRadians(0));
+            startPose = new Pose(122, 125.5, Math.toRadians(36));
             follower.setStartingPose(startPose);
 
             // Waypoints
@@ -327,9 +327,9 @@ public class Auton {
             Pose set2DrivePose = new Pose(105, 60, Math.toRadians(0));
             Pose set2PickPose = new Pose(130, 60, Math.toRadians(0)); // tangential
             Pose set2ScorePose = new Pose(90, 90, Math.toRadians(36));
-            Pose gate1PickPose = new Pose(133.348, 61.079, Math.toRadians(40));
+            Pose gate1PickPose = new Pose(132.7, 56, Math.toRadians(36));
             Pose gate1ScorePose = new Pose(90, 90, Math.toRadians(36));
-            Pose gate2PickPose = new Pose(133.348, 61.079, Math.toRadians(40));
+            Pose gate2PickPose = new Pose(132.7, 56, Math.toRadians(36));
             Pose gate2ScorePose = new Pose(90, 90, Math.toRadians(36));
             Pose set3DrivePose = new Pose(105.485, 47.156, Math.toRadians(-40));
             Pose set3PickPose = new Pose(127.581, 35.945, Math.toRadians(-5));
@@ -442,8 +442,8 @@ public class Auton {
         protected final boolean isNet;
         protected final boolean useTimer;
 
-        public static double timerTimeout = 4;
+        public static double timerTimeout = 4.5;
-        public static double gateWaitTime = 3.0;
+        public static double gateWaitTime = 4.0;
 
         protected Pose startPose;
 
@@ -517,8 +517,6 @@ 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);
 
@@ -538,7 +536,7 @@ public class Auton {
             turretHw.resetTurret();
 
             waitForStart();
-            systems.update(allianceTarget, follower.getPose());
+            systems.update(allianceTarget, follower.getPose(), new Vector(0, 0));
             opmodeTimer.resetTimer();
             setPathState(PathState.DRIVE_TO_SCORE_PRELOAD);
 
@@ -548,7 +546,7 @@ public class Auton {
                 }
                 follower.update();
                 statePathUpdate();
-                systems.update(allianceTarget, follower.getPose());
+                systems.update(allianceTarget, follower.getPose(), follower.getVelocity());
 
                 telemetry.addData("Path State", pathState);
                 telemetry.addData("System State", systems.getCurrentState());

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

@@ -137,7 +137,7 @@ public abstract class TimeAuton extends LinearOpMode {
         timer.reset();
         while(opModeIsActive() && timer.seconds() < 2.0) {
             updateSystems();
-            if(turret.isTurretReady()) break;
+            if(turret.isTurretReady(true)) break;
 
         }
 
@@ -167,13 +167,13 @@ public abstract class TimeAuton extends LinearOpMode {
         AutoTransfer.updatePose(currentPose);
 
         // Update Turret with current Pose
-        turret.updateAuton(alliance, currentPose, 0.2);
+        turret.updateAuton(alliance, currentPose, follower.getVelocity(), 0.2);
 
-        transfer.updateTurretReady(turret.isTurretReady());
+        transfer.updateTurretReady(turret.isTurretReady(true));
         transfer.update();
 
         // Telemetry for debugging
-        telemetry.addData("Turret Ready", turret.isTurretReady());
+        telemetry.addData("Turret Ready", turret.isTurretReady(false));
         telemetry.addData("Transfer State", transfer.getTransferState());
         telemetry.update();
     }

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

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.auton;
 
 import com.pedropathing.geometry.Pose;
+import com.pedropathing.math.Vector;
 
 import org.firstinspires.ftc.teamcode.Hware.hwMap;
 import org.firstinspires.ftc.teamcode.subsystems.DriveTrain; // Unused but kept for reference
@@ -88,8 +89,8 @@ public class pedroStateMachine {
         }
     }
 
-    public void update(int allianceTarget, Pose currentPose) {
+    public void update(int allianceTarget, Pose currentPose, Vector currentVelocity) {
-        m_turret.updateAuton(allianceTarget, currentPose, 0.2);
+        m_turret.updateAuton(allianceTarget, currentPose, currentVelocity, 0.2);
 
         if (currentState == AutonState.SCORING) {
             m_transfer.updateTurretReady(m_turret.hasReachedVelocity());

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

@@ -40,7 +40,7 @@ public class DriveTrain {
     public enum DriveState {
         NORMAL, TURBO, PRECISION, STOP
     }
-    private DriveState currentState = DriveState.NORMAL;
+    private DriveState currentState = DriveState.TURBO;
     private double speedMultiplier = 1.0;
 
     public DriveTrain(hwMap.DriveHwMap hardware) {
@@ -59,8 +59,8 @@ public class DriveTrain {
         if(dt == 0) dt = 0.001;
         lastLoopTime = currentTime;
 
-        double currentFwd = h_driveTrain.backRightMotor.getCurrentPosition();
+        double currentFwd = h_driveTrain.frontLeftMotor.getCurrentPosition();
-        double currentStrafe = h_driveTrain.frontLeftMotor.getCurrentPosition();
+        double currentStrafe = h_driveTrain.frontRightMotor.getCurrentPosition();
         double currentHeading = getHeadingDegrees();
 
         // 2. Calculate Errors
@@ -72,8 +72,6 @@ public class DriveTrain {
         while (errorTurn > 180) errorTurn -= 360;
         while (errorTurn < -180) errorTurn += 360;
 
-        // 3. PID Calculations
-
         // Forward
         sumFwd += errorForward * dt;
         double dFwd = (errorForward - lastFwdErr) / dt;

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

@@ -1,36 +1,20 @@
 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 currentLedState = -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);
-
     public LED(hwMap.LedHwMap hardware) {
         this.hardware = hardware;
-
+        turnOn();
-        int LED_BRIGHTNESS = Constants.LEDConstants.LAYER_BRIGHTNESS;
-        solidRed.setBrightness(LED_BRIGHTNESS);
-        solidOrange.setBrightness(LED_BRIGHTNESS);
-        solidYellow.setBrightness(LED_BRIGHTNESS);
-        solidGreen.setBrightness(LED_BRIGHTNESS);
     }
 
     public void update(boolean isTurretLocked, boolean isFlywheelUpToSpeed) {
-        /*int newLedState = 0;
+        int newLedState = 0;
 
         if (isTurretLocked && isFlywheelUpToSpeed) {
             newLedState = 1;
@@ -42,15 +26,25 @@ public class LED {
 
         if (newLedState != currentLedState) {
             if (newLedState == 0) {
-                hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
+                hardware.setLedPos(Constants.LEDConstants.COLOR_RED);
             } else if (newLedState == 1) {
-                hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
+                hardware.setLedPos(Constants.LEDConstants.COLOR_GREEN);
             } else if (newLedState == 2) {
-                hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
+                hardware.setLedPos(Constants.LEDConstants.COLOR_ORANGE);
             } else if (newLedState == 3) {
-                hardware.led.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidYellow);
+                hardware.setLedPos(Constants.LEDConstants.COLOR_YELLOW);
             }
+
             currentLedState = newLedState;
-        }*/
+        }
+    }
+
+    public void turnOff() {
+        hardware.setLedOn(false);
+        currentLedState = -1;
+    }
+    
+    public void turnOn() {
+        hardware.setLedOn(true);
     }
 }

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

@@ -7,6 +7,8 @@ import com.qualcomm.robotcore.util.Range;
 import org.firstinspires.ftc.teamcode.Hware.hwMap;
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
 
+import com.pedropathing.math.Vector;
+
 public class Turret {
 
     private final hwMap.TurretHwMap hardware;
@@ -24,11 +26,14 @@ public class Turret {
     private double targetCorrectionOffsetTicks = 0;
     private double fusedTargetTicks = 0;
     public double turretPowerRotation;
-    public static double offsetTicks = 0.0;
+    private double offsetTicks = 0.0;
 
     public double offsetHood = 0;
 
     private Pose robotPose;
+    private Vector robotVelocity = new Vector(0,0); // NEW
+    private double kinematicDistanceOffset = 0.0; // NEW
+    private double kinematicAngularOffsetTicks = 0.0; // NEW
 
     public double targetTick;
 
@@ -59,6 +64,7 @@ public class Turret {
         if (this.alliance != alliance) {
             this.alliance = alliance;
             targetCorrectionOffsetTicks = 0; // Reset correction when alliance changes
+            offsetTicks = 0; // Reset manual offset when alliance changes
             if (alliance == 2) {
                 hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
             } else if (alliance == 1) {
@@ -108,17 +114,17 @@ public class Turret {
         return hardware.hasTarget();
     }
 
-    public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, double elapsedTime) {
+    public void update(boolean manualTracking, boolean manualSAM, Pose currentPose, Vector currentVelocity, double elapsedTime) {
         double dt = loopTimer.seconds();
         loopTimer.reset();
         if (dt < 0.001) dt = 0.001;
 
-        updatePose(currentPose);
+        updatePose(currentPose, currentVelocity);
 
         double rawTicks = hardware.getTurretRotationPosition();
         double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
 
-        distance = getDistance();
+        distance = getDistance() + kinematicDistanceOffset;
 
         if (!manualTracking) {
             calculateHybridTarget(currentPose, currentDegrees, rawTicks);
@@ -131,19 +137,21 @@ public class Turret {
             handleLaunchLogic(elapsedTime);
         }
         else {
-           // idleLaunch();
+            idleLaunch();
         }
     }
 
+
+
     public void idleLaunch() {
-        hardware.setTurretVelocity(1000);
+        hardware.setTurretVelocity(1300);
 
         double calculatedHoodPos = getHoodPOS(65);
         setHoodPos(calculatedHoodPos);
     }
 
-    public void updateAuton(int alliance, Pose currentPose, double offset) {
+    public void updateAuton(int alliance, Pose currentPose, Vector currentVelocity, double offset) {
-        updatePose(currentPose);
+        updatePose(currentPose, currentVelocity); // Updated
         double dt = loopTimer.seconds();
         loopTimer.reset();
         if (dt < 0.001) dt = 0.001;
@@ -158,11 +166,11 @@ public class Turret {
         double rawTicks = hardware.getTurretRotationPosition();
         double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
 
-        distance = getDistance();
+        distance = getDistance() + kinematicDistanceOffset;
 
         calculateHybridTarget(currentPose, currentDegrees, rawTicks);
         runPIDControl(rawTicks, dt);
-        handleLaunchLogicOffset(-offset);
+        handleLaunchLogicOffset(0);
     }
 
     private void handleLaunchLogic(double elapsedTime) {
@@ -170,11 +178,6 @@ public class Turret {
             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;
@@ -190,7 +193,7 @@ public class Turret {
             double calculatedHoodPos = getHoodPOS(distance);
             double currentVel = hardware.getFlywheelVelocities()[1];
 
-            double recoilOffset = (currentVel - targetVelocity) * -0.000185;
+            double recoilOffset = (currentVel - targetVelocity) * -0.000385;
 
             setHoodPos(calculatedHoodPos + recoilOffset - 0.03);
         } else {
@@ -218,7 +221,7 @@ public class Turret {
             double calculatedHoodPos = getHoodPOS(distance);
             double currentVel = hardware.getFlywheelVelocities()[1];
 
-            double recoilOffset = (currentVel - targetVelocity) * -0.000185;
+            double recoilOffset = (currentVel - targetVelocity) * -0.000385;
 
             setHoodPos(calculatedHoodPos + recoilOffset);
         } else {
@@ -226,21 +229,27 @@ public class Turret {
         }
     }
 
+
+
+
     private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) {
         double odoTargetTicks = calculateOdometryTargetTicks(robotPose);
         LLResult result = hardware.getLimelightResult();
 
         if (result != null && result.isValid()) {
-            // 1. Calculate Vision Truth: Where the goal sits based on the camera right now
             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;
 
-            // 2. Calculate Current Fused Target: Where we currently think the goal is (Odo + existing Offset)
+            visionTargetTicks += kinematicAngularOffsetTicks;
+
+            // Shift the vision target by our manual offset so LL doesn't fight it
+            double visionTargetWithManualOffset = visionTargetTicks + offsetTicks;
+
             double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks;
 
-            double correctionError = visionTargetTicks - currentFusedTarget;
+            double correctionError = visionTargetWithManualOffset - currentFusedTarget;
 
             // 4. Normalize the error for wrapping (360 degrees)
             double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE;
@@ -277,21 +286,19 @@ public class Turret {
 
         while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
         while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
-
         double relativeDegrees = Math.toDegrees(relativeRad);
         double staticOffset = (alliance == 2) ? Constants.TurretConstants.RED_TARGET_OFFSET_DEGREES : Constants.TurretConstants.BLUE_TARGET_OFFSET_DEGREES;
 
-        return (relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE;
+        // APPLY THE KINEMATIC SHIFT
+        return ((relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE) + kinematicAngularOffsetTicks;
     }
 
     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 = 10;
-        double entryThreshold = 15;  // Enter hold mode if error is within this range
+        double exitThreshold = 30;
-        double exitThreshold = 30;   // Exit hold mode if error exceeds this range
 
         if (!turretModeHold && Math.abs(error) <= entryThreshold) {
             turretModeHold = true;
@@ -302,7 +309,7 @@ public class Turret {
         if (turretModeHold) {
             if (lastTurretModeHold != turretModeHold) {
                 hardware.runToPos((int) fusedTargetTicks); // Set target first
-                hardware.enableTurretHold();               // Then set mode
+                hardware.enableTurretHold();
                 hardware.setTurretRotationPower(Constants.TurretConstants.TURRET_HOLD_POWER); // Set a holding power
             }
             hardware.runToPos((int) fusedTargetTicks); // Keep target updated even in hold mode
@@ -337,20 +344,60 @@ public class Turret {
             double absTarget = Math.abs(targetVelocity);
 
             double error = Math.abs(absCurrent - absTarget);
-            double tolerance = absTarget * 0.03;
+            double tolerance = absTarget * 0.05;
 
             return error <= tolerance;
         }
         return false;
     }
 
-    public boolean isTurretReady() {
+    public boolean isTurretReady(boolean isAuton) {
-        return hasReachedVelocity() && isTurretRotationLocked();
+
+        if (!isAuton) return hasReachedVelocity() && isTurretRotationLocked();
+        else return hasReachedVelocity();
     }
 
 
-    public void updatePose(Pose robotPose) {
+    public void updatePose(Pose robotPose) { // Fallback if called somewhere else without velocity
+        updatePose(robotPose, new Vector(0,0));
+    }
+
+    public void updatePose(Pose robotPose, Vector robotVelocity) {
         this.robotPose = robotPose;
+        this.robotVelocity = robotVelocity != null ? robotVelocity : new Vector(0,0);
+        calculateKinematicOffsets();
+    }
+
+    private void calculateKinematicOffsets() {
+        if (robotPose == null || robotVelocity == null) {
+            kinematicDistanceOffset = 0;
+            kinematicAngularOffsetTicks = 0;
+            return;
+        }
+
+        double targetX = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_X : Constants.TurretConstants.GOAL_BLUE_X;
+        double targetY = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_Y : Constants.TurretConstants.GOAL_BLUE_Y;
+
+        double virtualX = targetX - (robotVelocity.getXComponent() * Constants.TurretConstants.SHOT_TIME_OF_FLIGHT);
+        double virtualY = targetY - (robotVelocity.getYComponent() * Constants.TurretConstants.SHOT_TIME_OF_FLIGHT);
+
+        double realDx = targetX - robotPose.getX();
+        double realDy = targetY - robotPose.getY();
+        double virtualDx = virtualX - robotPose.getX();
+        double virtualDy = virtualY - robotPose.getY();
+
+        double realDistance = Math.hypot(realDx, realDy);
+        double virtualDistance = Math.hypot(virtualDx, virtualDy);
+        kinematicDistanceOffset = virtualDistance - realDistance;
+
+        double realAngle = Math.atan2(realDy, realDx);
+        double virtualAngle = Math.atan2(virtualDy, virtualDx);
+
+        double angleDiff = virtualAngle - realAngle;
+        while (angleDiff > Math.PI) angleDiff -= (2 * Math.PI);
+        while (angleDiff < -Math.PI) angleDiff += (2 * Math.PI);
+
+        kinematicAngularOffsetTicks = Math.toDegrees(angleDiff) * Constants.TurretConstants.TICKS_PER_DEGREE;
     }
 
     public double getDistance() {
@@ -375,7 +422,11 @@ public class Turret {
     }
 
     private double getFlywheelVelocity(double dist) {
-        return (0.0000433916 * Math.pow((dist), 4) - 0.0090065 * Math.pow(dist, 3) + 0.588434 * Math.pow(dist, 2) + (-7.67701 * dist) + 1485.89126);
+        return (-0.0000144121 * Math.pow(dist, 4)
+                + 0.00398607 * Math.pow(dist, 3)
+                - 0.342198 * Math.pow(dist, 2)
+                + (16.43014 * dist)
+                + 1198.24434);
     }
 
     private double getHoodPOS(double dist) {
@@ -392,6 +443,7 @@ public class Turret {
 
     public void updateOffsetTicks(double change) {
         offsetTicks += change;
+        targetCorrectionOffsetTicks += change;
     }
 
     public double getTurretPower() {
@@ -414,4 +466,4 @@ public class Turret {
     public boolean isTurretRotationLocked() {
         return turretModeHold && Math.abs(lastError) <= 10;
     }
-    }
+}

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

@@ -40,8 +40,12 @@ public class Constants {
 
     public static class LEDConstants {
         public static final String LED = "prism";
-        public static final int LAYER_BRIGHTNESS = 50;
 
+        // TODO: Tune these!
+        public static final double COLOR_RED = 0.28;
+        public static final double COLOR_GREEN = 0.45;
+        public static final double COLOR_ORANGE = 0.32;
+        public static final double COLOR_YELLOW = 0.38;
     }
 
 
@@ -93,7 +97,7 @@ public class Constants {
 
         public static final double UNLOCK_POS = 0.82;
 
-        public static double launch_duration = 0.77;
+        public static double launch_duration = 0.9;
 
     }
 
@@ -122,8 +126,8 @@ public class Constants {
 
         public static final DcMotorSimple.Direction TURRET_ROTATION_DIR = DcMotorSimple.Direction.FORWARD;
 
-        public static double SOFT_LIMIT_NEG = -324;
+        public static double SOFT_LIMIT_NEG = -370;
-        public static double SOFT_LIMIT_POS = 195;
+        public static double SOFT_LIMIT_POS = 370;
 
         public static final double HOOD_POS_CLOSE = 0.3;
         public static final double HOOD_POS_FAR = 0.7;
@@ -167,13 +171,15 @@ public class Constants {
         // 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_2 = 45.0;
 
-        public static double ARTIFACT_3 = -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;
+
+        public static double SHOT_TIME_OF_FLIGHT = 0.83;
     }
 
 

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

@@ -12,10 +12,15 @@ import org.firstinspires.ftc.teamcode.subsystems.Intake;
 import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
 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.pedropathing.math.Vector;
+
 import com.qualcomm.robotcore.hardware.VoltageSensor;
+
 public class SOLOTeleOP {
 
     @TeleOp(name = "Red SOLO", group = "FINAL")
@@ -24,6 +29,7 @@ public class SOLOTeleOP {
             super(2);
         }
     }
+
     @TeleOp(name = "Blue SOLO", group = "FINAL")
     public static class BlueSOLO extends BaseSoloTeleOp {
         public BlueSOLO() {
@@ -96,8 +102,9 @@ public class SOLOTeleOP {
 
                 follower.update();
                 Pose currentPose = follower.getPose();
+                Vector currentVelocity = follower.getVelocity();
 
-                stateMachine.update(false, false, currentPose);
+                stateMachine.update(false, false, currentPose, currentVelocity);
 
 
                 handleGlobalLogic();
@@ -173,9 +180,12 @@ public class SOLOTeleOP {
 
         private void handleDriverControls() {
 
-            if (gamepad1.dpad_up) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
+            if (gamepad1.dpad_up)
-            else if (gamepad1.dpad_left) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+                stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
-            else if (gamepad1.dpad_down) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION);
+            else if (gamepad1.dpad_left)
+                stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+            else if (gamepad1.dpad_down)
+                stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION);
 
             if (stateMachine.getCurrentGameState() != GameState.SCORING) {
                 if (gamepad1.right_trigger > 0.1) {
@@ -193,11 +203,12 @@ public class SOLOTeleOP {
             if (gamepad1.yWasPressed()) {
                 isScoringActive = !isScoringActive;
                 if (isScoringActive) stateMachine.setGameState(GameState.SCORING);
-                else if (stateMachine.getCurrentGameState() == GameState.SCORING) stateMachine.setGameState(GameState.IDLE);
+                else if (stateMachine.getCurrentGameState() == GameState.SCORING)
+                    stateMachine.setGameState(GameState.IDLE);
             }
 
-            if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.2);
+            if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(3);
-            if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.2);
+            if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-3);
 
             if (gamepad1.aWasPressed()) {
                 stateMachine.forceLaunchSequence();
@@ -220,10 +231,10 @@ public class SOLOTeleOP {
             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("Turret Ready", stateMachine.getTurret().isTurretReady(false));
             telemetry.addData("Target Tick", stateMachine.getTurret().targetTick);
 
             telemetry.update();
         }
     }
-            }
+}

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

@@ -1,7 +1,9 @@
 package org.firstinspires.ftc.teamcode.teleOp;
 
 import com.pedropathing.geometry.Pose; // ADDED
+import com.pedropathing.math.Vector;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.teamcode.Hware.hwMap;
 import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
@@ -29,11 +31,13 @@ public class StateMachine {
     private final Lift m_lift;
 
     private final LED m_led;
+    private final ElapsedTime ledTimer = new ElapsedTime();
 
     boolean isMotifEdited = false;
     private long lastIndexAllArtifactsTime = 0;
 
     public boolean isIntakeLaunching = false;
+    private boolean isForceLaunch = false;
 
     public StateMachine(hwMap.DriveHwMap h_driveTrain, hwMap.IntakeHwMap h_intake, hwMap.LiftHwMap h_lift, hwMap.TransferHwMap h_transfer, hwMap.TurretHwMap h_turret, hwMap.LedHwMap h_led, int initialMotif, int alliance) {
         this.m_driveTrain = new DriveTrain(h_driveTrain);
@@ -69,6 +73,7 @@ public class StateMachine {
                 m_turret.setTurretState(Turret.TurretState.IDLE);
                 m_intake.setIntakeState(Intake.IntakeState.IDLE);
                 isIntakeLaunching = false;
+                isForceLaunch = false;
                 //m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
                 m_driveTrain.stopAnchor();
                 break;
@@ -78,14 +83,17 @@ public class StateMachine {
         }
     }
 
-    public void update(boolean manualSAM, boolean manualTracking, Pose currentPose) {
+    public void update(boolean manualSAM, boolean manualTracking, Pose currentPose, Vector currentVelocity) {
         m_driveTrain.update();
 
-        m_turret.update(manualTracking, manualSAM, currentPose, m_transfer.getLaunchElapsedTime());
+        m_turret.update(manualTracking, manualSAM, currentPose, currentVelocity, m_transfer.getLaunchElapsedTime());
 
         if (currentGameState == GameState.SCORING) {
+            if (!isForceLaunch) {
                 m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
-            if (m_turret.hasReachedVelocity()) {
+            }
+
+            if (m_turret.hasReachedVelocity() || isForceLaunch) {
                 isIntakeLaunching = true;
                 m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
             }
@@ -182,6 +190,7 @@ public class StateMachine {
 
     public void forceLaunchSequence() {
         setGameState(GameState.SCORING);
+        isForceLaunch = true;
         m_transfer.updateTurretReady(true);
         m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
         isIntakeLaunching = true;

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

@@ -15,6 +15,8 @@ import org.firstinspires.ftc.teamcode.util.FPSCounter;
 import com.qualcomm.hardware.lynx.LynxModule;
 import java.util.List;
 
+import com.pedropathing.math.Vector;
+
 public class finalTeleOp {
 
     @TeleOp(name = "Red Final", group = "FINAL")
@@ -91,7 +93,7 @@ public class finalTeleOp {
 
                 if (stateMachine.getCurrentGameState() != GameState.SCORING) {
                     stateMachine.getTransfer().closeLimiter();
-                } else if (stateMachine.getCurrentGameState() == GameState.SCORING && stateMachine.getTurret().isTurretReady()) {
+                } else if (stateMachine.getCurrentGameState() == GameState.SCORING && stateMachine.getTurret().isTurretReady(false)) {
                     stateMachine.getTransfer().openLimiter();
                 }
 
@@ -101,12 +103,14 @@ public class finalTeleOp {
 
                 follower.update();
                 Pose currentPose = follower.getPose();
+                Vector currentVelocity = follower.getVelocity();
+
 
                 handleGlobalLogic();
                 handleDriverInput();
                 handleOperatorInput();
 
-                stateMachine.update(manualSAM, manualTracking, currentPose);
+                stateMachine.update(manualSAM, manualTracking, currentPose, currentVelocity);
 
                 if (gamepad2.a) {
                     stateMachine.getTransfer().openLimiter();
@@ -273,7 +277,7 @@ public class finalTeleOp {
             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("Turret Ready", stateMachine.getTurret().isTurretReady(false));
             telemetry.addData("Hood Offset", stateMachine.getTurret().offsetHood);
 
 

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

@@ -8,6 +8,7 @@ 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.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.util.Range;
 
@@ -35,17 +36,14 @@ public class flywheelTuning extends OpMode {
     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;
 
@@ -63,6 +61,8 @@ public class flywheelTuning extends OpMode {
         flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
         flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
 
+        flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(2, 0, 11, 14.3));
+        flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(2, 0, 11, 14.3));
         hoodServo = hardwareMap.servo.get("hoodservo");
         hoodServo.setDirection(Servo.Direction.REVERSE);
 
@@ -184,7 +184,9 @@ public class flywheelTuning extends OpMode {
             intake.setPower(0);
         }
 
-        hoodServo.setPosition(Range.clip(applyHood, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+        double recoilOffset = (currentVel - applyVel) * -0.000185;
+
+        hoodServo.setPosition(Range.clip(applyHood + recoilOffset, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
 
 
         // --- TELEMETRY ---
@@ -213,19 +215,19 @@ public class flywheelTuning extends OpMode {
     }
 
     private double getTrigBasedDistance(double targetOffsetAngleVertical) {
-        return (29.5-12.75) / Math.tan(Math.toRadians(19.9 + targetOffsetAngleVertical));
+        return (29.5-12.75) / Math.tan(Math.toRadians(19.8 + targetOffsetAngleVertical));
         //      ^ GOAL - CAMERA HEIGHT                ^ Offset Degrees
     }
 
     private double getFlywheelVelocity(double dist) {
-        return (0.0000119972 * Math.pow(dist, 4)
+        return (-0.0000144121 * Math.pow(dist, 4)
-                - 0.00249603 * Math.pow(dist, 3)
+                + 0.00398607 * Math.pow(dist, 3)
-                + 0.179513 * Math.pow(dist, 2)
+                - 0.342198 * Math.pow(dist, 2)
-                + (0.0519688 * dist)
+                + (16.43014 * dist)
-                + 1489.8908);
+                + 1198.24434);
     }
 
     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);
+        return (((3.55794 * Math.pow(10, -8)) * Math.pow(dist, 4)) + (-0.00000995887 * Math.pow(dist, 3)) - (0.000924672 * Math.pow(dist, 2)) + (-0.037262 * dist) + 0.943067);
     }
 }

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

@@ -0,0 +1,71 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+import com.qualcomm.robotcore.hardware.PwmControl;
+import com.bylazar.configurables.annotations.Configurable;
+
+@Configurable
+@TeleOp(name = "GoBilda RGB Indicator Test", group = "Tests")
+public class ledTest extends OpMode {
+
+    // Declare the light as a ServoImplEx
+    private ServoImplEx rgbLight;
+
+    public static double LED1 = 0.33;
+    public static double LED2 = 0.66;
+    public static double LED3 = 0.99;
+
+    private boolean lightOn = false;
+
+    private double lightPosition = 0;
+
+
+
+
+    @Override
+    public void init() {
+        rgbLight = hardwareMap.get(ServoImplEx.class, "prism");
+
+        rgbLight.setPwmRange(new PwmControl.PwmRange(500, 2500));
+
+        telemetry.addData("Status", "Initialized. Ready to test colors.");
+    }
+
+    @Override
+    public void loop() {
+
+        if (gamepad1.psWasPressed()) {
+            lightOn = !lightOn;
+        }
+
+        if (gamepad1.a) {
+            lightPosition = LED1;
+        }
+        else if (gamepad1.b) {
+            lightPosition = 0;
+        }
+        else if (gamepad1.x) {
+            lightPosition = LED2;
+        }
+        else if (gamepad1.y) {
+            lightPosition = LED3;
+        }
+
+        if (gamepad1.right_trigger > 0.1) {
+            lightPosition = gamepad1.right_trigger;
+        }
+
+        if (lightOn) {
+            rgbLight.setPwmEnable();
+        } else {
+            rgbLight.setPwmDisable();
+        }
+
+        rgbLight.setPosition(lightPosition);
+
+        telemetry.addData("Current Light Position", rgbLight.getPosition());
+        telemetry.update();
+    }
+}