night before

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,12 +39,24 @@ public class hwMap {
     }
 
     public static class LedHwMap {
-        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led1;
+        private ServoImplEx led;
-        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led2;
 
         public LedHwMap(HardwareMap hardwareMap) {
-            /*led1 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led1");
+
-            led2 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led2");*/
+            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();
+            }
         }
     }
 
@@ -89,7 +105,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 +266,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(10, 0, 11, 14.3);
 
         public TurretHwMap(HardwareMap hardwareMap) {
             noTargetTimer.reset();
@@ -283,12 +299,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

@@ -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;
@@ -30,80 +31,58 @@ public class Auton {
 
         @Override
         public void buildPaths() {
-            // Start Pose
             startPose = new Pose(59, 9, Math.toRadians(90));
             follower.setStartingPose(startPose);
 
-            // Poses
-            Pose scorePosePreload = new Pose(59, 15, Math.toRadians(118));
-            Pose scorePoseGeneric = new Pose(59, 15, Math.toRadians(118));
-            Pose set1Inter = new Pose(44, 36, Math.toRadians(180));
-            Pose set1Pick = new Pose(17, 36, Math.toRadians(180));
-            Pose set2Inter = new Pose(41, 60, Math.toRadians(180));
-            Pose set2Pick = new Pose(16, 63, Math.toRadians(180));
-            Pose set2Control1 = new Pose(17.711, 56.837);
-            Pose set2Control2 = new Pose(25.306, 62.677);
-            Pose scoreSet1Control = new Pose(36.621, 19.900);
-
-            // Corner Specific Poses
-            Pose cornerInter = new Pose(9.444, 24.561, Math.toRadians(-90));
-            Pose cornerInterControl = new Pose(37.157, 26.830);
-            Pose cornerArtifact1 = new Pose(12.598, 18.008, Math.toRadians(-135));
-            Pose cornerArtifact1Control = new Pose(11.997, 24.364);
-            Pose cornerArtifact2 = new Pose(10.551, 13.380, Math.toRadians(-160));
-            Pose cornerArtifact3 = new Pose(9.712, 8.993, Math.toRadians(-160));
-            Pose parkPose = new Pose(54.614, 19.545, Math.toRadians(135));
-
             // --- PATH DEFINITIONS ---
             scorePreloadPath = follower.pathBuilder()
-                    .addPath(new BezierLine(startPose, scorePosePreload))
+                    .addPath(new BezierLine(new Pose(59, 9), new Pose(59, 15)))
-                    .setLinearHeadingInterpolation(startPose.getHeading(), scorePosePreload.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(118))
                     .build();
 
             grabSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePosePreload, set1Inter))
+                    .addPath(new BezierLine(new Pose(59, 15), new Pose(44, 36)))
-                    .setLinearHeadingInterpolation(scorePosePreload.getHeading(), Math.toRadians(180))
+                    .setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180))
-                    .addPath(new BezierLine(set1Inter, set1Pick))
+                    .addPath(new BezierLine(new Pose(44, 36), new Pose(17, 36)))
                     .setConstantHeadingInterpolation(Math.toRadians(180))
                     .build();
 
             scoreSet1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set1Pick, scoreSet1Control, scorePoseGeneric))
+                    .addPath(new BezierCurve(new Pose(17, 36), new Pose(36.621, 19.900), new Pose(59, 15)))
-                    .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118))
                     .build();
 
             grabSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePoseGeneric, set2Inter))
+                    .addPath(new BezierLine(new Pose(59, 15), new Pose(41, 60)))
-                    .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), Math.toRadians(180))
+                    .setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180))
-                    .addPath(new BezierCurve(set2Inter, set2Control1, set2Control2, set2Pick))
+                    .addPath(new BezierCurve(new Pose(41, 60), new Pose(17.711, 56.837), new Pose(25.306, 62.677), new Pose(16, 63)))
                     .setConstantHeadingInterpolation(Math.toRadians(180))
                     .build();
 
             scoreSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set2Pick, scorePoseGeneric))
+                    .addPath(new BezierLine(new Pose(16, 63), new Pose(59, 15)))
-                    .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118))
                     .build();
 
-            // Corner Sequence
             grabEndGamePath = follower.pathBuilder()
-                    .addPath(new BezierCurve(scorePoseGeneric, cornerInterControl, cornerInter))
+                    .addPath(new BezierCurve(new Pose(59, 15), new Pose(37.157, 26.830), new Pose(10.400, 8)))
-                    .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), Math.toRadians(-90))
+                    .setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180))
-                    .addPath(new BezierCurve(cornerInter, cornerArtifact1Control, cornerArtifact1))
-                    .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-135))
-                    .addPath(new BezierLine(cornerArtifact1, cornerArtifact2))
-                    .setLinearHeadingInterpolation(Math.toRadians(-135), Math.toRadians(-160))
-                    .addPath(new BezierLine(cornerArtifact2, cornerArtifact3))
-                    .setConstantHeadingInterpolation(Math.toRadians(-160))
                     .build();
 
             scoreEndGamePath = follower.pathBuilder()
-                    .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric))
+                    .addPath(new BezierLine(new Pose(10.400, 8), new Pose(59, 15)))
-                    .setLinearHeadingInterpolation(Math.toRadians(-160), scorePoseGeneric.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118))
                     .build();
 
             parkPath = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePoseGeneric, parkPose))
+                    .addPath(new BezierCurve(new Pose(59, 15), new Pose(37.181, 26.706), new Pose(10.400, 27)))
-                    .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), parkPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(180))
+                    .addPath(new BezierLine(new Pose(10.400, 27), new Pose(10.439, 17.195)))
+                    .setConstantHeadingInterpolation(Math.toRadians(180))
+                    .addPath(new BezierLine(new Pose(10.439, 17.195), new Pose(59, 15)))
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(135))
+                    .addPath(new BezierLine(new Pose(59, 15), new Pose(54.614, 19.545)))
+                    .setConstantHeadingInterpolation(Math.toRadians(135))
                     .build();
         }
     }
@@ -116,110 +95,82 @@ 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
-            Pose preloadScorePose = new Pose(54, 90, Math.toRadians(144));
-            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 gate1ScorePose = new Pose(54, 90, Math.toRadians(144));
-            Pose gate2PickPose = new Pose(10.652, 61.079, Math.toRadians(140));
-            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));
-            Pose set3ScorePose = new Pose(54, 90, Math.toRadians(144));
-            Pose set1DrivePose = new Pose(39, 84, Math.toRadians(180));
-            Pose set1PickPose = new Pose(29.689, 106.279, Math.toRadians(144));
-            Pose set1ScorePose = new Pose(44, 100, Math.toRadians(148));
-            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);
-            Pose gate2PickControl = new Pose(42.081, 61.914);
-            Pose gate2ScoreControl = new Pose(42.318, 61.514);
-            Pose set3PickControl = new Pose(29.622, 37.842);
-            Pose set3ScoreControl = new Pose(9.594, 52.541);
-            Pose set1PickControl = new Pose(14.863, 77.692);
-
             scorePreloadPath = follower.pathBuilder()
