prelunch

FtcRobotController/build.gradle

@@ -3,6 +3,7 @@ import java.text.SimpleDateFormat
 //
 // build.gradle in FtcRobotController
 //
+
 apply plugin: 'com.android.library'
 
 android {

TeamCode/build.gradle

@@ -10,34 +10,10 @@
 
 
 // Custom definitions may go here
-repositories {
-    // Dairy releases repository
-    maven {
-        url = "https://repo.dairy.foundation/releases"
-    }
-    // Dairy snapshots repository
-    maven {
-        url = "https://repo.dairy.foundation/snapshots"
-    }
-}
-
-buildscript {
-    repositories {
-        mavenCentral()
-        maven {
-            url "https://repo.dairy.foundation/releases"
-        }
-    }
-    dependencies {
-        classpath "dev.frozenmilk:Load:0.2.4"
-    }
-}
-
 
 // Include common definitions from above.
 apply from: '../build.common.gradle'
 apply from: '../build.dependencies.gradle'
-apply plugin: 'dev.frozenmilk.sinister.sloth.load'
 
 android {
     namespace = 'org.firstinspires.ftc.teamcode'
@@ -49,6 +25,4 @@ android {
 
 dependencies {
     implementation project(':FtcRobotController')
-    implementation("dev.frozenmilk.sinister:Sloth:0.2.4")
-    implementation("com.bylazar.sloth:fullpanels:0.2.4+1.0.12")
 }

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;
-        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led2;
+        private ServoImplEx led;
 
         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
                     )
             );
@@ -172,7 +188,7 @@ public class hwMap {
         public NormalizedColorSensor[] sensors;
 
         public TransferHwMap(HardwareMap hardwareMap) {
-            Limiter = hardwareMap.servo.get("limiter");
+            Limiter = hardwareMap.servo.get(Constants.TransferConstants.LIMITER_SERVO);
 
 
             indexA = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_A);
@@ -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();
@@ -259,8 +275,8 @@ public class hwMap {
             turretLeftMotor = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
             turretRightMotor = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
 
-            turretLeftMotor.setDirection(Constants.TurretConstants.TURRET_MOTOR_L_DIRECTION);
-            turretRightMotor.setDirection(Constants.TurretConstants.TURRET_MOTOR_R_DIRECTION);
+            turretLeftMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+            turretRightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
 
             turretRightMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
             turretLeftMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
@@ -271,23 +287,43 @@ public class hwMap {
             // --- Turret Rotation Motor ---
             turretrotation = hardwareMap.dcMotor.get(Constants.TurretConstants.TURRET_ROTATION_MOTOR);
             turretrotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            turretrotation.setDirection(Constants.TurretConstants.TURRET_ROTATION_DIR);
             turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
             // --- Hood Servo ---
             hoodservo = hardwareMap.servo.get(Constants.TurretConstants.HOOD_TURRET_SERVO);
-            hoodservo.setDirection(Servo.Direction.FORWARD);
+            hoodservo.setDirection(Servo.Direction.REVERSE);
 
             // --- External Encoder (if used separately) ---
 
             initLimelight(hardwareMap);
         }
 
-        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 resetTurret() {
+            turretrotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         }
 
+        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,52 @@ 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))
-                    .setLinearHeadingInterpolation(startPose.getHeading(), scorePosePreload.getHeading())
+                    .addPath(new BezierLine(new Pose(59, 9), new Pose(59, 15)))
+                    .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(118))
                     .build();
 
             grabSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePosePreload, set1Inter))
-                    .setLinearHeadingInterpolation(scorePosePreload.getHeading(), Math.toRadians(180))
-                    .addPath(new BezierLine(set1Inter, set1Pick))
+                    .addPath(new BezierLine(new Pose(59, 15), new Pose(44, 36)))
+                    .setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180))
+                    .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))
-                    .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading())
+                    .addPath(new BezierCurve(new Pose(17, 36), new Pose(36.621, 19.900), new Pose(59, 15)))
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118))
                     .build();
 
-            grabSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePoseGeneric, set2Inter))
-                    .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), Math.toRadians(180))
-                    .addPath(new BezierCurve(set2Inter, set2Control1, set2Control2, set2Pick))
-                    .setConstantHeadingInterpolation(Math.toRadians(180))
-                    .build();
-
-            scoreSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set2Pick, scorePoseGeneric))
-                    .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading())
-                    .build();
-
-            // Corner Sequence
             grabEndGamePath = follower.pathBuilder()
-                    .addPath(new BezierCurve(scorePoseGeneric, cornerInterControl, cornerInter))
-                    .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), Math.toRadians(-90))
-                    .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))
+                    .addPath(new BezierCurve(new Pose(59, 15), new Pose(37.157, 26.830), new Pose(10.400, 8)))
+                    .setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180))
                     .build();
 
             scoreEndGamePath = follower.pathBuilder()
-                    .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric))
-                    .setLinearHeadingInterpolation(Math.toRadians(-160), scorePoseGeneric.getHeading())
+                    .addPath(new BezierLine(new Pose(10.400, 8), new Pose(59, 15)))
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118))
                     .build();
 
             parkPath = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePoseGeneric, parkPose))
-                    .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), parkPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(59, 15), new Pose(37.181, 26.706), new Pose(10.400, 27)))
+                    .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 BezierCurve(new Pose(59, 15), new Pose(35.728, 26.614), new Pose(10, 27)))
+                    .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(180))
+                    .addPath(new BezierLine(new Pose(10, 27), new Pose(10.219, 17.664)))
+                    .setConstantHeadingInterpolation(Math.toRadians(180))
+                    .addPath(new BezierLine(new Pose(10.219, 17.664), new Pose(59, 15)))
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(135))
+                    .addPath(new BezierLine(new Pose(59, 15), new Pose(55, 20)))
+                    .setConstantHeadingInterpolation(Math.toRadians(135))
                     .build();
         }
     }
@@ -116,110 +89,86 @@ 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))
-                    .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading())
+                    .addPath(new BezierLine(new Pose(21, 124), new Pose(54, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(143.5), Math.toRadians(144))
                     .build();
 
             driveToSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(preloadScorePose, set2DrivePose))
-                    .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading())
+                    .addPath(new BezierLine(new Pose(54, 90), new Pose(41.758, 61.439)))
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(180))
                     .build();
 
             grabSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set2DrivePose, set2PickPose))
-                    .setTangentHeadingInterpolation()
+                    .addPath(new BezierLine(new Pose(41.758, 61.439), new Pose(16, 59.830)))
+                    .setConstantHeadingInterpolation(Math.toRadians(180))
                     .build();
 
             scoreSet2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose))
-                    .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading())
+                    .addPath(new BezierCurve(new Pose(16, 59.830), new Pose(55.542, 82.280), new Pose(54, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(144))
                     .build();
 
             grabGate1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set2ScorePose, gate1PickControl, gate1PickPose))
-                    .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(54, 90), new Pose(40.119, 55.595), new Pose(15.300, 64)))
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(90))
+                    .addPath(new BezierCurve(new Pose(15.300, 64), new Pose(17.445, 52.820), new Pose(7.500, 50)))
+                    .setConstantHeadingInterpolation(Math.toRadians(90))
                     .build();
 
             scoreGate1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose))
-                    .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading())
+                    .addPath(new BezierCurve(new Pose(7.500, 50), new Pose(41.918, 62.187), new Pose(54, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(144))
                     .build();
 
             grabGate2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose))
-                    .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(54, 90), new Pose(42.081, 61.914), new Pose(14.600, 63.600)))
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(90))
+                    .addPath(new BezierCurve(new Pose(14.600, 63.600), new Pose(17.545, 53.278), new Pose(7.500, 50)))
+                    .setConstantHeadingInterpolation(Math.toRadians(90))
                     .build();
 
             scoreGate2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose))
-                    .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading())
+                    .addPath(new BezierCurve(new Pose(7.500, 50), new Pose(44, 62), new Pose(54, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(144))
                     .build();
 
             driveToSet3Path = follower.pathBuilder()
-                    .addPath(new BezierLine(gate2ScorePose, set3DrivePose))
-                    .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading())
+                    .addPath(new BezierLine(new Pose(54, 90), new Pose(38.515, 47.156)))
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(220))
                     .build();
 
             grabSet3Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose))
-                    .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(38.515, 47.156), new Pose(29.622, 37.842), new Pose(12, 36)))
+                    .setLinearHeadingInterpolation(Math.toRadians(220), Math.toRadians(185))
                     .build();
 
             scoreSet3Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose))
-                    .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading())
+                    .addPath(new BezierCurve(new Pose(12, 36), new Pose(9.594, 52.541), new Pose(54, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(185), Math.toRadians(144))
                     .build();
 
             driveToSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set3ScorePose, set1DrivePose))
-                    .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading())
+                    .addPath(new BezierLine(new Pose(54, 90), new Pose(39, 84)))
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(180))
                     .build();
 
             grabSet1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose))
-                    .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(39, 84), new Pose(14.863, 77.692), new Pose(29.689, 106.279)))
+                    .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(144))
                     .build();
 
             scoreSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set1PickPose, set1ScorePose))
-                    .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading())
+                    .addPath(new BezierLine(new Pose(29.689, 106.279), new Pose(44, 100)))
+                    .setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(148))
                     .build();
 
             parkPath = follower.pathBuilder()