-                    .addPath(new BezierCurve(startPose, preloadControl, preloadScorePose))
+                    .addPath(new BezierLine(new Pose(21, 124), new Pose(54, 90)))
-                    .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(143.5), Math.toRadians(144))
                     .build();
 
             driveToSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(preloadScorePose, set2DrivePose))
+                    .addPath(new BezierLine(new Pose(54, 90), new Pose(41.758, 61.439)))
-                    .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(180))
                     .build();
 
             grabSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set2DrivePose, set2PickPose))
+                    .addPath(new BezierLine(new Pose(41.758, 61.439), new Pose(16, 59.830)))
-                    .setTangentHeadingInterpolation()
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180))
                     .build();
 
             scoreSet2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose))
+                    .addPath(new BezierCurve(new Pose(16, 59.830), new Pose(55.542, 82.280), new Pose(54, 90)))
-                    .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(144))
                     .build();
 
             grabGate1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set2ScorePose, gate1PickControl, gate1PickPose))
+                    .addPath(new BezierCurve(new Pose(54, 90), new Pose(40.119, 55.595), new Pose(12, 58.3)))
-                    .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(138))
                     .build();
 
             scoreGate1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose))
+                    .addPath(new BezierCurve(new Pose(12, 58.3), new Pose(41.918, 62.187), new Pose(54, 90)))
-                    .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144))
                     .build();
 
             grabGate2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose))
+                    .addPath(new BezierCurve(new Pose(54, 90), new Pose(42.081, 61.914), new Pose(12, 58.3)))
-                    .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(138))
                     .build();
 
             scoreGate2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose))
+                    .addPath(new BezierCurve(new Pose(12, 58.3), new Pose(44, 62), new Pose(54, 90)))
-                    .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144))
                     .build();
 
             driveToSet3Path = follower.pathBuilder()
-                    .addPath(new BezierLine(gate2ScorePose, set3DrivePose))
+                    .addPath(new BezierLine(new Pose(54, 90), new Pose(38.515, 47.156)))
-                    .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(220))
                     .build();
 
             grabSet3Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose))
+                    .addPath(new BezierCurve(new Pose(38.515, 47.156), new Pose(29.622, 37.842), new Pose(12, 36)))
-                    .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(220), Math.toRadians(185))
                     .build();
 
             scoreSet3Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose))
+                    .addPath(new BezierCurve(new Pose(12, 36), new Pose(9.594, 52.541), new Pose(54, 90)))
-                    .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(185), Math.toRadians(144))
                     .build();
 
             driveToSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set3ScorePose, set1DrivePose))
+                    .addPath(new BezierLine(new Pose(54, 90), new Pose(39, 84)))
-                    .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(180))
                     .build();
 
             grabSet1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose))
+                    .addPath(new BezierCurve(new Pose(39, 84), new Pose(14.863, 77.692), new Pose(29.689, 106.279)))
-                    .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(144))
                     .build();
 
             scoreSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set1PickPose, set1ScorePose))
+                    .addPath(new BezierLine(new Pose(29.689, 106.279), new Pose(44, 100)))
-                    .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(148))
                     .build();
 
             parkPath = follower.pathBuilder()
-                    .addPath(new BezierLine(set1ScorePose, parkTargetPose))
+                    .addPath(new BezierLine(new Pose(44, 100), new Pose(36.219, 93.170)))
-                    .setConstantHeadingInterpolation(set1ScorePose.getHeading())
+                    .setConstantHeadingInterpolation(Math.toRadians(148))
                     .build();
         }
     }
@@ -239,74 +190,54 @@ public class Auton {
             startPose = new Pose(85, 9, Math.toRadians(90));
             follower.setStartingPose(startPose);
 
-            Pose scorePosePreload = new Pose(85, 15, Math.toRadians(62));
-            Pose scorePoseGeneric = new Pose(85, 15, Math.toRadians(62));
-            Pose set1Inter = new Pose(100, 36, Math.toRadians(0));
-            Pose set1Pick = new Pose(127, 36, Math.toRadians(0));
-            Pose set2Inter = new Pose(103, 60, Math.toRadians(0));
-            Pose set2Pick = new Pose(128, 63, Math.toRadians(0));
-            Pose set2Control1 = new Pose(126.289, 56.837);
-            Pose set2Control2 = new Pose(118.694, 62.677);
-            Pose scoreSet1Control = new Pose(107.379, 19.900);
-
-            // Corner Poses
-            Pose cornerInter = new Pose(134.556, 24.561, Math.toRadians(270));
-            Pose cornerInterControl = new Pose(106.843, 26.830);
-            Pose cornerArtifact1 = new Pose(131.402, 18.008, Math.toRadians(315));
-            Pose cornerArtifact1Control = new Pose(132.003, 24.364);
-            Pose cornerArtifact2 = new Pose(133.449, 13.380, Math.toRadians(340));
-            Pose cornerArtifact3 = new Pose(134.288, 8.993, Math.toRadians(340));
-            Pose parkPose = new Pose(89.386, 19.545, Math.toRadians(45));
-
             scorePreloadPath = follower.pathBuilder()
-                    .addPath(new BezierLine(startPose, scorePosePreload))
+                    .addPath(new BezierLine(new Pose(85, 9), new Pose(85, 15)))
-                    .setLinearHeadingInterpolation(startPose.getHeading(), Math.toRadians(62))
+                    .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(62))
                     .build();
 
             grabSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePosePreload, set1Inter))
+                    .addPath(new BezierLine(new Pose(85, 15), new Pose(100, 36)))
                     .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
-                    .addPath(new BezierLine(set1Inter, set1Pick))
+                    .addPath(new BezierLine(new Pose(100, 36), new Pose(127, 36)))
                     .setConstantHeadingInterpolation(Math.toRadians(0))
                     .build();
 
             scoreSet1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set1Pick, scoreSet1Control, scorePoseGeneric))
+                    .addPath(new BezierCurve(new Pose(127, 36), new Pose(107.379, 19.900), new Pose(85, 15)))
                     .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
                     .build();
 
             grabSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePoseGeneric, set2Inter))
+                    .addPath(new BezierLine(new Pose(85, 15), new Pose(103, 60)))
                     .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
-                    .addPath(new BezierCurve(set2Inter, set2Control1, set2Control2, set2Pick))
+                    .addPath(new BezierCurve(new Pose(103, 60), new Pose(126.289, 56.837), new Pose(118.694, 62.677), new Pose(128, 63)))
                     .setConstantHeadingInterpolation(Math.toRadians(0))
                     .build();
 
             scoreSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set2Pick, scorePoseGeneric))
+                    .addPath(new BezierLine(new Pose(128, 63), new Pose(85, 15)))
                     .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
                     .build();
 
-            // Corner Sequence
             grabEndGamePath = follower.pathBuilder()
-                    .addPath(new BezierCurve(scorePoseGeneric, cornerInterControl, cornerInter))
+                    .addPath(new BezierCurve(new Pose(85, 15), new Pose(106.843, 26.830), new Pose(133.600, 8)))
-                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(270))
+                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
-                    .addPath(new BezierCurve(cornerInter, cornerArtifact1Control, cornerArtifact1))
-                    .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(315))
-                    .addPath(new BezierLine(cornerArtifact1, cornerArtifact2))
-                    .setLinearHeadingInterpolation(Math.toRadians(315), Math.toRadians(340))
-                    .addPath(new BezierLine(cornerArtifact2, cornerArtifact3))
-                    .setConstantHeadingInterpolation(Math.toRadians(340))
                     .build();
 
             scoreEndGamePath = follower.pathBuilder()
-                    .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric))
+                    .addPath(new BezierLine(new Pose(133.600, 8), new Pose(85, 15)))
-                    .setLinearHeadingInterpolation(Math.toRadians(340), Math.toRadians(62))
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
                     .build();
 
             parkPath = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePoseGeneric, parkPose))
+                    .addPath(new BezierCurve(new Pose(85, 15), new Pose(106.819, 26.706), new Pose(133.600, 27)))
-                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(45))
+                    .setLinearHeadingInterpolation(Math.toRadians(45), Math.toRadians(0))
+                    .addPath(new BezierLine(new Pose(133.600, 27), new Pose(133.561, 17.195)))
+                    .setConstantHeadingInterpolation(Math.toRadians(0))
+                    .addPath(new BezierLine(new Pose(133.561, 17.195), new Pose(85, 15)))
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(45))
+                    .addPath(new BezierLine(new Pose(85, 15), new Pose(89.386, 19.545)))
+                    .setConstantHeadingInterpolation(Math.toRadians(45))
                     .build();
         }
     }
@@ -319,110 +250,82 @@ public class Auton {
 
         @Override
         public void buildPaths() {
-            startPose = new Pose(112.5, 137, Math.toRadians(0));
+            startPose = new Pose(123, 124, Math.toRadians(36.5));
             follower.setStartingPose(startPose);
 
-            // Waypoints
-            Pose preloadScorePose = new Pose(90, 90, Math.toRadians(36));
-            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 gate1ScorePose = new Pose(90, 90, Math.toRadians(36));
-            Pose gate2PickPose = new Pose(133.348, 61.079, Math.toRadians(40));
-            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));
-            Pose set3ScorePose = new Pose(90, 90, Math.toRadians(36));
-            Pose set1DrivePose = new Pose(105, 84, Math.toRadians(0));
-            Pose set1PickPose = new Pose(114.311, 106.279, Math.toRadians(36));
-            Pose set1ScorePose = new Pose(100, 100, Math.toRadians(32));
-            Pose parkTargetPose = new Pose(107.781, 93.170, Math.toRadians(32));
-
-            // Controls
-            Pose preloadControl = new Pose(116.600, 115.541);
-            Pose set2ScoreControl = new Pose(93.614, 78.923);
-            Pose gate1PickControl = new Pose(103.881, 55.595);
-            Pose gate1ScoreControl = new Pose(102.082, 62.187);
-            Pose gate2PickControl = new Pose(101.919, 61.914);
-            Pose gate2ScoreControl = new Pose(101.682, 61.514);
-            Pose set3PickControl = new Pose(114.378, 37.842);
-            Pose set3ScoreControl = new Pose(134.406, 52.541);
-            Pose set1PickControl = new Pose(129.137, 77.692);
-
             scorePreloadPath = follower.pathBuilder()
-                    .addPath(new BezierCurve(startPose, preloadControl, preloadScorePose))
+                    .addPath(new BezierLine(new Pose(123, 124), new Pose(90, 90)))
-                    .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(36.5), Math.toRadians(36))
                     .build();
 
             driveToSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(preloadScorePose, set2DrivePose))
+                    .addPath(new BezierLine(new Pose(90, 90), new Pose(102.242, 61.439)))
-                    .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(0))
                     .build();
 
             grabSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set2DrivePose, set2PickPose))
+                    .addPath(new BezierLine(new Pose(102.242, 61.439), new Pose(128, 59.830)))
-                    .setTangentHeadingInterpolation()
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0))
                     .build();
 
             scoreSet2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose))
+                    .addPath(new BezierCurve(new Pose(128, 59.830), new Pose(88.458, 82.280), new Pose(90, 90)))
-                    .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(36))
                     .build();
 
             grabGate1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set2ScorePose, gate1PickControl, gate1PickPose))
+                    .addPath(new BezierCurve(new Pose(90, 90), new Pose(103.881, 55.595), new Pose(132, 58.3)))
-                    .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(42))
                     .build();
 
             scoreGate1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose))
+                    .addPath(new BezierCurve(new Pose(132, 58.3), new Pose(102.082, 62.187), new Pose(90, 90)))
-                    .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36))
                     .build();
 
             grabGate2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose))
+                    .addPath(new BezierCurve(new Pose(90, 90), new Pose(101.919, 61.914), new Pose(132, 58.3)))
-                    .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(42))
                     .build();
 
             scoreGate2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose))
+                    .addPath(new BezierCurve(new Pose(132, 58.3), new Pose(100, 62), new Pose(90, 90)))
-                    .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36))
                     .build();
 
             driveToSet3Path = follower.pathBuilder()
-                    .addPath(new BezierLine(gate2ScorePose, set3DrivePose))
+                    .addPath(new BezierLine(new Pose(90, 90), new Pose(105.485, 47.156)))
-                    .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(-40))
                     .build();
 
             grabSet3Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose))
+                    .addPath(new BezierCurve(new Pose(105.485, 47.156), new Pose(114.378, 37.842), new Pose(132, 36)))
-                    .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(-40), Math.toRadians(-5))
                     .build();
 
             scoreSet3Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose))
+                    .addPath(new BezierCurve(new Pose(132, 36), new Pose(134.406, 52.541), new Pose(90, 90)))
-                    .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(-5), Math.toRadians(36))
                     .build();
 
             driveToSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set3ScorePose, set1DrivePose))
+                    .addPath(new BezierLine(new Pose(90, 90), new Pose(105, 84)))
-                    .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(0))
                     .build();
 
             grabSet1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose))
+                    .addPath(new BezierCurve(new Pose(105, 84), new Pose(129.137, 77.692), new Pose(114.311, 106.279)))
-                    .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(36))
                     .build();
 
             scoreSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set1PickPose, set1ScorePose))
+                    .addPath(new BezierLine(new Pose(114.311, 106.279), new Pose(100, 100)))
-                    .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading())
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(32))
                     .build();
 
             parkPath = follower.pathBuilder()