-                    .addPath(new BezierLine(set1ScorePose, parkTargetPose))
-                    .setConstantHeadingInterpolation(set1ScorePose.getHeading())
+                    .addPath(new BezierLine(new Pose(44, 100), new Pose(36.219, 93.170)))
+                    .setConstantHeadingInterpolation(Math.toRadians(148))
                     .build();
         }
     }
@@ -239,74 +188,48 @@ 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))
-                    .setLinearHeadingInterpolation(startPose.getHeading(), Math.toRadians(62))
+                    .addPath(new BezierLine(new Pose(85, 9), new Pose(85, 15)))
+                    .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))
-                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
-                    .addPath(new BezierCurve(set2Inter, set2Control1, set2Control2, set2Pick))
-                    .setConstantHeadingInterpolation(Math.toRadians(0))
-                    .build();
-
-            scoreSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set2Pick, scorePoseGeneric))
-                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
-                    .build();
-
-            // Corner Sequence
             grabEndGamePath = follower.pathBuilder()
-                    .addPath(new BezierCurve(scorePoseGeneric, cornerInterControl, cornerInter))
-                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(270))
-                    .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))
+                    .addPath(new BezierCurve(new Pose(85, 15), new Pose(106.843, 26.830), new Pose(133.600, 8)))
+                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
                     .build();
 
             scoreEndGamePath = follower.pathBuilder()
-                    .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric))
-                    .setLinearHeadingInterpolation(Math.toRadians(340), Math.toRadians(62))
+                    .addPath(new BezierLine(new Pose(133.600, 8), new Pose(85, 15)))
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
                     .build();
 
             parkPath = follower.pathBuilder()
-                    .addPath(new BezierLine(scorePoseGeneric, parkPose))
-                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(45))
+                    .addPath(new BezierCurve(new Pose(85, 15), new Pose(106.819, 26.706), new Pose(133.600, 27)))
+                    .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 BezierCurve(new Pose(85, 15), new Pose(108.272, 26.614), new Pose(134, 27)))
+                    .setLinearHeadingInterpolation(Math.toRadians(45), Math.toRadians(0))
+                    .addPath(new BezierLine(new Pose(134, 27), new Pose(133.781, 17.664)))
+                    .setConstantHeadingInterpolation(Math.toRadians(0))
+                    .addPath(new BezierLine(new Pose(133.781, 17.664), new Pose(85, 15)))
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(45))
+                    .addPath(new BezierLine(new Pose(85, 15), new Pose(89, 20)))
+                    .setConstantHeadingInterpolation(Math.toRadians(45))
                     .build();
         }
     }
@@ -319,110 +242,86 @@ 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))
-                    .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading())
+                    .addPath(new BezierLine(new Pose(123, 124), new Pose(90, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(36.5), Math.toRadians(36))
                     .build();
 
             driveToSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(preloadScorePose, set2DrivePose))
-                    .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading())
+                    .addPath(new BezierLine(new Pose(90, 90), new Pose(102.242, 61.439)))
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(0))
                     .build();
 
             grabSet2Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set2DrivePose, set2PickPose))
-                    .setTangentHeadingInterpolation()
+                    .addPath(new BezierLine(new Pose(102.242, 61.439), new Pose(128, 59.830)))
+                    .setConstantHeadingInterpolation(Math.toRadians(0))
                     .build();
 
             scoreSet2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose))
-                    .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading())
+                    .addPath(new BezierCurve(new Pose(128, 59.830), new Pose(88.458, 82.280), new Pose(90, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(36))
                     .build();
 
             grabGate1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set2ScorePose, gate1PickControl, gate1PickPose))
-                    .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(90, 90), new Pose(103.881, 55.595), new Pose(128.700, 64)))
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(90))
+                    .addPath(new BezierCurve(new Pose(128.700, 64), new Pose(126.555, 52.820), new Pose(136.500, 50)))
+                    .setConstantHeadingInterpolation(Math.toRadians(90))
                     .build();
 
             scoreGate1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose))
-                    .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading())
+                    .addPath(new BezierCurve(new Pose(136.500, 50), new Pose(102.082, 62.187), new Pose(90, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(36))
                     .build();
 
             grabGate2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose))
-                    .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(90, 90), new Pose(101.919, 61.914), new Pose(129.400, 63.600)))
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(90))
+                    .addPath(new BezierCurve(new Pose(129.400, 63.600), new Pose(126.455, 53.278), new Pose(136.500, 50)))
+                    .setConstantHeadingInterpolation(Math.toRadians(90))
                     .build();
 
             scoreGate2Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose))
-                    .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading())
+                    .addPath(new BezierCurve(new Pose(136.500, 50), new Pose(100, 62), new Pose(90, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(36))
                     .build();
 
             driveToSet3Path = follower.pathBuilder()
-                    .addPath(new BezierLine(gate2ScorePose, set3DrivePose))
-                    .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading())
+                    .addPath(new BezierLine(new Pose(90, 90), new Pose(105.485, 47.156)))
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(-40))
                     .build();
 
             grabSet3Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose))
-                    .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(105.485, 47.156), new Pose(114.378, 37.842), new Pose(132, 36)))
+                    .setLinearHeadingInterpolation(Math.toRadians(-40), Math.toRadians(-5))
                     .build();
 
             scoreSet3Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose))
-                    .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading())
+                    .addPath(new BezierCurve(new Pose(132, 36), new Pose(134.406, 52.541), new Pose(90, 90)))
+                    .setLinearHeadingInterpolation(Math.toRadians(-5), Math.toRadians(36))
                     .build();
 
             driveToSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set3ScorePose, set1DrivePose))
-                    .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading())
+                    .addPath(new BezierLine(new Pose(90, 90), new Pose(105, 84)))
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(0))
                     .build();
 
             grabSet1Path = follower.pathBuilder()
-                    .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose))
-                    .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading())
+                    .addPath(new BezierCurve(new Pose(105, 84), new Pose(129.137, 77.692), new Pose(114.311, 106.279)))
+                    .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(36))
                     .build();
 
             scoreSet1Path = follower.pathBuilder()
-                    .addPath(new BezierLine(set1PickPose, set1ScorePose))
-                    .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading())
+                    .addPath(new BezierLine(new Pose(114.311, 106.279), new Pose(100, 100)))
+                    .setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(32))
                     .build();
 
             parkPath = follower.pathBuilder()
-                    .addPath(new BezierLine(set1ScorePose, parkTargetPose))
-                    .setConstantHeadingInterpolation(set1ScorePose.getHeading())
+                    .addPath(new BezierLine(new Pose(100, 100), new Pose(107.781, 93.170)))
+                    .setConstantHeadingInterpolation(Math.toRadians(32))
                     .build();
         }
     }