-                    .addPath(new BezierLine(set1ScorePose, parkTargetPose))
+                    .addPath(new BezierLine(new Pose(100, 100), new Pose(107.781, 93.170)))
-                    .setConstantHeadingInterpolation(set1ScorePose.getHeading())
+                    .setConstantHeadingInterpolation(Math.toRadians(32))
                     .build();
         }
     }
@@ -442,7 +345,7 @@ public class Auton {
         protected final boolean isNet;
         protected final boolean useTimer;
 
-        public static double timerTimeout = 4;
+        public static double timerTimeout = 2.8;
         public static double gateWaitTime = 3.0;
 
         protected Pose startPose;
@@ -531,19 +434,22 @@ public class Auton {
                 telemetry.update();
             }
 
+            follower.update();
+
+            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);
 
             while (opModeIsActive()) {
-
                 for (LynxModule hub : allHubs) {
                     hub.clearBulkCache();
                 }
                 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());
@@ -630,7 +536,7 @@ public class Auton {
                     if (!follower.isBusy()) {
                         systems.setAutonState(pedroStateMachine.AutonState.SCORING);
                         if (checkScoringCondition()) {
-                            setPathState(PathState.DRIVE_TO_PARK);
+                            setPathState(PathState.DRIVE_TO_ENDGAME_COLLECTION);
                         }
                     }
                     break;

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
@@ -12,6 +13,7 @@ import org.firstinspires.ftc.teamcode.subsystems.TransferSys.TransferState;
 import org.firstinspires.ftc.teamcode.subsystems.Intake.IntakeState;
 
 import org.firstinspires.ftc.teamcode.util.AutoTransfer;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
 
 public class pedroStateMachine {
 
@@ -27,6 +29,10 @@ public class pedroStateMachine {
     private final Turret m_turret;
 
 
+    private boolean PIDFShooting = false;
+    private boolean lastPIDFShooting = false;
+
+
 
     private int alliance = 0;
 
@@ -35,6 +41,9 @@ public class pedroStateMachine {
         this.m_transfer = new TransferSys(h_transfer);
         this.m_turret = new Turret(h_turret);
 
+        m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL);
+        m_turret.setLaunching(false);
+
         setAutonState(AutonState.IDLE);
         AutoTransfer.reset();
     }
@@ -57,26 +66,28 @@ public class pedroStateMachine {
         switch (oldState) {
             case INTAKING:
                 m_intake.setIntakeState(IntakeState.IDLE);
-                // Ensure gate is closed when we stop intaking to hold rings
                 m_transfer.setTransferState(TransferState.IDLE);
                 break;
             case SCORING:
                 m_transfer.setTransferState(TransferState.IDLE);
                 m_turret.setTurretState(Turret.TurretState.IDLE);
                 m_intake.setIntakeState(Intake.IntakeState.IDLE);
+                PIDFShooting = false;
+                m_turret.setLaunching(false);
+                m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL);
                 break;
         }
     }
 
+
     private void handleStateEntry(AutonState newState) {
         switch (newState) {
             case INTAKING:
                 m_intake.setIntakeState(IntakeState.INTAKE);
-                m_turret.setTurretState(TurretState.INTAKING); // Optional: position turret for intake stability
+                m_turret.setTurretState(TurretState.INTAKING);
-                m_transfer.closeLimiter(); // Ensure rings don't fly out back
+                m_transfer.closeLimiter();
                 break;
             case SCORING:
-                m_intake.setIntakeState(IntakeState.LAUNCH); // Feed rings
                 m_turret.setTurretState(TurretState.LAUNCH);
                 m_transfer.setTransferState(TransferState.LAUNCHING);
                 break;
@@ -88,15 +99,32 @@ 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());
+
+            if (m_turret.hasReachedVelocity() || PIDFShooting) {
+                m_intake.setIntakeState(IntakeState.LAUNCH);
+                PIDFShooting = true;
+            }
+        } else {
+            PIDFShooting = false;
         }
 
-        m_transfer.update();
+        if (PIDFShooting != lastPIDFShooting) {
+            if (PIDFShooting) {
+                m_turret.setLaunching(true); // Freeze Vision Offsets
+                m_turret.setPIDF(Constants.TurretConstants.PIDF_Shooting); // High-power recovery PID
+            } else {
+                m_turret.setLaunching(false); // Resume Vision Tracking
+                m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL); // Precise tracking PID
+            }
+        }
+        lastPIDFShooting = PIDFShooting;
 
+        m_transfer.update();
 
         if (currentState == AutonState.SCORING) {
             if (m_transfer.getTransferState() != TransferSys.TransferState.LAUNCHING) {

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/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,76 +1,50 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
 import org.firstinspires.ftc.teamcode.Hware.hwMap;
-import org.firstinspires.ftc.teamcode.Prism.Color;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
-import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
-import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
 
 public class LED {
     private final hwMap.LedHwMap hardware;
 
-    // Track states to prevent spamming the I2C bus
+    private int currentLedState = -1;
-    private int currentLed1State = -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 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;
-
+        turnOn();
-        solidRed.setBrightness(50);
-        solidOrange.setBrightness(50);
-        solidGreen.setBrightness(50);
-        solidBlue.setBrightness(50);
-        solidPurple.setBrightness(50);
     }
 
-    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.setLedPos(Constants.LEDConstants.COLOR_RED);
-            } else if (newLed1State == 1) {
+            } else if (newLedState == 1) {
-                hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
+                hardware.setLedPos(Constants.LEDConstants.COLOR_GREEN);
-            } else if (newLed1State == 2) {
+            } else if (newLedState == 2) {
-                hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
+                hardware.setLedPos(Constants.LEDConstants.COLOR_ORANGE);
+            } else if (newLedState == 3) {
+                hardware.setLedPos(Constants.LEDConstants.COLOR_YELLOW);
             }
-            currentLed1State = newLed1State;
-        }
 
-        int newLed2State = 0;
+            currentLedState = newLedState;
-        if (hasTarget && isAnchorActive) {
-            newLed2State = 3;
-        } else if (hasTarget) {
-            newLed2State = 1;
-        } else if (isAnchorActive) {
-            newLed2State = 2;
         }
+    }
 