@@ -442,7 +341,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 +430,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());
@@ -601,36 +503,7 @@ public class Auton {
                     if (!follower.isBusy()) {
                         systems.setAutonState(pedroStateMachine.AutonState.SCORING);
                         if (checkScoringCondition()) {
-                            setPathState(PathState.DRIVE_TO_GRAB_SET_2);
-                        }
-                    }
-                    break;
-
-                // --- SET 2 ---
-                case DRIVE_TO_GRAB_SET_2:
-                    follower.followPath(grabSet2Path, true);
-                    systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
-                    setPathState(PathState.GRAB_SET_2);
-                    break;
-
-                case GRAB_SET_2:
-                    if (!follower.isBusy()) {
-                        systems.setAutonState(pedroStateMachine.AutonState.IDLE);
-                        setPathState(PathState.DRIVE_TO_SCORE_SET_2);
-                    }
-                    break;
-
-                case DRIVE_TO_SCORE_SET_2:
-                    follower.followPath(scoreSet2Path, true);
-                    systems.initiateTurretSpinUp();
-                    setPathState(PathState.SCORE_SET_2);
-                    break;
-
-                case SCORE_SET_2:
-                    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_transfer.closeLimiter(); // Ensure rings don't fly out back
+                m_turret.setTurretState(TurretState.INTAKING);
+                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) {
-        m_turret.updateAuton(allianceTarget, currentPose, 0.2);
+    public void update(int allianceTarget, Pose currentPose, Vector currentVelocity) {
+        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

@@ -18,10 +18,16 @@ import org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants;
 
 public class Constants {
     public static FollowerConstants followerConstants = new FollowerConstants()
-            .mass(12);
+            .mass(12)
+            .translationalPIDFCoefficients(new PIDFCoefficients(0.26, 0, 0.01, 0.03))
+            .headingPIDFCoefficients(new PIDFCoefficients(1.8, 0, 0.01, 0.03))
+            .forwardZeroPowerAcceleration(-39.64668514000648)
+            .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)
             .rightRearMotorName(DriveConstants.BACK_RIGHT_MOTOR)
             .leftRearMotorName(DriveConstants.BACK_LEFT_MOTOR)
@@ -35,9 +41,9 @@ public class Constants {
 
 
     public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants()
-            .forwardTicksToInches(0.001978956)
-            .strafeTicksToInches(0.001978956)
-            .turnTicksToInches(0.001978956)
+            .forwardTicksToInches(0.0020041908950982315)
+            .strafeTicksToInches(0.002004094712407555)
+            .turnTicksToInches(0.0022824233082645137)
             .leftPodY(3.75)
             .rightPodY(-3.25)
             .strafePodX(-6.25)

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 currentStrafe = h_driveTrain.frontLeftMotor.getCurrentPosition();
+        double currentFwd = 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;
@@ -130,7 +128,7 @@ public class DriveTrain {
         if (isAnchorActive) {
             isAnchorActive = false;
             stop();
-            this.currentState = DriveState.NORMAL;
+            this.currentState = DriveState.TURBO;
         }
     }
 

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.Prism.GoBildaPrismDriver.LayerHeight;
-import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
 
 public class LED {
     private final hwMap.LedHwMap hardware;
 
-    // Track states to prevent spamming the I2C bus
-    private int currentLed1State = -1;
-    private int 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
+    private int currentLedState = -1;
 
     public LED(hwMap.LedHwMap hardware) {
         this.hardware = hardware;
-
-        solidRed.setBrightness(50);
-        solidOrange.setBrightness(50);
-        solidGreen.setBrightness(50);
-        solidBlue.setBrightness(50);
-        solidPurple.setBrightness(50);
+        turnOn();
     }
 
-    public void update(boolean isScoring, boolean isTurretReady, boolean hasTarget, boolean isAnchorActive) {
-    /*
-        int newLed1State = 0;
-        if (isScoring) {
-            if (isTurretReady) {
-                newLed1State = 2;
-            } else {
-                newLed1State = 1;
+    public void update(boolean isTurretLocked, boolean isFlywheelUpToSpeed) {
+        int newLedState = 0;
+
+        if (isTurretLocked && isFlywheelUpToSpeed) {
+            newLedState = 1;
+        } else if (isTurretLocked) {
+            newLedState = 2;
+        } else if (isFlywheelUpToSpeed) {
+            newLedState = 3;
+        }
+
+        if (newLedState != currentLedState) {
+            if (newLedState == 0) {
+                hardware.setLedPos(Constants.LEDConstants.COLOR_RED);
+            } else if (newLedState == 1) {
+                hardware.setLedPos(Constants.LEDConstants.COLOR_GREEN);
+            } else if (newLedState == 2) {
+                hardware.setLedPos(Constants.LEDConstants.COLOR_ORANGE);
+            } else if (newLedState == 3) {
+                hardware.setLedPos(Constants.LEDConstants.COLOR_YELLOW);
+            }
+
+            currentLedState = newLedState;
         }
     }
 
-        if (newLed1State != currentLed1State) {
-            if (newLed1State == 0) {
-                hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
-            } else if (newLed1State == 1) {
-                hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
-            } else if (newLed1State == 2) {
-                hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
-            }
-            currentLed1State = newLed1State;
+    public void turnOff() {
+        hardware.setLedOn(false);
+        currentLedState = -1;
     }
     
-        int newLed2State = 0;
-        if (hasTarget && isAnchorActive) {
-            newLed2State = 3;
-        } else if (hasTarget) {
-            newLed2State = 1;
-        } else if (isAnchorActive) {
-            newLed2State = 2;
-        }
-
-        if (newLed2State != currentLed2State) {
-            if (newLed2State == 0) {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
-            } else if (newLed2State == 1) {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
-            } else if (newLed2State == 2) {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidBlue);
-            } else if (newLed2State == 3) {
-                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidPurple);
-            }
-            currentLed2State = newLed2State;
-        }*/
+    public void turnOn() {
+        hardware.setLedOn(true);
     }
 }

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,15 +56,22 @@ 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) {
+        if (this.alliance != alliance) {
             this.alliance = alliance;
+            targetCorrectionOffsetTicks = 0; // Reset correction when alliance changes
+            offsetTicks = 0; // Reset manual offset when alliance changes
             if (alliance == 2) {
                 hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
             } else if (alliance == 1) {
@@ -58,6 +80,7 @@ public class Turret {
                 hardware.setPipeline(0);
             }
         }
+    }
 
     public void stop() {
         if (currentState != TurretState.IDLE) {
@@ -68,7 +91,9 @@ public class Turret {
     }
 
     public void setTurretState(TurretState state) {
+        if (this.currentState != state) {
             this.currentState = state;
+            stateTimer.reset();
 
             switch (state) {
                 case IDLE:
@@ -85,6 +110,7 @@ public class Turret {
                     break;
             }
         }
+    }
 
 
     public TurretState getTurretState() {
@@ -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 dropThreshold = targetVelocity * 0.95;
-            double recoilOffset = 0.0;
+            double currentVel = hardware.getFlywheelVelocities()[1];
 
-            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,91 @@ public class Turret {
         }
     }
 
-    private void handleLaunchLogic(double offset) {
-        if (distance > 0) {
-            targetVelocity = getFlywheelVelocity(distance);
-            hardware.setTurretVelocity(targetVelocity);
 
-            double calculatedHoodPos = getHoodPOS(distance);
-            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) {
+    /*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);
-            targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - Constants.TurretConstants.OFFSET_SMOOTHING))
-                    + (rawErrorTicks * Constants.TurretConstants.OFFSET_SMOOTHING);
+            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;
+
+            // 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()) {
+                offsetTicks = 0;
+                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,32 +336,38 @@ 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;
-    }
-
-    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;
+        // APPLY THE KINEMATIC SHIFT
+        return ((relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE) + kinematicAngularOffsetTicks;
     }
 
     private void runPIDControl(double currentTicks, double dt) {
-        fusedTargetTicks = fusedTargetTicks + offsetTicks;
         double error = fusedTargetTicks - currentTicks;
         double derivative = (error - lastError) / dt;
 
+        double entryThreshold = 10;
+        double exitThreshold = 30;
+
+        if (!turretModeHold && Math.abs(error) <= entryThreshold) {
+            turretModeHold = true;
+        } else if (turretModeHold && Math.abs(error) > exitThreshold) {
+            turretModeHold = false;
+        }
+
+        if (turretModeHold) {
+            if (lastTurretModeHold != turretModeHold) {
+                hardware.runToPos((int) fusedTargetTicks); // Set target first
+                hardware.enableTurretHold();
+                hardware.setTurretRotationPower(Constants.TurretConstants.TURRET_HOLD_POWER); // Set a holding power
+            }
+            hardware.runToPos((int) fusedTargetTicks); // Keep target updated even in hold mode
+        } else {
+            if (lastTurretModeHold != turretModeHold) {
+                hardware.disableTurretHold();
+            }
+
             if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) {
                 integralSum += (error * dt);
             } else {
@@ -239,48 +380,94 @@ public class Turret {
 
             hardware.setTurretRotationPower(output);
             turretPowerRotation = output;
+        }
+        lastTurretModeHold = turretModeHold;
         lastError = error;
     }
 
     public boolean hasReachedVelocity() {
-        double currentVel = hardware.getFlywheelVelocities()[0];
+        double currentVel = hardware.getFlywheelVelocities()[1];
 
         if (Math.abs(targetVelocity) > 1000) {
             double absCurrent = Math.abs(currentVel);
             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() {
-        return hasReachedVelocity();
+    public boolean isTurretReady(boolean isAuton) {
+
+        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);
-        }
-        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;
+            double rawVisionDistance = (Constants.TurretConstants.GOAL_HEIGHT_INCHES - Constants.TurretConstants.CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians);
 
-            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 +475,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 2400.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)
+                + 1350.12895;
+
+        return Range.clip(velocity, 1300.0, 2450.0);
+    }
     private double getHoodPOS(double dist) {
-        return ((-(3.62429 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000638975 * Math.pow(dist, 3) - (0.000153252 * Math.pow(dist, 2)) + (-0.0197027 * dist) + 1.40511);
+        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 +530,52 @@ 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 double getDistanceOffset() {
+        return distanceOffset;
+    }
+
+    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 {
@@ -31,26 +32,32 @@ public class Constants {
         public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
         public static final double TICKS_PER_REVOLUTION = 537.6;
 
-        // Drive characteristics
-        public static final double MAX_VELOCITY = 30.0; // inches per second
-        public static final double MAX_ANGULAR_VELOCITY = Math.toRadians(180.0); // radians per second
-        public static final double MAX_ACCELERATION = 30.0; // inches per second squared
-
         public static final double STOP_SPEED_MULTIPLIER = 0.0;
         public static final double PRECISION_SPEED_MULTIPLIER = 0.3;
         public static final double NORMAL_SPEED_MULTIPLIER = 0.6;
         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";
 
-        public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.REVERSE;
+        public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.FORWARD;
 
         public static DcMotor.ZeroPowerBehavior INTAKE_ZERO_POWER_BEHAVIOR = DcMotor.ZeroPowerBehavior.BRAKE;
         public static DcMotor.RunMode INTAKE_RUNMODE = DcMotor.RunMode.RUN_WITHOUT_ENCODER;
 
-        public static final double INTAKE_POWER = 1;
+        public static final double INTAKE_POWER = 0.9;
         public static final double EXTAKE_POWER = -0.8;
         public static final double LAUNCH_POWER = 1;
         public static final double IDLE_POWER = 0;
@@ -73,8 +80,11 @@ public class Constants {
         public static final double POWER_HOLDING = 0.1;
     }
 
+
+    @Configurable
+
     public static class TransferConstants {
-        public static final String LIMITER_SERVO = "activeTransfer";
+        public static final String LIMITER_SERVO = "limiter";
 
         public static final Servo.Direction LIMITER_SERVO_DIR = Servo.Direction.FORWARD;
 
@@ -83,40 +93,49 @@ public class Constants {
         public static final String INDEX_SENSOR_B = "colorB";
         public static final String INDEX_SENSOR_C = "colorC";
 
-        public static final double LIMIT_POS = 0.99;
+        public static final double LIMIT_POS = 1;
 
-        public static final double UNLOCK_POS = 0.81;
+        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 final double EXTERNAL_GEAR_RATIO = 1.315994798439532;
+        public static double TICKS_PER_REV_MOTOR = 145.1;
+        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.028;
-        public static final double KI = 0.0;
-        public static final double KD = 0.00055;
-        public static final double MAX_POWER = 0.6;
+        public static double KP = 0.0043;
+        public static double KI = 0.00801;
+        public static double KD = 0.00076;
+        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, 16};
 
-        public static final double GOAL_RED_X = 138;
-        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 double[] PIDF_Shooting = {27, 0, 2, 2.4};
 
-        public static final double RED_TARGET_OFFSET_DEGREES = 14;
-        public static final double BLUE_TARGET_OFFSET_DEGREES = 17;
-        public static final double LL_TARGET_OFFSET_DEGREES = -6;
-        public static final double LL_LOGIC_MULTIPLIER = -1.0;
-        public static final double OFFSET_SMOOTHING = 0.3;
+        public static double GOAL_RED_X = 140;
+        public static double GOAL_RED_Y = 140;
+        public static double GOAL_BLUE_X = 4;
+        public static double GOAL_BLUE_Y = 140;
 
-        public static final double SOFT_LIMIT_NEG = -100.0;
-        public static final double SOFT_LIMIT_POS = 250.0;
+        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 double HOOD_POS_CLOSE = 0.3;
-        public static final double HOOD_POS_FAR = 0.7;
-        public static final double HOOD_POS_MID = 0.5;
+        public static final DcMotorSimple.Direction TURRET_ROTATION_DIR = DcMotorSimple.Direction.FORWARD;
+
+        public static double SOFT_LIMIT_NEG = -370;
+        public static double SOFT_LIMIT_POS = 370;
+
+        public static final double HOOD_POS_CLOSE = 0.1;
+        public static final double HOOD_POS_FAR = 0.5;
+        public static final double HOOD_POS_MID = 0.3;
 
         public static double HOOD_OFFSET = 0.3;
 
@@ -125,30 +144,46 @@ 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;
 
         public static final String TURRET_ROTATION_MOTOR = "turretRotation";
-        public static final String HOOD_TURRET_SERVO = "hoodServo";
+        public static final String HOOD_TURRET_SERVO = "hoodservo";
         public static final Servo.Direction HOOD_SERVO_DIR = Servo.Direction.FORWARD;
 
         public static final String TURRET_RIGHT_MOTOR = "rturret";
         public static final String TURRET_LEFT_MOTOR = "lturret";
 
-        public static final DcMotorSimple.Direction TURRET_MOTOR_R_DIRECTION = DcMotorSimple.Direction.REVERSE;
+        public static final DcMotorSimple.Direction TURRET_MOTOR_R_DIRECTION = DcMotorSimple.Direction.FORWARD;
         public static final DcMotorSimple.Direction TURRET_MOTOR_L_DIRECTION = DcMotorSimple.Direction.REVERSE;
 
         public static final int LIMELIGHT_PIPELINE_MOTIF = 0;
         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 = 15.5;
-        public static final double GOAL_HEIGHT_INCHES = 29.5;
-        public static final double CAMERA_MOUNT_ANGLE_DEGREES = 2.0;
+        public static double CAMERA_HEIGHT_INCHES = 12.75;
+        public static double GOAL_HEIGHT_INCHES = 29.5;
+        public static double CAMERA_MOUNT_ANGLE_DEGREES = 19.8;
 
-        public static final double SERVO_MAX = 0.7;
-        public static final double SERVO_MIN = 0.26;
+        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.43;
     }
 
 

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()) {
-                    int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
-                    gamepad1.rumble(rumbleDuration);
+                if (!stateMachine.getTurret().hasReachedVelocity() && (System.currentTimeMillis() % 100 < 10)) {
+                    gamepad1.rumble(50);
                 }
             }
 
@@ -172,9 +180,12 @@ public class SOLOTeleOP {
 
         private void handleDriverControls() {
 
-            if (gamepad1.dpad_up) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
-            else if (gamepad1.dpad_left) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
-            else if (gamepad1.dpad_down) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION);
+            if (gamepad1.dpad_up)
+                stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
+            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.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.06);
-
+            if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(3);
+            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) {
+            if (!isForceLaunch) {
                 m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
-            if (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.hasReachedVelocity(),
-                m_turret.hasTarget(),
-                m_driveTrain.isAnchorActive()
+                m_turret.isTurretRotationLocked(),
+                m_turret.hasReachedVelocity()
         );
 
         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) {
@@ -167,7 +199,7 @@ public class StateMachine {
     private void handleRobotStateEntry(RobotState newState) {
         switch (newState) {
             case TELEOP:
-                m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+                m_driveTrain.setDriveState(DriveTrain.DriveState.TURBO);
                 m_intake.setIntakeState(Intake.IntakeState.IDLE);
                 break;
             case ESTOP:
@@ -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);
-                //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();
-                    }
-                }
+                stateMachine.update(manualSAM, manualTracking, currentPose, currentVelocity);
 
 
                 updateTelemetry(currentPose);
@@ -148,15 +140,14 @@ public class finalTeleOp {
                     currentAlliance
             );
 
-
-            stateMachine.getTransfer().closeLimiter();
+            stateMachine.getTransfer().setTransferState(TransferSys.TransferState.STOP);
 
             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()) {
-                int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
-                gamepad2.rumble(rumbleDuration);
+            if (stateMachine.getCurrentGameState() != GameState.SCORING) {
+                if (!stateMachine.getTurret().hasReachedVelocity() && (System.currentTimeMillis() % 100 < 10)) {
+                    gamepad1.rumble(50);
+                }
             }
 
             if (gamepad1.psWasPressed()) {
@@ -218,8 +210,8 @@ public class finalTeleOp {
 
 
 
-            if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.6);
-            if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.6);
+            if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(1);
+            if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(-1);
         }
 
         private void handleOperatorInput() {
@@ -240,30 +232,27 @@ public class finalTeleOp {
                 manualTracking = !manualTracking;
             }
 
-            if (manualSAM) {
-                handleManualSAM();
-            }
+            if (gamepad2.aWasPressed()) {
+                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 (gamepad2.dpadUpWasPressed()) {
+                stateMachine.getTurret().updateDistanceOffset(7);
+            } else if (gamepad2.dpadDownWasPressed()) {
+                stateMachine.getTurret().updateDistanceOffset(-7);
+            } else if (gamepad2.dpadLeftWasPressed()) {
+                stateMachine.getTurret().setDistanceOffset(0);
             }
 
-            if (Math.abs(gamepad2.left_stick_y) > 0.13) {
-                hoodPosition += -gamepad2.left_stick_y * 0.05;
-                hoodPosition = Range.clip(hoodPosition, 0.0, 1.0);
+            if (gamepad2.dpadRightWasPressed()) {
+                org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.ARTIFACT_1 = -90;
+            } else if (gamepad2.dpadLeftWasReleased()) {
+                org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.ARTIFACT_1 = -150;
             }
-            stateMachine.getTurret().setHoodPos(hoodPosition);
+
         }
 
+
         private void updateTelemetry(Pose p) {
             telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps());
 
@@ -274,11 +263,12 @@ public class finalTeleOp {
             telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE");
             telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
             telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState());
+            telemetry.addData("Distance offset", stateMachine.getTurret().getDistanceOffset());
 
             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/ColorSensorTuning.java

@@ -7,6 +7,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.NormalizedRGBA;
 import org.firstinspires.ftc.teamcode.Hware.hwMap;
+
+@Disabled
 @TeleOp(name="Color Sensor Tuning", group="Diagnostics")
 public class ColorSensorTuning extends LinearOpMode {
 

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

@@ -1,84 +1,70 @@
 package org.firstinspires.ftc.teamcode.tuning;
 
 import com.bylazar.configurables.annotations.Configurable;
-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;
 @Configurable
 @TeleOp
 public class PIDFTuning extends OpMode {
 
-
-    private Limelight3A limelight;
-
     public DcMotorEx flywheelMotorR;
     public DcMotorEx flywheelMotorL;
 
-    public Servo hoodServo;
+    public static double targetVelocity = 0;
 
-    public static double curTargetVelocity = 0;
-    public static double targetPOS = 0;
-    public static int pipeline = 1;
+    public static double targetVelocity1 = 1500;
+    public static double targetVelocity2 = 900;
+    public static double f = 0;
+    public static double p = 0;
 
-    double distance = 0;
+    public static double d = 0;
+
+    public static boolean reverse = false;
 
     @Override
     public void init() {
-        flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
-        flywheelMotorL = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
+        flywheelMotorR = hardwareMap.get(DcMotorEx.class, "rturret");
+        flywheelMotorR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        flywheelMotorR.setDirection(DcMotorSimple.Direction.FORWARD);
 
-        flywheelMotorR.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
-        flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE);
-        flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
-
-        hoodServo = hardwareMap.servo.get("hoodServo");
-
-        hoodServo.setDirection(Servo.Direction.REVERSE);
-
-        limelight = hardwareMap.get(Limelight3A.class, "limelight");
-        limelight.pipelineSwitch(pipeline);
-        limelight.start();
+        flywheelMotorL = hardwareMap.get(DcMotorEx.class, "lturret");
+        flywheelMotorL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
 
         telemetry.addLine("Init complete");
     }
 
     @Override
     public void loop() {
-        limelight.pipelineSwitch(pipeline);
+        PIDFCoefficients pidfCoefficients = new PIDFCoefficients(p, 0, d, f);
+        flywheelMotorR.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients);
 
-        LLResult result = limelight.getLatestResult();
+        flywheelMotorL.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients);
 
-        if (result.isValid()) {
-            double targetY = result.getTy();
-            telemetry.addData("Ty", targetY);
-            distance = getTrigBasedDistance(targetY);
+        flywheelMotorR.setVelocity(targetVelocity);
+        flywheelMotorL.setVelocity(targetVelocity);
+
+        if (gamepad1.aWasPressed()) {
+            targetVelocity = targetVelocity1;
+        } else if (gamepad1.bWasPressed()) {
+            targetVelocity = targetVelocity2;
         }
 
-        if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
-        flywheelMotorL.setVelocity(curTargetVelocity);}
 
-        hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+        double curVelocity = flywheelMotorR.getVelocity();
+        double error = targetVelocity - curVelocity;
 
-
-
-        telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity());
-        telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity);
-        telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS);
-        telemetry.addData("Distance", distance);
+        telemetry.addData("Target Velocity", targetVelocity);
+        telemetry.addData("Current Velocity", curVelocity);
+        telemetry.addData("Error", error);
+        telemetry.addData("Tuning P", p);
+        telemetry.addData("Tuning F", f);
 
         telemetry.update();
     }
-
-    private double getTrigBasedDistance(double targetOffsetAngleVertical) {
-        return (14) / Math.tan(Math.toRadians(2.0 + targetOffsetAngleVertical));
-    }
 }

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 EXTERNAL_GEAR_RATIO = 2.68888889;
+    public static double TICKS_PER_REV_MOTOR = 145.1;
+    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) {
-        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);
+    private double getHoodPOS(double dist) {
+        return ((-(8.57654 * Math.pow(10, -9)) * Math.pow(dist, 4)) + (0.00000166094 * Math.pow(dist, 3)) - (0.0000796795 * Math.pow(dist, 2)) + (-0.00326804 * dist) + 0.433859);
     }
 }

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

@@ -9,30 +9,36 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import org.firstinspires.ftc.teamcode.Hware.hwMap;
 import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
 
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+
 @Configurable
 @TeleOp(name = "Anchor Mode Tuner", group = "Tuning")
 public class anchorTuning extends LinearOpMode {
 
-    public static double driveP = -0.001;
+    public static double driveP = 0.0;
     public static double driveI = 0.0;
-    public static double driveD = -0.00015;
+    public static double driveD = 0.0;
 
-    public static double strafeP = -0.002;
+    public static double strafeP = 0.26;
     public static double strafeI = 0.0;
-    public static double strafeD = -0.00008;
+    public static double strafeD = 0.01;
 
-    public static double turnP = -0.08;
+    public static double turnP = 1.8;
     public static double turnI = 0.0;
-    public static double turnD = -0.0001; // Tuned for Degrees
+    public static double turnD = 0.01; // Tuned for Degrees
 
 
-    public static double MAX_POWER = 0.2;
+    public static double MAX_POWER = 0.4;
+
+    public static int targetFPS = 110;
 
 
     private DriveTrain driveTrain;
     private TelemetryManager telemetryM;
     private boolean lastPS = false;
 
+    FPSCounter fps = new FPSCounter();
+
     @Override
     public void runOpMode() throws InterruptedException {
         // Initialize Telemetry
@@ -44,11 +50,14 @@ public class anchorTuning extends LinearOpMode {
 
         telemetryM.debug("Status", "Initialized. Press PS/Guide to Toggle Anchor.");
         telemetryM.update(telemetry);
+        fps.reset();
 
         waitForStart();
 
         while (opModeIsActive()) {
             // Update PID values from Dashboard to Subsystem in real-time
+
+            fps.startLoop();
             driveTrain.updateAnchorPID(
                     driveP, driveI, driveD,
                     strafeP, strafeI, strafeD,
@@ -76,11 +85,15 @@ public class anchorTuning extends LinearOpMode {
 
             telemetryM.debug("Anchor Active", driveTrain.isAnchorActive());
 
+            telemetry.addData("FPS", fps.getAverageFps());
+
             telemetryM.debug("Error FWD (Ticks)", driveTrain.getLastForwardError());
             telemetryM.debug("Error STR (Ticks)", driveTrain.getLastStrafeError());
             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

@@ -0,0 +1,443 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+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.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import java.util.List;
+
+@Configurable
+@TeleOp(name = "Auto-Collect Testing", group = "Turret")
+public class autoCollection extends LinearOpMode {
+
+    public static double kP = 0.02;
+    public static double kI = 0.0;
+    public static double kD = 0.001;
+    public static double MAX_POWER = 0.6;
+    public static double MAX_INTEGRAL = 0.5;
+
+    public static double TICKS_PER_REV_MOTOR = 384.5;
+    public static double EXTERNAL_GEAR_RATIO = 2.68888889;
+    public static boolean MOTOR_REVERSED = true;
+
+    public static double LL_LOGIC_MULTIPLIER = 1.0;
+    public static double SOFT_LIMIT_NEG = -230;
+    public static double SOFT_LIMIT_POS = 230;
+
+    public static double RED_TARGET_OFFSET_DEGREES = 14;
+    public static double BLUE_TARGET_OFFSET_DEGREES = 17;
+    public static double LL_TARGET_OFFSET_DEGREES = -2;
+
+    public static double GOAL_RED_X = 138;
+    public static double GOAL_RED_Y = 138;
+    public static double GOAL_BLUE_X = 6;
+    public static double GOAL_BLUE_Y = 138;
+
+    public static double BLUE_OBSERVATION_ANGLE = 160;
+    public static double RED_OBSERVATION_ANGLE = -160;
+    public static double BLUE_RAY_TARGET_X = 4.0;
+    public static double RED_RAY_TARGET_X = 140.0;
+    public static double BLUE_DRIVE_TARGET_X = 8.5;
+    public static double RED_DRIVE_TARGET_X = 135.5;
+
+    public static double CLUSTER_RADIUS_DEG = 5.0;
+    public static double MIN_SNAPSHOT_WAIT_MS = 300;
+    public static double TURRET_STABLE_ERROR_DEG = 2.0;
+
+    public static int PIPELINE_GOAL_BLUE = 1;
+    public static int PIPELINE_GOAL_RED = 2;
+    public static int PIPELINE_ARTIFACTS = 3;
+
+    public static double OFFSET_SMOOTHING = 0.2;
+    public static int targetFPS = 110;
+
+    private Limelight3A limelight;
+    private DcMotorEx turretMotor;
+    private DcMotor intake;
+    private Follower follower;
+
+    private double integralSum = 0;
+    private double lastError = 0;
+    private double zeroOffsetTicks = 0;
+    private double ticksPerDegree = 0;
+    private double targetCorrectionOffsetTicks = 0;
+    private double fusedTargetTicks = 0;
+
+    private enum AutoCollectState {
+        IDLE,
+        INIT_COLLECT,
+        WAIT_FOR_TURRET_AND_VISION,
+        CALCULATE_SNAPSHOT,
+        DRIVING_TO_INTAKE,
+        PATH_RETURN,
+        DRIVING_RETURN
+    }
+
+    private AutoCollectState autoCollectState = AutoCollectState.IDLE;
+
+    private enum Alliance { RED, BLUE }
+    private Alliance currentAlliance = Alliance.BLUE;
+    private Alliance lastAlliance = null;
+
+    private Pose returnPose;
+    private final ElapsedTime sequenceTimer = new ElapsedTime();
+
+    private final ElapsedTime timer = new ElapsedTime();
+    protected FPSCounter fpsCounter = new FPSCounter();
+    protected List<LynxModule> allHubs;
+
+    public static double start_x = 72;
+    public static double start_y = 8.5;
+    public static double start_heading = 90;
+
+    @Override
+    public void runOpMode() {
+        turretMotor = hardwareMap.get(DcMotorEx.class, "turretRotation");
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        intake = hardwareMap.get(DcMotor.class, "intake");
+
+        follower = Constants.createFollower(hardwareMap);
+        follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading)));
+
+        turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        turretMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        turretMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        intake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+
+        allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+
+        applyMotorDirection();
+        recalculateTicksPerDegree();
+
+        limelight.start();
+
+        telemetry.addData("Status", "Initialized. Auto-Track is Armed.");
+        telemetry.update();
+
+        waitForStart();
+        timer.reset();
+
+        follower.startTeleopDrive();
+        fpsCounter.reset();
+
+        while (opModeIsActive()) {
+            for (LynxModule hub : allHubs) { hub.clearBulkCache(); }
+            fpsCounter.startLoop();
+
+            follower.update();
+
+            double dt = timer.seconds();
+            timer.reset();
+            if (dt < 0.001) dt = 0.001;
+
+            double rawTicks = turretMotor.getCurrentPosition();
+            double currentTicks = rawTicks - zeroOffsetTicks;
+            double currentDegrees = currentTicks / ticksPerDegree;
+
+            handleInput();
+
+            if (autoCollectState == AutoCollectState.IDLE) {
+                updateAlliancePipeline();
+            }
+
+            updateTurretTarget(currentTicks, currentDegrees);
+
+            runPidControl(currentTicks, dt);
+
+            switch (autoCollectState) {
+                case IDLE:
+                    follower.setTeleOpDrive(
+                            -gamepad1.left_stick_y,
+                            -gamepad1.left_stick_x,
+                            -gamepad1.right_stick_x,
+                            true
+                    );
+                    break;
+
+                case INIT_COLLECT:
+                    returnPose = follower.getPose();
+
+                    limelight.pipelineSwitch(PIPELINE_ARTIFACTS);
+                    sequenceTimer.reset();
+
+                    autoCollectState = AutoCollectState.WAIT_FOR_TURRET_AND_VISION;
+                    break;
+
+                case WAIT_FOR_TURRET_AND_VISION:
+                    double errorDeg = Math.abs(fusedTargetTicks - currentTicks) / ticksPerDegree;
+                    if (errorDeg <= TURRET_STABLE_ERROR_DEG && sequenceTimer.milliseconds() > MIN_SNAPSHOT_WAIT_MS) {
+                        autoCollectState = AutoCollectState.CALCULATE_SNAPSHOT;
+                    }
+                    break;
+
+                case CALCULATE_SNAPSHOT:
+                    boolean snapshotSuccess = calculateSnapshotAndPath(currentDegrees);
+                    if (snapshotSuccess) {
+                        intake.setPower(1.0);
+                        autoCollectState = AutoCollectState.DRIVING_TO_INTAKE;
+                    } else {
+                        abortAutoCollectSequence();
+                    }
+                    break;
+
+                case DRIVING_TO_INTAKE:
+                    if (!follower.isBusy()) {
+                        autoCollectState = AutoCollectState.PATH_RETURN;
+                    }
+                    break;
+
+                case PATH_RETURN:
+                    PathChain returnChain = follower.pathBuilder()
+                            .addPath(new BezierLine(follower.getPose(), returnPose))
+                            .setLinearHeadingInterpolation(follower.getPose().getHeading(), returnPose.getHeading())
+                            .build();
+
+                    follower.followPath(returnChain, true);
+                    autoCollectState = AutoCollectState.DRIVING_RETURN;
+                    break;
+
+                case DRIVING_RETURN:
+                    if (!follower.isBusy()) {
+                        abortAutoCollectSequence();
+                    }
+                    break;
+            }
+
+            updateTelemetry(currentTicks, currentDegrees);
+            sleep(fpsCounter.getSyncTime(targetFPS));
+        }
+        limelight.stop();
+    }
+
+    private boolean calculateSnapshotAndPath(double turretCurrentDegrees) {
+        LLResult result = limelight.getLatestResult();
+        if (result == null || !result.isValid()) return false;
+
+        List<LLResultTypes.ColorResult> targets = result.getColorResults();
+        if (targets == null || targets.isEmpty()) return false;
+
+        if (targets.size() > 8) {
+            targets = targets.subList(0, 8);
+        }
+
+        LLResultTypes.ColorResult bestTarget = null;
+        int bestCount = 0;
+
+        for (int i = 0; i < targets.size(); i++) {
+            LLResultTypes.ColorResult a = targets.get(i);
+            int count = 0;
+            for (int j = 0; j < targets.size(); j++) {
+                if (i == j) continue;
+                LLResultTypes.ColorResult b = targets.get(j);
+                double dx = a.getTargetXDegrees() - b.getTargetXDegrees();
+                double dy = a.getTargetYDegrees() - b.getTargetYDegrees();
+                double dist = Math.sqrt(dx * dx + dy * dy);
+                if (dist < CLUSTER_RADIUS_DEG) {
+                    count++;
+                }
+            }
+            if (count >= bestCount) {
+                bestCount = count;
+                bestTarget = a;
+            }
+        }
+
+        double sumTx = 0;
+        int clusterSize = 0;
+        for (LLResultTypes.ColorResult t : targets) {
+            double dx = bestTarget.getTargetXDegrees() - t.getTargetXDegrees();
+            double dy = bestTarget.getTargetYDegrees() - t.getTargetYDegrees();
+            double dist = Math.sqrt(dx * dx + dy * dy);
+            if (dist < CLUSTER_RADIUS_DEG) {
+                sumTx += t.getTargetXDegrees();
+                clusterSize++;
+            }
+        }
+        double avgTx = sumTx / clusterSize;
+
+        Pose robotPose = follower.getPose();
+
+        double rayHeadingRad = robotPose.getHeading() + Math.toRadians(turretCurrentDegrees) + Math.toRadians(avgTx * LL_LOGIC_MULTIPLIER);
+
+        while (rayHeadingRad > Math.PI) rayHeadingRad -= (2 * Math.PI);
+        while (rayHeadingRad < -Math.PI) rayHeadingRad += (2 * Math.PI);
+
+        double dx = (currentAlliance == Alliance.BLUE) ?
+                (BLUE_RAY_TARGET_X - robotPose.getX()) :
+                (RED_RAY_TARGET_X - robotPose.getX());
+
+        double dy = dx * Math.tan(rayHeadingRad);
+        double targetY = robotPose.getY() + dy;
+
+        if (targetY < 0 || targetY > 144) {
+            return false;
+        }
+
+        Pose targetPose;
+        if (currentAlliance == Alliance.BLUE) {
+            targetPose = new Pose(BLUE_DRIVE_TARGET_X, targetY, Math.toRadians(180));
+        } else {
+            targetPose = new Pose(RED_DRIVE_TARGET_X, targetY, Math.toRadians(0));
+        }
+
+        PathChain collectPath = follower.pathBuilder()
+                .addPath(new BezierLine(robotPose, targetPose))
+                .setLinearHeadingInterpolation(robotPose.getHeading(), targetPose.getHeading())
+                .build();
+
+        follower.followPath(collectPath, true);
+        return true;
+    }
+
+    private void updateTurretTarget(double currentTicks, double currentDegrees) {
+        if (autoCollectState == AutoCollectState.IDLE) {
+            double odoTargetTicks = calculateOdometryTargetTicks();
+            LLResult result = limelight.getLatestResult();
+
+            if (result != null && result.isValid()) {
+                double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
+                targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - OFFSET_SMOOTHING)) + (rawErrorTicks * OFFSET_SMOOTHING);
+            }
+            double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+            fusedTargetTicks = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+
+        } else if (autoCollectState == AutoCollectState.INIT_COLLECT || autoCollectState == AutoCollectState.WAIT_FOR_TURRET_AND_VISION) {
+            double obsAngle = (currentAlliance == Alliance.BLUE) ? BLUE_OBSERVATION_ANGLE : RED_OBSERVATION_ANGLE;
+            fusedTargetTicks = obsAngle * ticksPerDegree;
+
+        } else {
+            fusedTargetTicks = 0;
+            targetCorrectionOffsetTicks = 0;
+        }
+    }
+
+    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;
+
+        double dx = targetX - robotPose.getX();
+        double dy = targetY - robotPose.getY();
+
+        double targetFieldHeading = Math.atan2(dy, dx);
+        double robotHeading = robotPose.getHeading();
+
+        double relativeRad = targetFieldHeading - robotHeading;
+
+        while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
+        while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
+
+        double relativeDegrees = Math.toDegrees(relativeRad);
+        double staticOffset = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES;
+
+        return (relativeDegrees + staticOffset) * ticksPerDegree;
+    }
+
+    private void runPidControl(double currentTicks, double dt) {
+        double error = fusedTargetTicks - currentTicks;
+        double derivative = (error - lastError) / dt;
+
+        if (Math.abs(error) < (15 * ticksPerDegree)) {
+            integralSum += (error * dt);
+        } else {
+            integralSum = 0;
+        }
+
+        double integralTerm = kI * integralSum;
+        if (Math.abs(integralTerm) > MAX_INTEGRAL) {
+            integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL;
+        }
+
+        double output = (kP * error) + integralTerm + (kD * derivative);
+        output = Range.clip(output, -MAX_POWER, MAX_POWER);
+
+        turretMotor.setPower(output);
+        lastError = error;
+    }
+
+    private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
+        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 void handleInput() {
+        if (gamepad1.psWasPressed()) {
+            currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
+        }
+
+        if (gamepad1.x && autoCollectState == AutoCollectState.IDLE) {
+            autoCollectState = AutoCollectState.INIT_COLLECT;
+        }
+
+        if (gamepad1.b && autoCollectState != AutoCollectState.IDLE) {
+            abortAutoCollectSequence();
+        }
+    }
+
+    private void abortAutoCollectSequence() {
+        autoCollectState = AutoCollectState.IDLE;
+        intake.setPower(0);
+        follower.startTeleopDrive();
+        lastAlliance = null;
+        targetCorrectionOffsetTicks = 0;
+    }
+
+    private void updateAlliancePipeline() {
+        if (lastAlliance != currentAlliance) {
+            if (currentAlliance == Alliance.RED) {
+                limelight.pipelineSwitch(PIPELINE_GOAL_RED);
+            } else {
+                limelight.pipelineSwitch(PIPELINE_GOAL_BLUE);
+            }
+            lastAlliance = currentAlliance;
+        }
+    }
+
+    private void updateTelemetry(double currentTicks, double currentDegrees) {
+        telemetry.addData("State", autoCollectState);
+        telemetry.addData("Alliance", currentAlliance);
+        telemetry.addLine("------------------");
+        telemetry.addData("Target Ticks", fusedTargetTicks);
+        telemetry.addData("Current Ticks", currentTicks);
+        telemetry.addData("Error Ticks", fusedTargetTicks - currentTicks);
+        telemetry.addLine("------------------");
+        telemetry.addData("FPS", "%.2f", fpsCounter.getAverageFps());
+        telemetry.update();
+    }
+
+    private void applyMotorDirection() {
+        turretMotor.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+    }
+
+    private void recalculateTicksPerDegree() {
+        ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
+    }
+}

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

@@ -9,7 +9,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 
 import com.bylazar.telemetry.PanelsTelemetry;
 
-
+@Disabled
 @TeleOp(name = "Encoders Test", group = "Test")
 public class encoderTest extends OpMode {
 

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

@@ -1,370 +0,0 @@
-package org.firstinspires.ftc.teamcode.tuning;
-
-import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_HEIGHT_INCHES;
-import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_MOUNT_ANGLE_DEGREES;
-import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.GOAL_HEIGHT_INCHES;
-
-import com.bylazar.configurables.annotations.Configurable;
-import com.bylazar.telemetry.PanelsTelemetry;
-import com.bylazar.telemetry.TelemetryManager;
-import com.pedropathing.follower.Follower;
-import com.pedropathing.geometry.Pose;
-import com.qualcomm.hardware.limelightvision.LLResult;
-import com.qualcomm.hardware.limelightvision.Limelight3A;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-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.ElapsedTime;
-import com.qualcomm.robotcore.util.Range;
-
-import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
-import com.qualcomm.hardware.lynx.LynxModule;
-
-import java.util.List;
-
-@Configurable
-@TeleOp(name = "Flywheel Testing", group = "All")
-public class flywheelTesting extends LinearOpMode {
-
-    // --- PID Constants ---
-    public static double F = 15.6;
-    public static double P = 22.5;
-    public static double D = 0.001;
-
-    // Turret PID
-    public static double kP_LL = 0.02;
-    public static double kP_ODO = 0.035;
-    public static double kI = 0.0;
-    public static double kD_LL = 0.001;
-    public static double kD_ODO = 0.0007;
-
-    public static double MAX_POWER = 0.5;
-    public static double MAX_INTEGRAL = 0.5;
-
-    public static double TICKS_PER_REV_MOTOR = 384.5;
-    public static double EXTERNAL_GEAR_RATIO = 1.315994798439532;
-
-    // FIX: Static Motor Direction & Logic Multiplier
-    public static boolean MOTOR_REVERSED = false;
-    public static double LL_LOGIC_MULTIPLIER = -1.0;
-
-    public static double LL_TARGET_OFFSET_DEGREES = 4;
-    public static double RED_TARGET_OFFSET_DEGREES = 14;
-    public static double BLUE_TARGET_OFFSET_DEGREES = 17;
-
-    private double TARGET_OFFSET_DEGREES = BLUE_TARGET_OFFSET_DEGREES;
-
-    public static double SOFT_LIMIT_NEG = -250.0;
-    public static double SOFT_LIMIT_POS = 250.0;
-    public static double WRAP_THRESHOLD_DEGREES = 180.0;
-
-    // --- Goal Coordinates for Odometry ---
-    public static double GOAL_RED_X = 138;
-    public static double GOAL_RED_Y = 138;
-    public static double GOAL_BLUE_X = 6;
-    public static double GOAL_BLUE_Y = 138;
-
-    private double targetTicks = 0;
-    private double ticksPerDegree = 0;
-
-    // --- Hardware Constants ---
-    public static double SERVO_MIN = 0.4;
-    public static double SERVO_MAX = 1.0;
-    public static double OFFSET_GAIN = -0.0005;
-
-    private Limelight3A limelight;
-    private DcMotorEx turretRotation;
-    private DcMotorEx leftTurret, rightTurret;
-    private Servo hoodServo;
-    private Follower follower;
-
-    private TelemetryManager telemetryM;
-
-    private double integralSum = 0;
-    private double lastError = 0;
-
-    private boolean launch = false;
-    private int pipelineIndex = 1;
-
-    private final ElapsedTime loopTimer = new ElapsedTime();
-    private final ElapsedTime limelightFailureTimer = new ElapsedTime();
-
-    private double loopTimeSum = 0;
-    private int loopCount = 0;
-
-    private enum Alliance { RED, BLUE }
-    private Alliance currentAlliance = Alliance.BLUE;
-
-    private enum TrackingSource { LIMELIGHT, ODOMETRY }
-    private TrackingSource activeSource = TrackingSource.ODOMETRY;
-    private TrackingSource lastActiveSource = TrackingSource.ODOMETRY;
-
-    List<LynxModule> allHubs;
-
-    public static double start_x = 72;
-    public static double start_y = 8.5;
-    public static double start_heading = 90;
-
-    @Override
-    public void runOpMode() {
-        telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
-        allHubs = hardwareMap.getAll(LynxModule.class);
-
-        for (LynxModule hub : allHubs) {
-            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
-        }
-
-        // --- Hardware Map ---
-        turretRotation = hardwareMap.get(DcMotorEx.class, "turretRotation");
-
-        // STATIC DIRECTION CONFIGURATION
-        turretRotation.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
-
-        turretRotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        turretRotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        turretRotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-        leftTurret = hardwareMap.get(DcMotorEx.class, "lturret");
-        rightTurret = hardwareMap.get(DcMotorEx.class, "rturret");
-        hoodServo = hardwareMap.get(Servo.class, "hoodServo");
-
-        // PedroPathing Follower Setup
-        follower = Constants.createFollower(hardwareMap);
-        follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading)));
-
-        limelight = hardwareMap.get(Limelight3A.class, "limelight");
-
-        leftTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        rightTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        rightTurret.setDirection(DcMotorSimple.Direction.REVERSE);
-
-        PIDFCoefficients pidfCoefficients = new PIDFCoefficients(P, 0, D, F);
-        rightTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
-        leftTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
-
-        recalculateTicksPerDegree();
-
-        limelight.pipelineSwitch(pipelineIndex);
-        limelight.start();
-
-        telemetryM.debug("Status", "Initialized");
-        telemetryM.update(telemetry);
-
-        waitForStart();
-        loopTimer.reset();
-        limelightFailureTimer.reset();
-
-        follower.startTeleopDrive(true);
-        follower.update();
-
-        loopTimeSum = 0;
-        loopCount = 0;
-
-        while (opModeIsActive()) {
-            update();
-        }
-        limelight.stop();
-    }
-
-    private void update() {
-        for (LynxModule hub : allHubs) {
-            hub.clearBulkCache();
-        }
-
-        follower.update();
-
-        double dt = loopTimer.seconds();
-        loopTimer.reset();
-        loopTimeSum += dt;
-        loopCount++;
-        if (dt < 0.001) dt = 0.001;
-
-        // Pass movement to follower for pose updates (Robot Centric)
-        follower.setTeleOpDrive(
-                -gamepad1.left_stick_y,
-                -gamepad1.left_stick_x,
-                -gamepad1.right_stick_x,
-                true
-        );
-
-        if (gamepad1.startWasPressed()) {
-            pipelineIndex = (pipelineIndex + 1) % 3;
-            limelight.pipelineSwitch(pipelineIndex);
-        }
-
-        if (gamepad1.psWasPressed()) {
-            currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
-        }
-
-        if (gamepad1.yWasPressed()) launch = !launch;
-
-        // --- TURRET TRACKING LOGIC ---
-        if (currentAlliance == Alliance.BLUE) limelight.pipelineSwitch(1);
-        else limelight.pipelineSwitch(2);
-
-        LLResult result = limelight.getLatestResult();
-
-        double rawTicks = turretRotation.getCurrentPosition();
-        double currentDegrees = rawTicks / ticksPerDegree;
-
-        double calculatedTargetTicks = Double.NaN;
-
-        // Hybrid Logic
-        calculatedTargetTicks = calculateLimelightTargetTicks(result, currentDegrees, rawTicks);
-
-        if (Double.isNaN(calculatedTargetTicks)) {
-            if (limelightFailureTimer.seconds() > 0.5) {
-                calculatedTargetTicks = calculateOdometryTargetTicks();
-                activeSource = TrackingSource.ODOMETRY;
-            } else {
-                calculatedTargetTicks = targetTicks;
-            }
-        } else {
-            limelightFailureTimer.reset();
-            activeSource = TrackingSource.LIMELIGHT;
-        }
-
-        if (!Double.isNaN(calculatedTargetTicks)) {
-            targetTicks = calculatedTargetTicks;
-        }
-
-        if (activeSource != lastActiveSource) {
-            integralSum = 0;
-            lastError = 0;
-        }
-        lastActiveSource = activeSource;
-
-        double kP_use = (activeSource == TrackingSource.ODOMETRY) ? kP_ODO : kP_LL;
-        double kD_use = (activeSource == TrackingSource.ODOMETRY) ? kD_ODO : kD_LL;
-
-        double error = targetTicks - rawTicks;
-        double derivative = (error - lastError) / dt;
-
-        if (Math.abs(error) < (15 * ticksPerDegree)) {
-            integralSum += (error * dt);
-        } else {
-            integralSum = 0;
-        }
-
-        double integralTerm = kI * integralSum;
-        if (Math.abs(integralTerm) > MAX_INTEGRAL) {
-            integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL;
-        }
-
-        double output = (kP_use * error) + integralTerm + (kD_use * derivative);
-        output = Range.clip(output, -MAX_POWER, MAX_POWER);
-
-        turretRotation.setPower(output);
-        lastError = error;
-
-        // --- SHOOTER CALCULATIONS (Using TY) ---
-        double calculatedDistance = 0;
-        double calculatedVelocity = 0;
-        double calculatedHoodPos = 0;
-
-        if (result != null && result.isValid()) {
-            double ty = result.getTy();
-            calculatedDistance = getDistanceFromTag(ty);
-            calculatedVelocity = getFlywheelVelocity(calculatedDistance);
-            calculatedHoodPos = getHoodPOS(calculatedDistance, calculatedVelocity);
-
-            if (launch) {
-                leftTurret.setVelocity(calculatedVelocity);
-                rightTurret.setVelocity(calculatedVelocity);
-                hoodServo.setPosition(calculatedHoodPos);
-            }
-        }
-
-        if (launch) {
-            if (leftTurret.getVelocity() * 0.9 <= calculatedVelocity) {
-                // unlockTurret();
-            }
-        }
-
-        // Telemetry
-        telemetryM.debug("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
-        telemetryM.debug("Source", activeSource);
-        telemetryM.debug("Turret Angle", currentDegrees);
-        telemetryM.debug("Hood Target", calculatedHoodPos);
-        telemetryM.debug("Distance", calculatedDistance);
-        telemetryM.debug("Velocity", calculatedVelocity);
-
-        if (loopCount > 0) {
-            telemetryM.debug("▰▰▰▰▰▰▰ FPS ▰▰▰▰▰▰▰");
-            telemetryM.debug("FPS", "%.2f", 1000 / (dt * 1000));
-        }
-
-        telemetryM.update(telemetry);
-    }
-
-    private double calculateLimelightTargetTicks(LLResult result, double currentDegrees, double currentTicks) {
-        if (result != null && result.isValid()) {
-            double tx = result.getTx();
-            double calculatedTargetAngle = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
-
-            TARGET_OFFSET_DEGREES = LL_TARGET_OFFSET_DEGREES;
-            double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
-            double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
-
-            double wrapLimitTicks = WRAP_THRESHOLD_DEGREES * ticksPerDegree;
-            if (potentialTargetTicks < -wrapLimitTicks) {
-                potentialTargetTicks = SOFT_LIMIT_POS;
-            } else if (potentialTargetTicks > wrapLimitTicks) {
-                potentialTargetTicks = SOFT_LIMIT_NEG;
-            }
-
-            return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
-        }
-        return Double.NaN;
-    }
-
-    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;
-
-        double dx = targetX - robotPose.getX();
-        double dy = targetY - robotPose.getY();
-
-        double targetFieldHeading = Math.atan2(dy, dx);
-        double robotHeading = robotPose.getHeading();
-
-        TARGET_OFFSET_DEGREES = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES;
-
-        double relativeRad = targetFieldHeading - robotHeading;
-
-        while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
-        while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
-
-        double calculatedTargetAngle = Math.toDegrees(relativeRad);
-        double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
-        double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
-
-        return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
-    }
-
-    private void recalculateTicksPerDegree() {
-        ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
-    }
-
-    private double getDistanceFromTag(double x) {
-        // Uses Trigonometry (TY) instead of Area (TA)
-        return (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(Math.toRadians(CAMERA_MOUNT_ANGLE_DEGREES + x));
-    }
-
-    private double calculateHoodOffset(double target) {
-        return (target - leftTurret.getVelocity()) * OFFSET_GAIN;
-    }
-
-    private double getFlywheelVelocity(double dist) {
-        return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
-    }
-
-    private double getHoodPOS(double dist, double targetVelocity) {
-        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);
-    }
-}

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

@@ -0,0 +1,297 @@
+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 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;
+
+    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);
+        flywheelMotorL = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
+
+        flywheelMotorR.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+        flywheelMotorR.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+        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 != null && result.isValid()) {
+            double targetY = result.getTy();
+            telemetry.addData("Ty", targetY);
+            distance = getTrigBasedDistance(targetY) - distanceOffset;
+        }
+
+        boolean currentX = gamepad1.x;
+        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;
+
+        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);
+
+            lastP = P;
+            lastI = I;
+            lastD = D;
+            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 (29.5-12.75) / Math.tan(Math.toRadians(19.8 + targetOffsetAngleVertical));
+        //      ^ 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 kI = 0.0;
-    public static double kD = 0.001;
+    public static double kP = 0.0040;
+    public static double kI = 0.002;
+    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 EXTERNAL_GEAR_RATIO = 2.68888889;
-    public static boolean MOTOR_REVERSED = true;
+    public static double TICKS_PER_REV_MOTOR = 145.1;
+    public static double EXTERNAL_GEAR_RATIO = 9.179875;
+    public static boolean MOTOR_REVERSED = false;
 
     // ================= Limits & Logic =================
-    public static double LL_LOGIC_MULTIPLIER = 1.0;
-    public static double SOFT_LIMIT_NEG = -230;
-    public static double SOFT_LIMIT_POS = 230;
+    public static double LL_LOGIC_MULTIPLIER = -1.0;
+    public static double SOFT_LIMIT_NEG = -330;
+    public static double SOFT_LIMIT_POS = 350;
 
     // ================= Targeting Offsets =================
-    public static double RED_TARGET_OFFSET_DEGREES = 14;
-    public static double BLUE_TARGET_OFFSET_DEGREES = 17;
-    public static double LL_TARGET_OFFSET_DEGREES = -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;
 
     // ================= Field Coordinates =================
-    public static double GOAL_RED_X = 138;
-    public static double GOAL_RED_Y = 138;
-    public static double GOAL_BLUE_X = 6;
-    public static double GOAL_BLUE_Y = 138;
+    public static double GOAL_RED_X = 140;
+    public static double GOAL_RED_Y = 140;
+    public static double GOAL_BLUE_X = 4;
+    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))
-                    + (rawErrorTicks * OFFSET_SMOOTHING);
+            // 2. Calculate Current Fused Target: Where we currently think the goal is (Odo + existing Offset)
+            double currentFusedTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+
+            // 3. Calculate Correction Error: The delta between our current belief and camera reality
+            double correctionError = visionTargetTicks - currentFusedTarget;
+
+            // 4. Normalize the error for wrapping (360 degrees)
+            double ticksPerRev = 360.0 * ticksPerDegree;
+            while (correctionError > (ticksPerRev / 2))  correctionError -= ticksPerRev;
+            while (correctionError < -(ticksPerRev / 2)) correctionError += ticksPerRev;
+
+            // 5. Apply the error to the persistent offset (Bias-Shift)
+            // We use OFFSET_SMOOTHING as a gain to prevent single-frame noise from causing jitter
+            targetCorrectionOffsetTicks += (correctionError * OFFSET_SMOOTHING);
         }
 
         double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
 
-        fusedTargetTicks = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+        // 1. Apply initial soft limits
+        double preWrapTarget = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+
+        // 2. Implement wrapping logic: if target exceeds 180 degrees (670 ticks), normalize to the other side.
+        // This implies a full wrap cycle is 2 * 670 = 1340 ticks.
+        double fullWrapTicks = TURRET_FULL_WRAP_TICKS;
+        double halfWrapTicks = fullWrapTicks / 2.0;
+
+        // If target is beyond positive half-wrap threshold, wrap to negative equivalent
+        if (preWrapTarget > halfWrapTicks) {
+            preWrapTarget -= fullWrapTicks;
+        }
+        // If target is beyond negative half-wrap threshold, wrap to positive equivalent
+        else if (preWrapTarget < -halfWrapTicks) {
+            preWrapTarget += fullWrapTicks;
         }
 
-    private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
-        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;
+        // 3. Apply final soft limits after wrapping
+        fusedTargetTicks = Range.clip(preWrapTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
     }
 
-    private double calculateOdometryTargetTicks() {
-        Pose robotPose = follower.getPose();
+    private double calculateOdometryTargetTicks(Pose robotPose) {
         double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X;
         double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y;
 
@@ -237,6 +255,7 @@ public class odoTracking extends LinearOpMode {
 
         double output = (kP * error) + integralTerm + (kD * derivative);
         output = Range.clip(output, -MAX_POWER, MAX_POWER);
+        telemetry.addData("Power", output);
 
         turretMotor.setPower(output);
         lastError = error;

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

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

build.dependencies.gradle

@@ -17,6 +17,6 @@ dependencies {
 
     implementation 'com.pedropathing:ftc:2.0.6'
     implementation 'com.pedropathing:telemetry:1.0.0'
-    //implementation 'com.bylazar:fullpanels:1.0.9'
+    implementation 'com.bylazar:fullpanels:1.0.9'
 }
 

gradle.properties

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