-        if (newLed2State != currentLed2State) {
+    public void turnOff() {
-            if (newLed2State == 0) {
+        hardware.setLedOn(false);
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
+        currentLedState = -1;
-            } else if (newLed2State == 1) {
+    }
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
+    
-            } else if (newLed2State == 2) {
+    public void turnOn() {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidBlue);
+        hardware.setLedOn(true);
-            } 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,15 +1,15 @@
 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;
 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;
@@ -27,13 +27,28 @@ 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
+
+    private double distanceOffset = 0.0;
+    private double visionDistanceOffset = 0.0;
+
+    public double targetTick;
 
     private final ElapsedTime loopTimer = new ElapsedTime();
+    private final ElapsedTime stateTimer = new ElapsedTime();
+
+    public boolean turretModeHold = false;
+
+    public boolean lastTurretModeHold = false;
+
+    private boolean isLaunching = false;
 
     public enum TurretState {
         LAUNCH,
@@ -41,21 +56,29 @@ public class Turret {
         EXTAKE,
         INTAKING
     }
+
     private TurretState currentState = TurretState.IDLE;
 
     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) {
-        this.alliance = alliance;
+        if (this.alliance != alliance) {
-        if (alliance == 2) {
+            this.alliance = alliance;
-            hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
+            targetCorrectionOffsetTicks = 0; // Reset correction when alliance changes
-        } else if(alliance == 1) {
+            offsetTicks = 0; // Reset manual offset when alliance changes
-            hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_BLUE_TARGET);
+            if (alliance == 2) {
-        } else {
+                hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
-            hardware.setPipeline(0);
+            } else if (alliance == 1) {
+                hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_BLUE_TARGET);
+            } else {
+                hardware.setPipeline(0);
+            }
         }
     }
 
@@ -68,21 +91,24 @@ public class Turret {
     }
 
     public void setTurretState(TurretState state) {
-        this.currentState = state;
+        if (this.currentState != state) {
+            this.currentState = state;
+            stateTimer.reset();
 
-        switch (state) {
+            switch (state) {
-            case IDLE:
+                case IDLE:
-                hardware.turretOff();
+                    hardware.turretOff();
-                hardware.stopTurretRotation();
+                    hardware.stopTurretRotation();
-                integralSum = 0;
+                    integralSum = 0;
-                lastError = 0;
+                    lastError = 0;
-                break;
+                    break;
-            case EXTAKE:
+                case EXTAKE:
-                hardware.setTurretPower(Constants.TurretConstants.EXTAKE_POWER);
+                    hardware.setTurretPower(Constants.TurretConstants.EXTAKE_POWER);
-                break;
+                    break;
-            case INTAKING:
+                case INTAKING:
-                hardware.setTurretPower(Constants.TurretConstants.INTAKE_POWER);
+                    hardware.setTurretPower(Constants.TurretConstants.INTAKE_POWER);
-                break;
+                    break;
+            }
         }
     }
 
@@ -95,17 +121,17 @@ public class Turret {
         return hardware.hasTarget();
     }
 
-    public void update(boolean manualTracking, boolean manualSAM, Pose currentPose) {
+    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 + distanceOffset;
 
         if (!manualTracking) {
             calculateHybridTarget(currentPose, currentDegrees, rawTicks);
@@ -115,39 +141,93 @@ public class Turret {
         }
 
         if (currentState == TurretState.LAUNCH) {
-            handleLaunchLogic();
+            handleLaunchLogic(elapsedTime);
+        } else {
+            idleLaunch();
         }
     }
 
-    public void updateAuton(int alliance, Pose currentPose, double offset) {
+
-        updatePose(currentPose);
+    public void idleLaunch() {
+        targetVelocity = 1600;
+        hardware.setTurretVelocity(targetVelocity);
+
+        double calculatedHoodPos = getHoodPOS(65);
+        setHoodPos(calculatedHoodPos);
+    }
+
+    public void updateAuton(int alliance, Pose currentPose, Vector currentVelocity, double offset) {
+        updatePose(currentPose, currentVelocity); // Updated
         double dt = loopTimer.seconds();
         loopTimer.reset();
         if (dt < 0.001) dt = 0.001;
 
         setAlliance(alliance);
 
+        if (currentState == TurretState.IDLE) {
+            hardware.stopTurretRotation();
+            return;
+        }
+
         double rawTicks = hardware.getTurretRotationPosition();
         double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
 
-        distance = getDistance();
+        distance = getDistance() + kinematicDistanceOffset;
 
         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();
+
+            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.000385;
+
+            setHoodPos(calculatedHoodPos + recoilOffset - 0.03);
+        } else {
+            hardware.setTurretVelocity(0);
+        }
+    }
+
+    private void handleLaunchLogicOffset(double offset) {
+        if (distance > 0) {
+            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()[1];
+
+            double recoilOffset = (currentVel - targetVelocity) * -0.000385;
 
             setHoodPos(calculatedHoodPos + recoilOffset);
         } else {
@@ -155,36 +235,90 @@ public class Turret {
         }
     }
 
-    private void handleLaunchLogic(double offset) {
-        if (distance > 0) {
-            targetVelocity = getFlywheelVelocity(distance);
-            hardware.setTurretVelocity(targetVelocity);
 
-            double calculatedHoodPos = getHoodPOS(distance);
+    /*private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) {
-            double currentVel = hardware.getFlywheelVelocities()[0];
-            double dropThreshold = targetVelocity * 0.95;
-            double recoilOffset = 0.0;
-
-            recoilOffset = (currentVel - targetVelocity) * 0.000004;
-
-            setHoodPos(calculatedHoodPos + recoilOffset);
-        } else {
-            hardware.setTurretVelocity(0);
-        }
-    }
-
-    private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) {
         double odoTargetTicks = calculateOdometryTargetTicks(robotPose);
         LLResult result = hardware.getLimelightResult();
 
         if (result != null && result.isValid()) {
-            double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
+            double tx = result.getTx();
-            targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - Constants.TurretConstants.OFFSET_SMOOTHING))
+            double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER);
-                    + (rawErrorTicks * Constants.TurretConstants.OFFSET_SMOOTHING);
+            double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES;
+            double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE;
+
+            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 = visionTargetWithManualOffset - 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;
+    }*/ // <-- Old Logic (relies on both LL and ODO [Even while shooting]
+    private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) {
+        double odoTargetTicks = calculateOdometryTargetTicks(robotPose);
+
+        boolean isActivelyShooting = isLaunching();
+
+        if (!isActivelyShooting) {
+            LLResult result = hardware.getLimelightResult();
+
+            if (result != null && result.isValid()) {
+                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;
+
+                visionTargetTicks += kinematicAngularOffsetTicks;
+
+                double visionTargetWithManualOffset = visionTargetTicks + offsetTicks;
+                double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+                double correctionError = visionTargetWithManualOffset - currentFusedTarget;
+
+                double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE;
+                while (correctionError > (ticksPerRev / 2)) correctionError -= ticksPerRev;
+                while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev;
+
+                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) {
@@ -201,86 +335,138 @@ 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 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;
 
-        if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) {
+        double entryThreshold = 10;
-            integralSum += (error * dt);
+        double exitThreshold = 30;
-        } else {
+
-            integralSum = 0;
+        if (!turretModeHold && Math.abs(error) <= entryThreshold) {
+            turretModeHold = true;
+        } else if (turretModeHold && Math.abs(error) > exitThreshold) {
+            turretModeHold = false;
         }
 
-        double integralTerm = Range.clip(Constants.TurretConstants.KI * integralSum, -Constants.TurretConstants.MAX_INTEGRAL, Constants.TurretConstants.MAX_INTEGRAL);
+        if (turretModeHold) {
-        double output = (Constants.TurretConstants.KP * error) + integralTerm + (Constants.TurretConstants.KD * derivative);
+            if (lastTurretModeHold != turretModeHold) {
-        output = Range.clip(output, -Constants.TurretConstants.MAX_POWER, Constants.TurretConstants.MAX_POWER);
+                hardware.runToPos((int) fusedTargetTicks); // Set target first
+                hardware.enableTurretHold();
+                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();
+            }
 
-        hardware.setTurretRotationPower(output);
+            if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) {
-        turretPowerRotation = output;
+                integralSum += (error * dt);
+            } else {
+                integralSum = 0;
+            }
+
+            double integralTerm = Range.clip(Constants.TurretConstants.KI * integralSum, -Constants.TurretConstants.MAX_INTEGRAL, Constants.TurretConstants.MAX_INTEGRAL);
+            double output = (Constants.TurretConstants.KP * error) + integralTerm + (Constants.TurretConstants.KD * derivative);
+            output = Range.clip(output, -Constants.TurretConstants.MAX_POWER, Constants.TurretConstants.MAX_POWER);
+
+            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);
             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 error <= tolerance || absCurrent > absTarget;
         }
         return false;
     }
 
-    public boolean isTurretReady() {
+    public boolean isTurretReady(boolean isAuton) {
-        return hasReachedVelocity();
+
+        if (!isAuton) return hasReachedVelocity() && isTurretRotationLocked();
+        else return hasReachedVelocity();
     }
 
 
-    public void updatePose(Pose robotPose) {
+    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() {
-        if (hardware.hasTarget()) {
+        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 odoDistance = 0.0;
+        if (robotPose != null) {
+            odoDistance = Math.hypot(targetX - robotPose.getX(), targetY - robotPose.getY());
+        }
+
+        boolean isActivelyShooting = isLaunching();
+
+        if (!isActivelyShooting && hardware.hasTarget()) {
             double ty = hardware.getTargetTY();
             double angleToGoalDegrees = Constants.TurretConstants.CAMERA_MOUNT_ANGLE_DEGREES + ty;
             double angleToGoalRadians = Math.toRadians(angleToGoalDegrees);
 
-            return (Constants.TurretConstants.GOAL_HEIGHT_INCHES - Constants.TurretConstants.CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians);
+            double rawVisionDistance = (Constants.TurretConstants.GOAL_HEIGHT_INCHES - Constants.TurretConstants.CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians);
-        }
-        else if (robotPose != null) {
-            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;
 
-            return Math.hypot(targetX - robotPose.getX(), targetY - robotPose.getY());
+            double distanceError = rawVisionDistance - odoDistance;
+
+            visionDistanceOffset += (distanceError - visionDistanceOffset) * 0.1;
         }
-        return 0.0;
+        return odoDistance + visionDistanceOffset;
     }
 
     private double calculateHoodOffset(double target, double current) {
@@ -288,11 +474,49 @@ 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);
+        if (dist <= 0) return 0;
-    }
 
+        if (dist < 19.0) {
+            return 1300.0;
+        }
+
+        if (dist > 145.0) {
+            return 2300.0;
+        }
+
+        double x = dist;
+        double x2 = x * x;
+        double x3 = x2 * x;
+        double x4 = x3 * x;
+
+        double velocity = (-0.0000218345 * x4)
+                + (0.00636447 * x3)
+                + (-0.593959 * x2)
+                + (26.08276 * x)
+                + 1340.12895;
+
+        return Range.clip(velocity, 1300.0, 2450.0);
+    }
     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);
+        if (dist < 20.0) {
+            return 0.36;
+        }
+
+        if (dist > 125.0) {
+            return 0.09;
+        }
+        double x = dist;
+        double x2 = x * x;
+        double x3 = x2 * x;
+        double x4 = x3 * x;
+
+        double hoodPos = (5.34037e-9 * x4)
+                + (-1.70158e-6 * x3)
+                + (2.04794e-4 * x2)
+                + (-0.013153 * x)
+                + 0.55256;
+
+        return Range.clip(hoodPos, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX);
     }
 
     public void setHoodPos(double pos) {
@@ -305,13 +529,49 @@ public class Turret {
 
     public void updateOffsetTicks(double change) {
         offsetTicks += change;
+        targetCorrectionOffsetTicks += change;
     }
 
     public double getTurretPower() {
         return turretPower;
     }
 
+    public double returnFlywheelSpeed() {
+        return hardware.getFlywheelVelocities()[1];
+    }
+
+    public void setPIDF(double[] PIDF) {
+        hardware.setPIDF(PIDF[0], PIDF[1], PIDF[2], PIDF[3]);
+    }
+
+
     public double getTargetVelocity() {
         return targetVelocity;
     }
+
+
+    public void resetTurret() {
+        hardware.resetTurret();
+    }
+
+    public boolean isTurretRotationLocked() {
+        return turretModeHold && Math.abs(lastError) <= 10;
+    }
+
+    public void updateDistanceOffset(double offset) {
+        distanceOffset += offset;
+    }
+    public void setDistanceOffset(double offset) {
+        distanceOffset = offset;
+    }
+
+
+    public void setLaunching(boolean state) {
+        isLaunching = state;
+    }
+
+    public boolean isLaunching() {
+        return isLaunching;
+    }
+
 }

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,17 @@ public class Constants {
         public static final double TURBO_SPEED_MULTIPLIER = 1.0;
     }
 
+    public static class LEDConstants {
+        public static final String LED = "prism";
+
+        // TODO: Tune these!
+        public static final double COLOR_RED = 0.301;
+        public static final double COLOR_GREEN = 0.5;
+        public static final double COLOR_ORANGE = 0.32;
+        public static final double COLOR_YELLOW = 0.38;
+    }
+
+
     public static class IntakeConstants {
         public static final String FRONT_INTAKE_MOTOR = "intake";
 
@@ -68,6 +80,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,38 +97,45 @@ public class Constants {
 
         public static final double UNLOCK_POS = 0.82;
 
+        public static double launch_duration = 0.9;
+
     }
 
+    @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.0043;
-        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.00076;
-        public static final double MAX_POWER = 0.6;
+        public static final double MAX_POWER = 0.8;
         public static final double MAX_INTEGRAL = 0.5;
+        // PIDF Values                        P    I    D    F
+        public static double[] PIDF_NORMAL = {1.3, 0, 2, 13.5};
 
-        public static final double GOAL_RED_X = 138;
+        public static double[] PIDF_Shooting = {27, 0, 2, 2.4};
-        public static final double GOAL_RED_Y = 138;
-        public static final double GOAL_BLUE_X = 6;
-        public static final double GOAL_BLUE_Y = 138;
 
-        public static final double RED_TARGET_OFFSET_DEGREES = 14;
+        public static double GOAL_RED_X = 140;
-        public static final double BLUE_TARGET_OFFSET_DEGREES = 17;
+        public static double GOAL_RED_Y = 140;
-        public static final double LL_TARGET_OFFSET_DEGREES = -2;
+        public static double GOAL_BLUE_X = 4;
-        public static final double LL_LOGIC_MULTIPLIER = 1.0;
+        public static double GOAL_BLUE_Y = 140;
-        public static final double OFFSET_SMOOTHING = 0.2;
+
+        public static double RED_TARGET_OFFSET_DEGREES = 0;
+        public static double BLUE_TARGET_OFFSET_DEGREES = 0;
+        public static double LL_TARGET_OFFSET_DEGREES = 0;
+        public static double LL_LOGIC_MULTIPLIER = -1.0;
+        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 = -370;
-        public static final double SOFT_LIMIT_POS = 230;
+        public static double SOFT_LIMIT_POS = 370;
 
-        public static final double HOOD_POS_CLOSE = 0.3;
+        public static final double HOOD_POS_CLOSE = 0.1;
-        public static final double HOOD_POS_FAR = 0.7;
+        public static final double HOOD_POS_FAR = 0.5;
-        public static final double HOOD_POS_MID = 0.5;
+        public static final double HOOD_POS_MID = 0.3;
 
         public static double HOOD_OFFSET = 0.3;
 
@@ -122,6 +144,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 = 1.0;
 
         public static final double EXTAKE_POWER = 0.3;
         public static final double INTAKE_POWER = -0.04;
@@ -140,12 +163,27 @@ 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 = 45.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,8 +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")
@@ -22,6 +29,7 @@ public class SOLOTeleOP {
             super(2);
         }
     }
+
     @TeleOp(name = "Blue SOLO", group = "FINAL")
     public static class BlueSOLO extends BaseSoloTeleOp {
         public BlueSOLO() {
@@ -35,14 +43,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 +74,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");
             }
 
@@ -90,17 +102,14 @@ public class SOLOTeleOP {
 
                 follower.update();
                 Pose currentPose = follower.getPose();
+                Vector currentVelocity = follower.getVelocity();
+
+                stateMachine.update(false, false, currentPose, currentVelocity);
 
-                //Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
-                //if (healingPose != null) {
-                   // follower.setPose(healingPose);
-                //}
 
                 handleGlobalLogic();
                 handleDriverControls();
 
-                stateMachine.update(false, false, currentPose);
-
 
                 updateTelemetry(currentPose);
             }
@@ -137,7 +146,7 @@ public class SOLOTeleOP {
 
         private void configureStartSettings() {
             stateMachine.setRobotState(RobotState.TELEOP);
-            stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+            stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
             stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
             stateMachine.getTransfer().closeLimiter();
             stateMachine.initUpdate();
@@ -159,9 +168,8 @@ public class SOLOTeleOP {
             }
 
             if (stateMachine.getCurrentGameState() != GameState.SCORING) {
-                if (!stateMachine.getTurret().hasReachedVelocity()) {
+                if (!stateMachine.getTurret().hasReachedVelocity() && (System.currentTimeMillis() % 100 < 10)) {
-                    int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
+                    gamepad1.rumble(50);
-                    gamepad1.rumble(rumbleDuration);
                 }
             }
 
@@ -172,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) {
@@ -192,17 +203,20 @@ 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.06);
+            if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(3);
-            if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.06);
+            if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-3);
-
 
+            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 +226,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("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,17 @@ 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;
+
+    private boolean PIDFShooting = false;
+
+    private boolean lastPIDFShooting = PIDFShooting;
 
     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);
@@ -44,6 +52,10 @@ public class StateMachine {
         this.m_led = new LED(h_led);
 
 
+        m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL);
+        m_turret.setLaunching(false);
+
+
         setRobotState(RobotState.INIT);
         setGameState(GameState.IDLE);
     }
@@ -69,6 +81,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,26 +91,45 @@ 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_turret.update(manualTracking, manualSAM, currentPose, currentVelocity, m_transfer.getLaunchElapsedTime());
 
         if (currentGameState == GameState.SCORING) {
-            m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
+            if (!isForceLaunch) {
-            if (m_turret.hasReachedVelocity()) {
+                m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
+            }
+
+            if (m_turret.hasReachedVelocity() || isForceLaunch || PIDFShooting) {
                 isIntakeLaunching = true;
                 m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
+                PIDFShooting = true;
+            }
+        } else {
+            PIDFShooting = false;
+        }
+
+        if (PIDFShooting != lastPIDFShooting) {
+            if (PIDFShooting) {
+
+                m_turret.setLaunching(true);
+                m_turret.setPIDF(Constants.TurretConstants.PIDF_Shooting);
+            } else {
+
+                m_turret.setLaunching(false);
+                m_turret.setPIDF(Constants.TurretConstants.PIDF_NORMAL);
             }
         }
+        lastPIDFShooting = PIDFShooting;
+
+
 
         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 +140,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 +214,14 @@ public class StateMachine {
         setGameState(GameState.IDLE);
     }
 
+    public void forceLaunchSequence() {
+        setGameState(GameState.SCORING);
+        isForceLaunch = true;
+        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

@@ -10,11 +10,14 @@ import org.firstinspires.ftc.teamcode.Hware.hwMap;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
 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;
+
 public class finalTeleOp {
 
     @TeleOp(name = "Red Final", group = "FINAL")
@@ -65,7 +68,7 @@ public class finalTeleOp {
             if (AutoTransfer.isAutonRan) {
                 currentAlliance = AutoTransfer.alliance;
                 if (currentAlliance != initialAlliance) {
-                    telemetry.addLine("WARNING: AUTON ALLIANCE DOES NOT MATCH TELEOP ALLIANCE!");
+                    telemetry.addLine("!!! WARNING: AUTON ALLIANCE DOES NOT MATCH TELEOP ALLIANCE!");
                 }
                 follower.setStartingPose(AutoTransfer.transferPose);
                 telemetry.addLine("AUTON DATA LOADED");
@@ -73,6 +76,7 @@ public class finalTeleOp {
                 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,7 +95,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,26 +105,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);
-                //Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
-
-               // if (healingPose != null) {
-               //     follower.setPose(healingPose);
-               // }
-
-                if (gamepad2.a) {
-                    stateMachine.getTransfer().openLimiter();
-                }
-                else {
-                    if (stateMachine.getCurrentGameState() != GameState.SCORING) {
-                        stateMachine.getTransfer().closeLimiter();
-                    }
-                }
 
 
                 updateTelemetry(currentPose);
@@ -148,15 +140,14 @@ public class finalTeleOp {
                     currentAlliance
             );
 
-
+            stateMachine.getTransfer().setTransferState(TransferSys.TransferState.STOP);
-            stateMachine.getTransfer().closeLimiter();
 
             stateMachine.getTurret().setAlliance(currentAlliance);
         }
 
         private void configureStartSettings() {
             stateMachine.setRobotState(RobotState.TELEOP);
-            stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+            stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
             stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
             stateMachine.getTransfer().closeLimiter();
             stateMachine.initUpdate();
@@ -168,9 +159,10 @@ public class finalTeleOp {
                 stateMachine.emergencyStop();
             }
 
-            if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity()) {
+            if (stateMachine.getCurrentGameState() != GameState.SCORING) {
-                int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
+                if (!stateMachine.getTurret().hasReachedVelocity() && (System.currentTimeMillis() % 100 < 10)) {
-                gamepad2.rumble(rumbleDuration);
+                    gamepad1.rumble(50);
+                }
             }
 
             if (gamepad1.psWasPressed()) {
@@ -218,8 +210,8 @@ public class finalTeleOp {
 
 
 
-            if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.6);
+            if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(1);
-            if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.6);
+            if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(-1);
         }
 
         private void handleOperatorInput() {
@@ -240,30 +232,22 @@ public class finalTeleOp {
                 manualTracking = !manualTracking;
             }
 
-            if (manualSAM) {
+            if (gamepad2.aWasPressed()) {
-                handleManualSAM();
+                stateMachine.forceLaunchSequence();
-            }
-        }
-
-        private void handleManualSAM() {
-            if (gamepad2.dpad_up) {
-                stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MAX);
-                stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_FAR);
-            } else if (gamepad2.dpad_left) {
-                stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
-                stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_MID);
-            } else if (gamepad2.dpad_down) {
-                stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_LOW);
-                stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_CLOSE);
             }
 
-            if (Math.abs(gamepad2.left_stick_y) > 0.13) {
+            if (gamepad2.dpadUpWasPressed()) {
-                hoodPosition += -gamepad2.left_stick_y * 0.05;
+                stateMachine.getTurret().updateDistanceOffset(7);
-                hoodPosition = Range.clip(hoodPosition, 0.0, 1.0);
+            } else if (gamepad2.dpadDownWasPressed()) {
+                stateMachine.getTurret().updateDistanceOffset(7);
+            } else if (gamepad2.dpadLeftWasPressed()) {
+                stateMachine.getTurret().setDistanceOffset(0);
             }
-            stateMachine.getTurret().setHoodPos(hoodPosition);
+
+
         }
 
+
         private void updateTelemetry(Pose p) {
             telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps());
 
@@ -278,7 +262,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/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

@@ -1,37 +1,74 @@
 package org.firstinspires.ftc.teamcode.tuning;
 
 import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.PanelsTelemetry;
 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.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.util.Range;
 
 // Make sure your import points to your actual Constants file
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+import com.bylazar.telemetry.TelemetryManager;
+
 @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;
+
+    public static boolean autoMode = false;
     public static double curTargetVelocity = 0;
     public static double targetPOS = 0;
     public static int pipeline = 1;
-
+    public static boolean reverse = true;
-    public static boolean reverse = false;
 
     double distance = 0;
 
+    CalibrationState calibState = CalibrationState.IDLE;
+    boolean lastX = false;
+
+    double maxError = 0.0;
+    double snapshotAutoHood = 0.0;
+    double calculatedGain = 0.0;
+
+    public static double P = 1.3;
+    public static double I = 0;
+    public static double D = 2;
+    public static double F = 13.5;
+
+    private double lastP = P;
+    private double lastI = I;
+    private double lastD = D;
+    private double lastF = F;
+
+    public static double distanceOffset = 0.0;
+
+    private TelemetryManager telemetryM;
+
+
     @Override
     public void init() {
         flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
@@ -42,46 +79,219 @@ 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(P, I, D, F));
+        flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(P, I, D, F));
         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);
+
+        telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+
         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);
+            distance = getTrigBasedDistance(targetY) - distanceOffset;
         }
 
-        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);
 
 
+        if (P != lastP || I != lastI || D != lastD || F != lastF) {
+            PIDFCoefficients newCoefficients = new PIDFCoefficients(P, I, D, F);
+            flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, newCoefficients);
+            flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, newCoefficients);
 
-        telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity());
+            lastP = P;
-        telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity);
+            lastI = I;
-        telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS);
+            lastD = D;
-        telemetry.addData("Distance", distance);
+            lastF = F;
+        }
+
+
+        if (autoMode) {
+            switch (calibState) {
+                case IDLE:
+                    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);
+        }
+
+        double recoilOffset = (currentVel - applyVel) * -0.000185;
+
+        hoodServo.setPosition(Range.clip(applyHood + recoilOffset, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+
+        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();
+
+        telemetryM.addData("Current Velocity", currentVel);
+
+        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();
+        telemetryM.update(telemetry);
     }
 
     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.8 + targetOffsetAngleVertical));
-        //      ^ GOAL - CAMERA HEIGHT         ^ Offset Degrees
+        //      ^ GOAL - CAMERA HEIGHT                ^ Offset Degrees
+    }
+
+    private double getFlywheelVelocity(double dist) {
+        if (dist <= 0) return 0;
+
+        if (dist < 19.0) {
+            return 1300.0;
+        }
+
+        if (dist > 145.0) {
+            return 2300.0;
+        }
+
+        double x = dist;
+        double x2 = x * x;
+        double x3 = x2 * x;
+        double x4 = x3 * x;
+
+        double velocity = (-0.0000218345 * x4)
+                + (0.00636447 * x3)
+                + (-0.593959 * x2)
+                + (26.08276 * x)
+                + 1218.12895;
+
+        return Range.clip(velocity, 1300.0, 2350.0);
+    }
+    private double getHoodPOS(double dist) {
+        if (dist < 20.0) {
+            return 0.36;
+        }
+
+        if (dist > 125.0) {
+            return 0.09;
+        }
+        double x = dist;
+        double x2 = x * x;
+        double x3 = x2 * x;
+        double x4 = x3 * x;
+
+        double hoodPos = (5.34037e-9 * x4)
+                + (-1.70158e-6 * x3)
+                + (2.04794e-4 * x2)
+                + (-0.013153 * x)
+                + 0.55256;
+
+        return Range.clip(hoodPos, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX);
     }
 }

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();
+    }
+}

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;
+        }
+
+        // 3. Apply final soft limits after wrapping
+        fusedTargetTicks = Range.clip(preWrapTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
     }
 
-    private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
+    private double calculateOdometryTargetTicks(Pose robotPose) {
-        double tx = result.getTx();
-
-        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() {
-        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) {
+        if (targetFps <= 0) return 0;
+        
         double targetMs = 1000.0 / targetFps;
-
+        double now = loopTimer.milliseconds();
-        double elapsedWorkMs = loopTimer.milliseconds() - lastTime;
+        double elapsedWorkMs = now - lastTime;
         double remainingMs = targetMs - elapsedWorkMs;
 
         return (remainingMs > 0) ? (long) remainingMs : 0;
@@ -57,4 +67,4 @@ public class FPSCounter {
         loopCount = 0;
         currentLoopTime = 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.configuration-cache=true
+
+org.gradle.parallel=true
+org.gradle.caching=true
+
+org.gradle.daemon=true