Main Commit

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

@@ -0,0 +1,403 @@
+package org.firstinspires.ftc.teamcode.Hware;
+
+import android.graphics.Color;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU; // Added
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.SwitchableLight;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class hwMap {
+    public static class IntakeHwMap {
+        public DcMotor frontIntakeMotor;
+
+        public IntakeHwMap(HardwareMap hardwareMap) {
+            frontIntakeMotor = hardwareMap.dcMotor.get(Constants.IntakeConstants.FRONT_INTAKE_MOTOR);
+            frontIntakeMotor.setDirection(Constants.IntakeConstants.INTAKE_DIRECTION);
+            frontIntakeMotor.setZeroPowerBehavior(Constants.IntakeConstants.INTAKE_ZERO_POWER_BEHAVIOR);
+            frontIntakeMotor.setMode(Constants.IntakeConstants.INTAKE_RUNMODE);
+        }
+    }
+
+    public static class LedHwMap {
+        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led1;
+        public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led2;
+
+        public LedHwMap(HardwareMap hardwareMap) {
+            /*led1 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led1");
+            led2 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led2");*/
+        }
+    }
+
+    public static class LiftHwMap {
+        public Servo ptoLeft;
+        public Servo ptoRight;
+        public DcMotorEx leftFront, leftRear, rightFront, rightRear;
+
+        public LiftHwMap(HardwareMap hardwareMap) {/*
+            ptoLeft = hardwareMap.servo.get(Constants.LiftConstants.PTO_LEFT);
+            ptoRight = hardwareMap.servo.get(Constants.LiftConstants.PTO_RIGHT);
+
+            leftFront = hardwareMap.get(DcMotorEx.class, "fl");
+            leftRear = hardwareMap.get(DcMotorEx.class, "bl");
+            rightFront = hardwareMap.get(DcMotorEx.class, "fr");
+            rightRear = hardwareMap.get(DcMotorEx.class, "br");
+
+            ptoRight.setDirection(Constants.LiftConstants.PTO_RIGHT_DIR);
+            ptoLeft.setDirection(Constants.LiftConstants.PTO_LEFT_DIR);
+
+            leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);*/
+        }
+    }
+
+    public static class DriveHwMap {
+        public DcMotorEx frontLeftMotor;
+        public DcMotorEx backLeftMotor;
+        public DcMotorEx frontRightMotor;
+        public DcMotorEx backRightMotor;
+        public IMU imu;
+
+        public DriveHwMap(HardwareMap hardwareMap) {
+            frontLeftMotor = hardwareMap.get(DcMotorEx.class, Constants.DriveConstants.FRONT_LEFT_MOTOR);
+            backLeftMotor = hardwareMap.get(DcMotorEx.class, Constants.DriveConstants.BACK_LEFT_MOTOR);
+            frontRightMotor = hardwareMap.get(DcMotorEx.class, Constants.DriveConstants.FRONT_RIGHT_MOTOR);
+            backRightMotor = hardwareMap.get(DcMotorEx.class, Constants.DriveConstants.BACK_RIGHT_MOTOR);
+
+            frontLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+            backLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+            frontRightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+            backRightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+
+            imu = hardwareMap.get(IMU.class, "imu");
+            IMU.Parameters parameters = new IMU.Parameters(
+                    new RevHubOrientationOnRobot(
+                            RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
+                            RevHubOrientationOnRobot.UsbFacingDirection.UP
+                    )
+            );
+            imu.initialize(parameters);
+            imu.resetYaw();
+
+
+            resetEncoders();
+            setDriveMotorModes(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+            setDriveMotorZero(DcMotor.ZeroPowerBehavior.BRAKE);
+        }
+
+        public void setDriveMotorZero(DcMotor.ZeroPowerBehavior zero) {
+            frontLeftMotor.setZeroPowerBehavior(zero);
+            backLeftMotor.setZeroPowerBehavior(zero);
+            frontRightMotor.setZeroPowerBehavior(zero);
+            backRightMotor.setZeroPowerBehavior(zero);
+        }
+        public void setMotorTargetPositions(double inches){
+            setDriveMotorModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            setDriveMotorModes(DcMotor.RunMode.RUN_USING_ENCODER);
+            frontLeftMotor.setTargetPosition(-inchesToTicks(inches));
+            frontRightMotor.setTargetPosition(inchesToTicks(inches));
+            backLeftMotor.setTargetPosition(-inchesToTicks(inches));
+            backRightMotor.setTargetPosition(inchesToTicks(inches));
+
+        }
+        public void setDriveMotorModes(DcMotor.RunMode mode) {
+            frontLeftMotor.setMode(mode);
+            backLeftMotor.setMode(mode);
+            frontRightMotor.setMode(mode);
+            backRightMotor.setMode(mode);
+        }
+
+        public void setMotorPowers(double frontLeftPower, double frontRightPower,
+                                   double backLeftPower, double backRightPower) {
+            frontLeftMotor.setPower(frontLeftPower);
+            frontRightMotor.setPower(frontRightPower);
+            backLeftMotor.setPower(backLeftPower);
+            backRightMotor.setPower(backRightPower);
+        }
+
+
+        public void stopMotors() {
+            setMotorPowers(0, 0, 0, 0);
+        }
+
+        public void resetEncoders() {
+            setDriveMotorModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            setDriveMotorModes(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        }
+
+        public int[] getEncoderPositions() {
+            return new int[] {
+                    frontLeftMotor.getCurrentPosition(),
+                    frontRightMotor.getCurrentPosition(),
+                    backLeftMotor.getCurrentPosition(),
+                    backRightMotor.getCurrentPosition()
+            };
+        }
+
+        private int inchesToTicks(double inches) {
+            double wheelDiameter = Constants.DriveConstants.WHEEL_DIAMETER;  // in inches
+            double ticksPerRev = Constants.DriveConstants.TICKS_PER_REVOLUTION;
+            double gearRatio = 2.0;
+            double circumference = Math.PI * wheelDiameter;
+
+            return (int) (inches * (ticksPerRev * gearRatio) / circumference);
+        }
+    }
+
+    public static class TransferHwMap {
+        public Servo Limiter;
+
+        public NormalizedColorSensor indexA;
+        public NormalizedColorSensor indexB;
+        public NormalizedColorSensor indexC;
+
+        public NormalizedColorSensor[] sensors;
+
+        public TransferHwMap(HardwareMap hardwareMap) {
+            Limiter = hardwareMap.servo.get("limiter");
+
+
+            indexA = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_A);
+            indexB = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_B);
+            indexC = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_C);
+
+            Limiter.setDirection(Constants.TransferConstants.LIMITER_SERVO_DIR);
+
+            this.sensors = new NormalizedColorSensor[]{ indexA, indexB, indexC };
+
+
+            float gainFactor = 30.0f;
+
+            for (NormalizedColorSensor sensor : sensors) {
+                if (sensor instanceof SwitchableLight) ((SwitchableLight) sensor).enableLight(true);
+                sensor.setGain(gainFactor);
+            }
+        }
+
+        public void resetLimiter() {
+            setLimiter(false);
+        }
+
+        public double getServoPos(int input) {
+            return Limiter.getPosition();
+        }
+
+        public void setLimiter(boolean limit) {
+            Limiter.setPosition(limit ? Constants.TransferConstants.LIMIT_POS : Constants.TransferConstants.UNLOCK_POS);
+        }
+
+        public int detectArtifactColor(int index) {
+            if(index < 1 || index > 3) return 0;
+            int i = index - 1;
+
+            NormalizedRGBA colors2 = sensors[i].getNormalizedColors();
+
+            float[] hsv = new float[3];
+            Color.colorToHSV(colors2.toColor(), hsv);
+
+            int color = detectColor(hsv[0], hsv[1], hsv[2]);
+
+            double confidence = hsv[1] * hsv[2];
+
+            // Decision Matrix
+            if (color == 0 && confidence == 0) return 0;
+            else {
+                return color;
+            }
+
+        }
+
+        private int detectColor(float hue, float sat, float val) {
+            if (sat < 0.15 || val < 0.1) return 0;
+
+            if (hue > 170 && hue < 310) return 1;
+            if (hue > 80 && hue < 170) return 2;
+
+            return 0;
+        }
+    }
+
+    public static class TurretHwMap {
+
+        public Limelight3A limelight;
+
+        public DcMotorEx turretLeftMotor;
+        public DcMotorEx turretRightMotor;
+
+        public DcMotorEx turretEncoder; // External encoder for flywheels if needed, or specific port
+
+        public DcMotor turretrotation;
+        public Servo hoodservo;
+
+        private double lastKnownDistance = 0.0;
+        private ElapsedTime noTargetTimer = new ElapsedTime();
+
+        PIDFCoefficients pidfCoefficients = new PIDFCoefficients(22.5, 0, 0.001, 15.6);
+
+        public TurretHwMap(HardwareMap hardwareMap) {
+            noTargetTimer.reset();
+
+            // --- Flywheel Motors ---
+            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);
+
+            turretRightMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+            turretLeftMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+
+            turretRightMotor.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
+            turretLeftMotor.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
+
+            // --- Turret Rotation Motor ---
+            turretrotation = hardwareMap.dcMotor.get(Constants.TurretConstants.TURRET_ROTATION_MOTOR);
+            turretrotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+            // --- Hood Servo ---
+            hoodservo = hardwareMap.servo.get(Constants.TurretConstants.HOOD_TURRET_SERVO);
+            hoodservo.setDirection(Servo.Direction.FORWARD);
+
+            // --- 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 double getTurretRotationPosition() {
+            return turretrotation.getCurrentPosition();
+        }
+
+        public void setTurretRotationPower(double power) {
+            turretrotation.setPower(power);
+        }
+
+        public void stopTurretRotation() {
+            turretrotation.setPower(0);
+        }
+
+        public void setTurretVelocity(double velocity) {
+            turretLeftMotor.setVelocity(-velocity);
+            turretRightMotor.setVelocity(velocity);
+        }
+
+        public double[] getFlywheelVelocities() {
+            double[] velArray = {turretLeftMotor.getVelocity(), turretRightMotor.getVelocity()};
+            return velArray;
+        }
+
+        public double getTurretVelocity(){
+            return turretLeftMotor.getVelocity();
+        }
+
+        public void setTurretPower(double power) {
+            turretLeftMotor.setPower(power);
+            turretRightMotor.setPower(-power);
+        }
+
+        public void turretOff() {
+            turretLeftMotor.setPower(0);
+            turretRightMotor.setPower(0);
+        }
+
+        public void setHoodPos(double pos) {
+            hoodservo.setPosition(pos);
+        }
+
+        public LLResult getLimelightResult() {
+            return limelight.getLatestResult();
+        }
+
+        public boolean hasTarget() {
+            LLResult result = limelight.getLatestResult();
+            return result != null && result.isValid();
+        }
+
+        public double getTargetTX() {
+            LLResult result = limelight.getLatestResult();
+            if (result != null && result.isValid()) {
+                return result.getTx();
+            }
+            return 0;
+        }
+
+        public double getTargetTY() {
+            LLResult result = limelight.getLatestResult();
+            if (result != null && result.isValid()) {
+                return result.getTy();
+            }
+            return 0;
+        }
+
+        public double getTargetTA() { // Area
+            LLResult result = limelight.getLatestResult();
+            if (result != null && result.isValid()) {
+                return result.getTa();
+            }
+            return 0;
+        }
+
+        public double getDistanceFromAprilTag() {
+            if (hasTarget()) {
+                double x = getTargetTA();
+                if (x > 0) {
+                    double currentDistance = (74.34095 * Math.pow(x, -0.518935)) + 4;
+                    lastKnownDistance = currentDistance;
+                    noTargetTimer.reset();
+                    return currentDistance;
+                }
+            }
+            if (noTargetTimer.seconds() < 5.0) {
+                return lastKnownDistance;
+            }
+            return 0.0;
+        }
+
+        public void setPipeline(int pipelineIndex) {
+            if (pipelineIndex >= 0 && pipelineIndex <= 2) {
+                limelight.pipelineSwitch(pipelineIndex);
+            }
+        }
+
+        private void initLimelight(HardwareMap hardwareMap) {
+            limelight = hardwareMap.get(Limelight3A.class, "limelight");
+            limelight.pipelineSwitch(0);
+            limelight.start();
+        }
+
+        public void shutdown() {
+            if (limelight != null) {
+                limelight.stop();
+            }
+        }
+
+        public boolean isTurretReady(double velocity) {
+            return velocity >= getTurretVelocity() * 0.9;
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Color.java

@@ -0,0 +1,56 @@
+/*   MIT License
+ *   Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+public class Color {
+    public int red;
+    public int green;
+    public int blue;
+
+    public Color(int red, int green, int blue)
+    {
+        this.red = Math.min(red, 255);
+        this.green = Math.min(green, 255);
+        this.blue = Math.min(blue, 255);
+    }
+
+    @Override
+    public String toString()
+    {
+        return String.format("%d, %d, %d", red, green, blue);
+    }
+
+    public static final Color RED         = new Color(255, 0,   0);
+    public static final Color ORANGE      = new Color(255, 165, 0);
+    public static final Color YELLOW      = new Color(255, 255, 0);
+    public static final Color OLIVE       = new Color(128, 128, 0);
+    public static final Color GREEN       = new Color(0,   255, 0);
+    public static final Color CYAN        = new Color(0,   255, 255);
+    public static final Color BLUE        = new Color(0,   0,   255);
+    public static final Color TEAL        = new Color(0,   128, 128);
+    public static final Color MAGENTA     = new Color(255, 0,   255);
+    public static final Color PURPLE      = new Color(128, 0,   128);
+    public static final Color PINK        = new Color(255, 20,  128);
+    public static final Color WHITE       = new Color(255, 255, 255);
+    public static final Color TRANSPARENT = new Color(0,   0,   0);
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Direction.java

@@ -0,0 +1,28 @@
+/*   MIT License
+ *   Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+public enum Direction {
+    Forward,
+    Backward
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismDriver.java

@@ -0,0 +1,441 @@
+/*   MIT License
+ *   Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt;
+import static com.qualcomm.robotcore.util.TypeConversion.unsignedByteToInt;
+
+import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
+import com.qualcomm.robotcore.hardware.I2cAddr;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
+import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
+
+import com.qualcomm.robotcore.util.TypeConversion;
+
+import java.nio.ByteOrder;
+import java.util.Arrays;
+import java.util.concurrent.TimeUnit;
+
+@I2cDeviceType
+@DeviceProperties(
+        name = "goBILDA® Prism RGB LED Driver",
+        xmlTag = "goBILDAPrism",
+        description ="Prism RGB LED Driver (6-30V Input, I²C / PWM Control)"
+)
+public class GoBildaPrismDriver extends I2cDeviceSynchDevice<I2cDeviceSynchSimple> {
+    private static final byte DEFAULT_ADDRESS = 0x38;
+    private final int MAXIMUM_NUMBER_OF_ANIMATIONS = 10;
+    private final int MAXIMUM_NUMBER_OF_ANIMATION_GROUPS = 8;
+    private PrismAnimations.AnimationBase[] animations = new PrismAnimations.AnimationBase[MAXIMUM_NUMBER_OF_ANIMATIONS];
+
+    //#region Public Types
+    public enum LayerHeight
+    {
+        LAYER_0 (Register.ANIMATION_SLOT_0),
+        LAYER_1 (Register.ANIMATION_SLOT_1),
+        LAYER_2 (Register.ANIMATION_SLOT_2),
+        LAYER_3 (Register.ANIMATION_SLOT_3),
+        LAYER_4 (Register.ANIMATION_SLOT_4),
+        LAYER_5 (Register.ANIMATION_SLOT_5),
+        LAYER_6 (Register.ANIMATION_SLOT_6),
+        LAYER_7 (Register.ANIMATION_SLOT_7),
+        LAYER_8 (Register.ANIMATION_SLOT_8),
+        LAYER_9 (Register.ANIMATION_SLOT_9),
+        DISABLED(Register.NULL);
+
+        /* Package Private */ final Register register;
+        /* Package Private */ final int index;
+
+        LayerHeight(Register register){
+            this.register = register;
+            if(register == Register.NULL)
+                this.index = -1;
+            else
+                this.index = register.address - Register.ANIMATION_SLOT_0.address;
+        }
+    }
+
+    public enum Artboard
+    {
+        ARTBOARD_0 (0,0),
+        ARTBOARD_1 (1,1),
+        ARTBOARD_2 (2,2),
+        ARTBOARD_3 (3,3),
+        ARTBOARD_4 (4,4),
+        ARTBOARD_5 (5,5),
+        ARTBOARD_6 (6,6),
+        ARTBOARD_7 (7,7);
+
+        /* Package Private */ final byte bitmask;
+        public final int index;
+
+        Artboard(int val,int index){
+            this.bitmask = (byte)(1 << val);
+            this.index = index;
+        }
+    }
+    //#endregion
+
+    //#region Package-Private types
+    /**
+     * Captures the length of each type of register used on the device.
+     */
+    /* Package Private */ enum RegisterType
+    {
+        INT8(1,255),
+        INT16(2, 65535),
+        INT24(3, 16777215),
+        INT32(4, 2147483647);
+
+        /* Package Private */ final int lengthBytes;
+        /* Package Private */ final int maxValue;
+
+        RegisterType(int lengthBytes, int maxValue){
+            this.lengthBytes = lengthBytes;
+            this.maxValue = maxValue;
+        }
+    }
+
+    /* Package Private */ enum RegisterAccess
+    {
+        READ_ONLY,
+        WRITE_ONLY,
+        READ_AND_WRITE;
+    }
+
+    /**
+     * Register map, including register address and register type
+     */
+    /* Package Private */ enum Register
+    {
+        DEVICE_ID         (0 , RegisterType.INT8 , RegisterAccess.READ_ONLY),
+        FIRMWARE_VERSION  (1 , RegisterType.INT24, RegisterAccess.READ_ONLY),
+        HARDWARE_VERSION  (2 , RegisterType.INT16, RegisterAccess.READ_ONLY),
+        POWER_CYCLE_COUNT (3 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+        RUNTIME_IN_MINUTES(4 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+        STATUS            (5 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+        CONTROL           (6 , RegisterType.INT32, RegisterAccess.WRITE_ONLY),
+        ARTBOARD_CONTROL  (7 , RegisterType.INT32, RegisterAccess.WRITE_ONLY),
+        ANIMATION_SLOT_0  (8 , RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_1  (9 , RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_2  (10, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_3  (11, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_4  (12, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_5  (13, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_6  (14, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_7  (15, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_8  (16, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        ANIMATION_SLOT_9  (17, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+        NULL              (18, RegisterType.INT8 , RegisterAccess.READ_ONLY);
+
+        /* Package Private */ final int address;
+        /* Package Private */ final RegisterType registerType;
+        /* Package Private */ final RegisterAccess registerAccess;
+
+        Register(int address, RegisterType registerType, RegisterAccess registerAccess){
+            this.address = address;
+            this.registerType = registerType;
+            this.registerAccess = registerAccess;
+        }
+    }
+
+    private int readInt(Register register){
+        return byteArrayToInt(deviceClient.read(register.address,register.registerType.lengthBytes),ByteOrder.LITTLE_ENDIAN);
+    }
+
+    //#endregion
+
+    public GoBildaPrismDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned)
+    {
+        super(deviceClient, deviceClientIsOwned);
+
+        this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS));
+        super.registerArmingStateCallback(false);
+    }
+
+    @Override
+    public Manufacturer getManufacturer()
+    {
+        return Manufacturer.GoBilda;
+    }
+
+    @Override
+    protected synchronized boolean doInitialize()
+    {
+        ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
+        return true;
+    }
+
+    @Override
+    public String getDeviceName()
+    {
+        return "goBILDA® Prism RGB LED Driver";
+    }
+
+    /**
+     * @return 3 if device is functional.
+     */
+    public int getDeviceID(){
+        //return readInt(Register.DEVICE_ID);
+        byte[] packet = deviceClient.read(Register.DEVICE_ID.address, Register.DEVICE_ID.registerType.lengthBytes);
+        return packet[0];
+    }
+
+    /**
+     * @return Firmware Version of device. Array[0] is the major version, Array[1] is the minor version
+     * Array[2] is the patch version.
+     */
+    public int[] getFirmwareVersion(){
+        byte[] packet = deviceClient.read(Register.FIRMWARE_VERSION.address,Register.FIRMWARE_VERSION.registerType.lengthBytes);
+        int[] output = new int[3];
+        output[0] = packet[2];
+        output[1] = packet[1];
+        output[2] = packet[0];
+        return output;
+    }
+
+    /**
+     * @return Hardware version of the device as a string.
+     */
+    public String getFirmwareVersionString(){
+        byte[] packet = deviceClient.read(Register.FIRMWARE_VERSION.address,Register.FIRMWARE_VERSION.registerType.lengthBytes);
+        int[] output = new int[3];
+        output[0] = packet[2];
+        output[1] = packet[1];
+        output[2] = packet[0];
+        return String.format("%d.%d.%d", output[0], output[1], output[2]);
+    }
+
+    /**
+     * @return Hardware version of device. Array[0] is the major version, Array[1] is the minor version.
+     */
+    public int[] getHardwareVersion(){
+        byte[] packet = deviceClient.read(Register.HARDWARE_VERSION.address,Register.HARDWARE_VERSION.registerType.lengthBytes);
+        int[] output = new int[2];
+        output[0] = packet[1];
+        output[1] = packet[0];
+        return output;
+    }
+    public String getHardwareVersionString(){
+        byte[] packet = deviceClient.read(Register.HARDWARE_VERSION.address,Register.HARDWARE_VERSION.registerType.lengthBytes);
+        int[] output = new int[2];
+        output[0] = packet[1];
+        output[1] = packet[0];
+        return String.format("%d.%d", output[0], output[1]);
+    }
+
+    /**
+     * @return The number of times the device has power cycled in its lifetime.
+     */
+    public int getPowerCycleCount(){
+        return readInt(Register.POWER_CYCLE_COUNT);
+    }
+
+    /**
+     * @return Total device runtime in minutes
+     */
+    public long getRunTime(TimeUnit timeUnit){
+        return timeUnit.convert(readInt(Register.RUNTIME_IN_MINUTES),TimeUnit.MINUTES);
+    }
+
+    /**
+     * @return total LEDs in strip
+     */
+    public int getNumberOfLEDs(){
+       byte[] packet = deviceClient.read(Register.STATUS.address,Register.STATUS.registerType.lengthBytes);
+       return unsignedByteToInt(packet[0]);
+    }
+
+    /**
+     * @return Current Animation frames per second
+     */
+    public int getCurrentFPS(){
+        byte[] inputPacket = deviceClient.read(Register.STATUS.address,Register.STATUS.registerType.lengthBytes);
+        byte[] outputPacket = new byte[4];
+        outputPacket[0] = inputPacket[1];
+        outputPacket[1] = inputPacket[2];
+
+        return byteArrayToInt(outputPacket,ByteOrder.LITTLE_ENDIAN);
+    }
+
+    /**
+     * @return
+     */
+    public boolean fpsLimited(){
+        byte[] packet = deviceClient.read(Register.STATUS.address, Register.STATUS.registerType.lengthBytes);
+        //return packet[3] >> 7;
+        return false;
+    }
+
+    /**
+     * return What artboard is set as default boot animation?
+     */
+    public int getBootAnimationArtboard(){
+        byte[] packet = deviceClient.read(Register.STATUS.address, Register.STATUS.registerType.lengthBytes);
+        return 0;
+    }
+
+
+    /**
+     * Inserts an animation into the specified slot in the animations array.
+     * 
+     * @param height the height where the animation should be inserted (0-9)
+     * @param animation the AnimationBase object to insert into the array
+     * @return true if the animation was successfully inserted, false if the index 
+     *         is out of bounds or the animation is null
+     */
+    public boolean insertAnimation(LayerHeight height, PrismAnimations.AnimationBase animation)
+    {
+        if(height == LayerHeight.DISABLED || animation == null)
+            return false;
+
+        animations[height.index] = animation;
+        animations[height.index].layerHeight = height;
+        return true;
+    }
+
+    /**
+     * Inserts an animation at the specified index and immediately updates it over I2C.
+     * 
+     * @param height the height where the animation should be inserted and updated (0-9)
+     * @param animation the AnimationBase object to insert and update
+     * @return true if both insertion and update operations were successful, 
+     *         false if either operation failed
+     */
+    public boolean insertAndUpdateAnimation(LayerHeight height, PrismAnimations.AnimationBase animation)
+    {
+        if(insertAnimation(height, animation))
+            return updateAnimationFromIndex(height, true);
+        return false;
+    }
+
+    /**
+     * Updates all animations in the manager by sending their data over I2C.
+     * Iterates through all animation slots and updates any non-null animations.
+     * 
+     * @return true if the update process completed (currently always returns true)
+     */
+    public boolean updateAllAnimations()
+    {
+        for(int i = 0; i < MAXIMUM_NUMBER_OF_ANIMATIONS; i++){
+            if(animations[i] != null && animations[i].layerHeight != LayerHeight.DISABLED)
+                animations[i].updateAnimationOverI2C(this.deviceClient, false);
+        }
+        return true;    
+    }
+    
+    /**
+     * Updates a specific animation at the given Layer Height by sending its data over I2C.
+     * 
+     * @param height the height of the animation to update (0-9)
+     * @return true if the animation was successfully updated, false if the index 
+     *         is out of bounds or if no animation exists at the specified index
+     */
+    public boolean updateAnimationFromIndex(LayerHeight height)
+    {
+        return updateAnimationFromIndex(height, false);
+    }
+
+    public void clearAllAnimations()
+    {
+        byte[] packet = TypeConversion.intToByteArray(1 << 25, ByteOrder.LITTLE_ENDIAN);
+
+        deviceClient.write(Register.CONTROL.address, packet);
+        Arrays.fill(animations, null);
+    }
+
+    public void setTargetFPS(int targetFPS)
+    {
+        final int boundedTargetFps = Math.max(0, Math.min(targetFPS, 0x7FFF));
+        final int ChangeTargetFpsBit = 1 << 15;
+        final int ChangeTargetFpsCommand = ChangeTargetFpsBit | boundedTargetFps;
+        byte[] packet = TypeConversion.intToByteArray(ChangeTargetFpsCommand, ByteOrder.LITTLE_ENDIAN);
+        deviceClient.write(Register.CONTROL.address, packet);
+    }
+
+    public void setStripLength(int stripLength)
+    {
+        final int boundedStripLength = Math.max(0, Math.min(stripLength, 0xFF));
+        final int ChangeStripLengthBit = 1 << 24;
+        final int ChangeStripLengthCommand = ChangeStripLengthBit | (boundedStripLength << 16);
+        byte[] packet = TypeConversion.intToByteArray(ChangeStripLengthCommand, ByteOrder.LITTLE_ENDIAN);
+        deviceClient.write(Register.CONTROL.address, packet);
+    }
+
+    public void saveCurrentAnimationsToArtboard(Artboard artboard)
+    {
+        byte[] data = {
+            artboard.bitmask,
+            0,
+            0,
+            0
+        };
+        deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+    }
+
+    public void loadAnimationsFromArtboard(Artboard artboard)
+    {
+        byte[] data = {
+            0,
+            artboard.bitmask,
+            0,
+            0
+        };
+        deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+    }
+
+    public void setDefaultBootArtboard(Artboard artboard)
+    {
+        byte[] data = {
+            0,
+            0,
+            artboard.bitmask,
+            0
+        };
+        deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+    }
+
+    public void enableDefaultBootArtboard(boolean enable)
+    {
+        byte[] data = {
+                0,
+                0,
+                0,
+                enable ? (byte)0b00000001 : (byte)0b00000010
+        };
+        deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+    }
+
+    private boolean updateAnimationFromIndex(LayerHeight height, boolean isBeingInserted)
+    {
+        if(height == LayerHeight.DISABLED || animations[height.index] == null)
+            return false;
+
+        boolean animationEnabled = animations[height.index].layerHeight != LayerHeight.DISABLED;
+        if(animationEnabled)
+            animations[height.index].updateAnimationOverI2C(this.deviceClient, isBeingInserted);
+        return animationEnabled;
+    }
+}

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

@@ -0,0 +1,860 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
+import com.pedropathing.util.Timer;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import java.util.List;
+
+public class Auton {
+
+    // ==================================================================================
+    //                                  BLUE ALLIANCE
+    // ==================================================================================
+
+    @Autonomous(name = "Pedro Blue Far", group = "Pedro", preselectTeleOp = "Blue Final")
+    public static class BlueFar extends BaseAuto {
+        public BlueFar() {
+            super(1, false, true); // Alliance 1 (Blue), isNet=false, useTimer=true
+        }
+
+        @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())
+                    .build();
+
+            grabSet1Path = follower.pathBuilder()
+                    .addPath(new BezierLine(scorePosePreload, set1Inter))
+                    .setLinearHeadingInterpolation(scorePosePreload.getHeading(), Math.toRadians(180))
+                    .addPath(new BezierLine(set1Inter, set1Pick))
+                    .setConstantHeadingInterpolation(Math.toRadians(180))
+                    .build();
+
+            scoreSet1Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set1Pick, scoreSet1Control, scorePoseGeneric))
+                    .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading())
+                    .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))
+                    .build();
+
+            scoreEndGamePath = follower.pathBuilder()
+                    .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric))
+                    .setLinearHeadingInterpolation(Math.toRadians(-160), scorePoseGeneric.getHeading())
+                    .build();
+
+            parkPath = follower.pathBuilder()
+                    .addPath(new BezierLine(scorePoseGeneric, parkPose))
+                    .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), parkPose.getHeading())
+                    .build();
+        }
+    }
+
+    @Autonomous(name = "Pedro Blue Net", group = "Pedro", preselectTeleOp = "Blue Final")
+    public static class BlueNet extends BaseAuto {
+        public BlueNet() {
+            super(1, true, true); // Alliance 1 (Blue), isNet=true, useTimer=true
+        }
+
+        @Override
+        public void buildPaths() {
+            startPose = new Pose(31.5, 137, Math.toRadians(180));
+            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())
+                    .build();
+
+            driveToSet2Path = follower.pathBuilder()
+                    .addPath(new BezierLine(preloadScorePose, set2DrivePose))
+                    .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading())
+                    .build();
+
+            grabSet2Path = follower.pathBuilder()
+                    .addPath(new BezierLine(set2DrivePose, set2PickPose))
+                    .setTangentHeadingInterpolation()
+                    .build();
+
+            scoreSet2Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose))
+                    .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading())
+                    .build();
+
+            grabGate1Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set2ScorePose, gate1PickControl, gate1PickPose))
+                    .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading())
+                    .build();
+
+            scoreGate1Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose))
+                    .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading())
+                    .build();
+
+            grabGate2Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose))
+                    .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading())
+                    .build();
+
+            scoreGate2Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose))
+                    .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading())
+                    .build();
+
+            driveToSet3Path = follower.pathBuilder()
+                    .addPath(new BezierLine(gate2ScorePose, set3DrivePose))
+                    .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading())
+                    .build();
+
+            grabSet3Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose))
+                    .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading())
+                    .build();
+
+            scoreSet3Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose))
+                    .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading())
+                    .build();
+
+            driveToSet1Path = follower.pathBuilder()
+                    .addPath(new BezierLine(set3ScorePose, set1DrivePose))
+                    .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading())
+                    .build();
+
+            grabSet1Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose))
+                    .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading())
+                    .build();
+
+            scoreSet1Path = follower.pathBuilder()
+                    .addPath(new BezierLine(set1PickPose, set1ScorePose))
+                    .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading())
+                    .build();
+
+            parkPath = follower.pathBuilder()
+                    .addPath(new BezierLine(set1ScorePose, parkTargetPose))
+                    .setConstantHeadingInterpolation(set1ScorePose.getHeading())
+                    .build();
+        }
+    }
+
+    // ==================================================================================
+    //                                  RED ALLIANCE
+    // ==================================================================================
+
+    @Autonomous(name = "Pedro Red Far", group = "Pedro", preselectTeleOp = "Red Final")
+    public static class RedFar extends BaseAuto {
+        public RedFar() {
+            super(2, false, true); // Alliance 2 (Red), isNet=false, useTimer=true
+        }
+
+        @Override
+        public void buildPaths() {
+            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))
+                    .build();
+
+            grabSet1Path = follower.pathBuilder()
+                    .addPath(new BezierLine(scorePosePreload, set1Inter))
+                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
+                    .addPath(new BezierLine(set1Inter, set1Pick))
+                    .setConstantHeadingInterpolation(Math.toRadians(0))
+                    .build();
+
+            scoreSet1Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set1Pick, scoreSet1Control, scorePoseGeneric))
+                    .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))
+                    .build();
+
+            scoreEndGamePath = follower.pathBuilder()
+                    .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric))
+                    .setLinearHeadingInterpolation(Math.toRadians(340), Math.toRadians(62))
+                    .build();
+
+            parkPath = follower.pathBuilder()
+                    .addPath(new BezierLine(scorePoseGeneric, parkPose))
+                    .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(45))
+                    .build();
+        }
+    }
+
+    @Autonomous(name = "Pedro Red Net", group = "Pedro", preselectTeleOp = "Red Final")
+    public static class RedNet extends BaseAuto {
+        public RedNet() {
+            super(2, true, true); // Alliance 2 (Red), isNet=true, useTimer=true
+        }
+
+        @Override
+        public void buildPaths() {
+            startPose = new Pose(112.5, 137, Math.toRadians(0));
+            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())
+                    .build();
+
+            driveToSet2Path = follower.pathBuilder()
+                    .addPath(new BezierLine(preloadScorePose, set2DrivePose))
+                    .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading())
+                    .build();
+
+            grabSet2Path = follower.pathBuilder()
+                    .addPath(new BezierLine(set2DrivePose, set2PickPose))
+                    .setTangentHeadingInterpolation()
+                    .build();
+
+            scoreSet2Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose))
+                    .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading())
+                    .build();
+
+            grabGate1Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set2ScorePose, gate1PickControl, gate1PickPose))
+                    .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading())
+                    .build();
+
+            scoreGate1Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose))
+                    .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading())
+                    .build();
+
+            grabGate2Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose))
+                    .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading())
+                    .build();
+
+            scoreGate2Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose))
+                    .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading())
+                    .build();
+
+            driveToSet3Path = follower.pathBuilder()
+                    .addPath(new BezierLine(gate2ScorePose, set3DrivePose))
+                    .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading())
+                    .build();
+
+            grabSet3Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose))
+                    .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading())
+                    .build();
+
+            scoreSet3Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose))
+                    .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading())
+                    .build();
+
+            driveToSet1Path = follower.pathBuilder()
+                    .addPath(new BezierLine(set3ScorePose, set1DrivePose))
+                    .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading())
+                    .build();
+
+            grabSet1Path = follower.pathBuilder()
+                    .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose))
+                    .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading())
+                    .build();
+
+            scoreSet1Path = follower.pathBuilder()
+                    .addPath(new BezierLine(set1PickPose, set1ScorePose))
+                    .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading())
+                    .build();
+
+            parkPath = follower.pathBuilder()
+                    .addPath(new BezierLine(set1ScorePose, parkTargetPose))
+                    .setConstantHeadingInterpolation(set1ScorePose.getHeading())
+                    .build();
+        }
+    }
+
+    // ==================================================================================
+    //                                  BASE CLASS
+    // ==================================================================================
+
+    @Configurable
+    public static abstract class BaseAuto extends LinearOpMode {
+        protected Follower follower;
+        protected List<LynxModule> allHubs;
+        protected Timer pathTimer, actionTimer, opmodeTimer;
+        protected pedroStateMachine systems;
+
+        protected final int allianceTarget;
+        protected final boolean isNet;
+        protected final boolean useTimer;
+
+        public static double timerTimeout = 4;
+        public static double gateWaitTime = 3.0;
+
+        protected Pose startPose;
+
+        // Shared/Common Path Chains
+        protected PathChain scorePreloadPath;
+        protected PathChain grabSet1Path, scoreSet1Path;
+        protected PathChain grabSet2Path, scoreSet2Path;
+        protected PathChain parkPath;
+
+        // Far Specific Path Chains
+        protected PathChain grabEndGamePath, scoreEndGamePath;
+
+        // Net Specific Path Chains
+        protected PathChain driveToSet2Path;
+        protected PathChain grabGate1Path, scoreGate1Path;
+        protected PathChain grabGate2Path, scoreGate2Path;
+        protected PathChain driveToSet3Path, grabSet3Path, scoreSet3Path;
+        protected PathChain driveToSet1Path;
+
+        public enum PathState {
+            DRIVE_TO_SCORE_PRELOAD, SCORE_PRELOAD,
+            DRIVE_TO_GRAB_SET_1, GRAB_SET_1,
+            DRIVE_TO_SCORE_SET_1, SCORE_SET_1,
+            DRIVE_TO_GRAB_SET_2, GRAB_SET_2,
+            DRIVE_TO_SCORE_SET_2, SCORE_SET_2,
+            DRIVE_TO_ENDGAME_COLLECTION, // Corner sequence
+            DO_ENDGAME_COLLECTION,
+            DRIVE_TO_SCORE_ENDGAME,
+            SCORE_ENDGAME,
+            DRIVE_TO_PARK, DONE,
+
+            // Net specific
+            GRAB_GATE_1, WAIT_GATE_1, SCORE_GATE_1,
+            GRAB_GATE_2, WAIT_GATE_2, SCORE_GATE_2,
+            DRIVE_TO_GRAB_SET_3, GRAB_SET_3,
+            DRIVE_TO_SCORE_SET_3, SCORE_SET_3
+        }
+
+        protected PathState pathState;
+
+        public BaseAuto(int allianceTarget, boolean isNet, boolean useTimer) {
+            this.allianceTarget = allianceTarget;
+            this.isNet = isNet;
+            this.useTimer = useTimer;
+        }
+
+        public abstract void buildPaths();
+
+        public void setPathState(PathState newPathState) {
+            pathState = newPathState;
+            pathTimer.resetTimer();
+            actionTimer.resetTimer();
+        }
+
+        @Override
+        public void runOpMode() throws InterruptedException {
+            pathTimer = new Timer();
+            actionTimer = new Timer();
+            opmodeTimer = new Timer();
+            follower = Constants.createFollower(hardwareMap);
+
+            allHubs = hardwareMap.getAll(LynxModule.class);
+            for (LynxModule hub : allHubs) {
+                hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+            }
+
+            // Subclass sets startPose and paths
+            buildPaths();
+
+            hwMap.IntakeHwMap intakeHw = new hwMap.IntakeHwMap(hardwareMap);
+            hwMap.TransferHwMap transferHw = new hwMap.TransferHwMap(hardwareMap);
+            hwMap.TurretHwMap turretHw = new hwMap.TurretHwMap(hardwareMap);
+
+            systems = new pedroStateMachine(intakeHw, transferHw, turretHw);
+            systems.setAlliance(allianceTarget);
+
+            pathState = PathState.DRIVE_TO_SCORE_PRELOAD;
+
+            while (!isStarted() && !isStopRequested()) {
+
+                telemetry.addLine("=== Unified Auton Ready ===");
+                telemetry.addData("Alliance", allianceTarget == 1 ? "Blue" : "Red");
+                telemetry.addData("Type", isNet ? "NET (Gate)" : "FAR (Corner)");
+                telemetry.addData("Safety Timer", useTimer ? "ON (1.5s)" : "OFF");
+                telemetry.update();
+            }
+
+            waitForStart();
+            systems.update(allianceTarget, follower.getPose());
+            opmodeTimer.resetTimer();
+            setPathState(PathState.DRIVE_TO_SCORE_PRELOAD);
+
+            while (opModeIsActive()) {
+
+                for (LynxModule hub : allHubs) {
+                    hub.clearBulkCache();
+                }
+                follower.update();
+                statePathUpdate();
+                systems.update(allianceTarget, follower.getPose());
+
+                telemetry.addData("Path State", pathState);
+                telemetry.addData("System State", systems.getCurrentState());
+                telemetry.update();
+            }
+        }
+
+        public void statePathUpdate() {
+            if (isNet) {
+                statePathUpdateNet();
+            } else {
+                statePathUpdateFar();
+            }
+        }
+
+        public void statePathUpdateFar() {
+            switch(pathState) {
+                // --- PRELOAD ---
+                case DRIVE_TO_SCORE_PRELOAD:
+                    follower.followPath(scorePreloadPath, true);
+                    systems.initiateTurretSpinUp();
+                    setPathState(PathState.SCORE_PRELOAD);
+                    break;
+
+                case SCORE_PRELOAD:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+                        if (checkScoringCondition()) {
+                            setPathState(PathState.DRIVE_TO_GRAB_SET_1);
+                        }
+                    }
+                    break;
+
+                // --- SET 1 ---
+                case DRIVE_TO_GRAB_SET_1:
+                    follower.followPath(grabSet1Path, true);
+                    systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+                    setPathState(PathState.GRAB_SET_1);
+                    break;
+
+                case GRAB_SET_1:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+                        setPathState(PathState.DRIVE_TO_SCORE_SET_1);
+                    }
+                    break;
+
+                case DRIVE_TO_SCORE_SET_1:
+                    follower.followPath(scoreSet1Path, true);
+                    systems.initiateTurretSpinUp();
+                    setPathState(PathState.SCORE_SET_1);
+                    break;
+
+                case SCORE_SET_1:
+                    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);
+                        }
+                    }
+                    break;
+
+                // --- END GAME (Corner) ---
+                case DRIVE_TO_ENDGAME_COLLECTION:
+                    follower.followPath(grabEndGamePath, true);
+                    systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+                    setPathState(PathState.DO_ENDGAME_COLLECTION);
+                    break;
+
+                case DO_ENDGAME_COLLECTION:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+                        setPathState(PathState.DRIVE_TO_SCORE_ENDGAME);
+                    }
+                    break;
+
+                case DRIVE_TO_SCORE_ENDGAME:
+                    follower.followPath(scoreEndGamePath, true);
+                    systems.initiateTurretSpinUp();
+                    setPathState(PathState.SCORE_ENDGAME);
+                    break;
+
+                case SCORE_ENDGAME:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+                        if (checkScoringCondition()) {
+                            setPathState(PathState.DRIVE_TO_PARK);
+                        }
+                    }
+                    break;
+
+                // --- PARK ---
+                case DRIVE_TO_PARK:
+                    follower.followPath(parkPath, true);
+                    setPathState(PathState.DONE);
+                    break;
+
+                case DONE:
+                    telemetry.addLine("Completed Far Auton");
+                    systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+                    break;
+            }
+        }
+
+        public void statePathUpdateNet() {
+            switch(pathState) {
+                // --- PRELOAD ---
+                case DRIVE_TO_SCORE_PRELOAD:
+                    follower.followPath(scorePreloadPath, true);
+                    systems.initiateTurretSpinUp();
+                    systems.setAutonState(pedroStateMachine.AutonState.SCORING); // Shoot while driving
+                    setPathState(PathState.SCORE_PRELOAD);
+                    break;
+
+                case SCORE_PRELOAD:
+                    if (!follower.isBusy() && checkScoringCondition()) {
+                        follower.followPath(driveToSet2Path, true);
+                        setPathState(PathState.DRIVE_TO_GRAB_SET_2);
+                    }
+                    break;
+
+                // --- SET 2 ---
+                case DRIVE_TO_GRAB_SET_2:
+                    if (!follower.isBusy()) {
+                        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);
+                        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()) {
+                            follower.followPath(grabGate1Path, true);
+                            systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+                            setPathState(PathState.GRAB_GATE_1);
+                        }
+                    }
+                    break;
+
+                // --- GATE 1 ---
+                case GRAB_GATE_1:
+                    if (!follower.isBusy()) {
+                        setPathState(PathState.WAIT_GATE_1);
+                    }
+                    break;
+
+                case WAIT_GATE_1:
+                    // Stop at the gate while intaking to pull in game elements
+                    if (actionTimer.getElapsedTimeSeconds() > gateWaitTime) {
+                        systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+                        follower.followPath(scoreGate1Path, true);
+                        systems.initiateTurretSpinUp();
+                        setPathState(PathState.SCORE_GATE_1);
+                    }
+                    break;
+
+                case SCORE_GATE_1:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+                        if (checkScoringCondition()) {
+                            follower.followPath(grabGate2Path, true);
+                            systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+                            setPathState(PathState.GRAB_GATE_2);
+                        }
+                    }
+                    break;
+
+                // --- GATE 2 ---
+                case GRAB_GATE_2:
+                    if (!follower.isBusy()) {
+                        setPathState(PathState.WAIT_GATE_2);
+                    }
+                    break;
+
+                case WAIT_GATE_2:
+                    if (actionTimer.getElapsedTimeSeconds() > gateWaitTime) {
+                        systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+                        follower.followPath(scoreGate2Path, true);
+                        systems.initiateTurretSpinUp();
+                        setPathState(PathState.SCORE_GATE_2);
+                    }
+                    break;
+
+                case SCORE_GATE_2:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+                        if (checkScoringCondition()) {
+                            follower.followPath(driveToSet3Path, true);
+                            setPathState(PathState.DRIVE_TO_GRAB_SET_3);
+                        }
+                    }
+                    break;
+
+                // --- SET 3 ---
+                case DRIVE_TO_GRAB_SET_3:
+                    if (!follower.isBusy()) {
+                        follower.followPath(grabSet3Path, true);
+                        systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+                        setPathState(PathState.GRAB_SET_3);
+                    }
+                    break;
+
+                case GRAB_SET_3:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+                        follower.followPath(scoreSet3Path, true);
+                        systems.initiateTurretSpinUp();
+                        setPathState(PathState.SCORE_SET_3);
+                    }
+                    break;
+
+                case SCORE_SET_3:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+                        if (checkScoringCondition()) {
+                            follower.followPath(driveToSet1Path, true);
+                            setPathState(PathState.DRIVE_TO_GRAB_SET_1);
+                        }
+                    }
+                    break;
+
+                // --- SET 1 ---
+                case DRIVE_TO_GRAB_SET_1:
+                    if (!follower.isBusy()) {
+                        follower.followPath(grabSet1Path, true);
+                        systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+                        setPathState(PathState.GRAB_SET_1);
+                    }
+                    break;
+
+                case GRAB_SET_1:
+                    if (!follower.isBusy()) {
+                        systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+                        follower.followPath(scoreSet1Path, true);
+                        systems.initiateTurretSpinUp();
+                        systems.setAutonState(pedroStateMachine.AutonState.SCORING); // Shoot while driving
+                        setPathState(PathState.SCORE_SET_1);
+                    }
+                    break;
+
+                case SCORE_SET_1:
+                    if (!follower.isBusy() && checkScoringCondition()) {
+                        follower.followPath(parkPath, true);
+                        setPathState(PathState.DRIVE_TO_PARK);
+                    }
+                    break;
+
+                // --- PARK ---
+                case DRIVE_TO_PARK:
+                    if (!follower.isBusy()) {
+                        setPathState(PathState.DONE);
+                    }
+                    break;
+
+                case DONE:
+                    telemetry.addLine("Completed Net Auton");
+                    systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+                    break;
+            }
+        }
+
+        protected boolean checkScoringCondition() {
+            if (systems.isScoringComplete()) {
+                return true;
+            }
+            if (useTimer && pathTimer.getElapsedTimeSeconds() > timerTimeout) {
+                // Failsafe triggered
+                telemetry.addLine("WARNING: Scoring Timeout Triggered!");
+                return true;
+            }
+            return false;
+        }
+    }
+}

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

@@ -0,0 +1,274 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
+import org.firstinspires.ftc.teamcode.subsystems.Turret;
+import org.firstinspires.ftc.teamcode.util.AutoTransfer;
+/**
+ * Unified Autonomous for Net and Far sides (Blue and Red).
+ * Uses PedroPathing for Pose estimation only.
+ * Uses Time/Power for movement.
+ */
+public abstract class TimeAuton extends LinearOpMode {
+
+    // Subsystems
+    protected Intake intake;
+    protected TransferSys transfer;
+    protected Turret turret;
+    protected Follower follower;
+
+    // Drive Motors (Direct access for time-based drive)
+    protected DcMotor frontLeft, frontRight, backLeft, backRight;
+
+    // Constants
+    protected static final double DRIVE_POWER = 0.8;
+    protected static final double STRAFE_POWER = 0.8;
+
+    // State
+    protected int alliance = 1; // 1 = Blue, 2 = Red
+    protected boolean isNetSide = false;
+    protected final ElapsedTime timer = new ElapsedTime();
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        // 1. Initialize Hardware
+        hwMap.IntakeHwMap intakeHw = new hwMap.IntakeHwMap(hardwareMap);
+        hwMap.TransferHwMap transferHw = new hwMap.TransferHwMap(hardwareMap);
+        hwMap.TurretHwMap turretHw = new hwMap.TurretHwMap(hardwareMap);
+
+        intake = new Intake(intakeHw);
+        transfer = new TransferSys(transferHw);
+        turret = new Turret(turretHw);
+
+        transfer.closeLimiter();
+
+
+        // Initialize Drive Motors
+        frontLeft = hardwareMap.get(DcMotor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants.FRONT_LEFT_MOTOR);
+        frontRight = hardwareMap.get(DcMotor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants.FRONT_RIGHT_MOTOR);
+        backLeft = hardwareMap.get(DcMotor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants.BACK_LEFT_MOTOR);
+        backRight = hardwareMap.get(DcMotor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants.BACK_RIGHT_MOTOR);
+
+        configureDriveMotors();
+
+        // Initialize Pedro Follower (For Pose detection only)
+        follower = Constants.createFollower(hardwareMap);
+
+
+        // 2. Setup Specific Auton Configuration
+        configureAuton();
+
+        // Set Start Pose based on Alliance/Side
+        setStartPose();
+
+        AutoTransfer.reset();
+
+
+        turret.setAlliance(alliance);
+        AutoTransfer.initAuton(alliance);
+
+        telemetry.addLine("Initialized Unified Auton");
+        telemetry.addData("Alliance", alliance == 1 ? "BLUE" : "RED");
+        telemetry.addData("Side", isNetSide ? "NET" : "FAR");
+        telemetry.update();
+
+        waitForStart();
+
+        if(opModeIsActive()) {
+            follower.startTeleopDrive(); // Keeps pose updating without following a path
+
+            // 3. Run the Sequence
+            if (!isNetSide) {
+                runFarSequence();
+            } else {
+                runNetSequence();
+            }
+        }
+    }
+
+    // --- Abstract Methods to be defined by subclasses ---
+    protected abstract void configureAuton();
+
+    // --- Sequences ---
+
+    private void runFarSequence() {
+        // 1. Drive Front for 0.6 seconds
+        drive(DRIVE_POWER, 0, 0, 600);
+
+        // 2. Shoot all artifacts
+        shootAllArtifacts();
+
+        // 3. Drive Front for 1.5 seconds
+        drive(DRIVE_POWER, 0, 0, 1500);
+    }
+
+    private void runNetSequence() {
+        // 1. Drive Backward for 1.5 seconds
+        drive(-DRIVE_POWER, 0, 0, 1500);
+
+        // 2. Stop (Handled automatically by drive finish) & Shoot
+        shootAllArtifacts();
+
+        // 3. Strafe based on alliance for 0.7 seconds
+        if (alliance == 1) { // Blue -> Strafe Left
+            drive(0, -STRAFE_POWER, 0, 1600);
+        } else { // Red -> Strafe Right
+            drive(0, STRAFE_POWER, 0, 1600);
+        }
+    }
+
+    // --- Core Logic Methods ---
+
+    protected void shootAllArtifacts() {
+        // 1. Spin up Turret
+        turret.setTurretState(Turret.TurretState.LAUNCH);
+
+        // 2. Wait for Velocity
+        timer.reset();
+        while(opModeIsActive() && timer.seconds() < 2.0) {
+            updateSystems();
+            if(turret.isTurretReady()) break;
+
+        }
+
+        telemetry.addLine("LAUNCHING");
+        telemetry.update();
+
+        // 3. START FEEDING: Open Gate AND Run Intake
+        transfer.openLimiter();
+        transfer.setTransferState(TransferSys.TransferState.LAUNCHING);
+        intake.setIntakeState(Intake.IntakeState.LAUNCH); // <--- ADD THIS LINE
+
+        // 4. Wait for shot to finish
+        while(opModeIsActive() && transfer.getTransferState() == TransferSys.TransferState.LAUNCHING) {
+            updateSystems();
+        }
+
+        // 5. Stop everything
+        turret.setTurretState(Turret.TurretState.IDLE);
+        transfer.setTransferState(TransferSys.TransferState.IDLE);
+        intake.setIntakeState(Intake.IntakeState.IDLE); // <--- ADD THIS LINE
+    }
+
+    protected void updateSystems() {
+        follower.update(); // Update Odometry Pose
+        Pose currentPose = follower.getPose();
+
+        AutoTransfer.updatePose(currentPose);
+
+        // Update Turret with current Pose
+        turret.updateAuton(alliance, currentPose, 0.2);
+
+        transfer.updateTurretReady(turret.isTurretReady());
+        transfer.update();
+
+        // Telemetry for debugging
+        telemetry.addData("Turret Ready", turret.isTurretReady());
+        telemetry.addData("Transfer State", transfer.getTransferState());
+        telemetry.update();
+    }
+
+    // --- Drive Helper Methods ---
+
+    protected void drive(double x, double y, double rot, long durationMs) {
+        timer.reset();
+        while (opModeIsActive() && timer.milliseconds() < durationMs) {
+            double denominator = Math.max(Math.abs(x) + Math.abs(y) + Math.abs(rot), 1);
+
+            // Standard Mecanum formulas
+            double fl = (x + y + rot) / denominator;
+            double bl = (x - y + rot) / denominator;
+            double fr = (x - y - rot) / denominator;
+            double br = (x + y - rot) / denominator;
+
+            frontLeft.setPower(fl);
+            backLeft.setPower(bl);
+            frontRight.setPower(fr);
+            backRight.setPower(br);
+
+            updateSystems();
+        }
+        stopDrive();
+    }
+
+    protected void stopDrive() {
+        frontLeft.setPower(0);
+        backLeft.setPower(0);
+        frontRight.setPower(0);
+        backRight.setPower(0);
+    }
+
+    private void configureDriveMotors() {
+        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        frontLeft.setDirection(DcMotor.Direction.REVERSE);
+        backLeft.setDirection(DcMotor.Direction.REVERSE);
+    }
+
+    private void setStartPose() {
+        if (alliance == 1) { // Blue
+            if (isNetSide) follower.setStartingPose(new Pose(21, 124.4, Math.toRadians(-36)));
+            else follower.setStartingPose(new Pose(54, 8.65, Math.toRadians(90)));
+        } else { // Red
+            if (isNetSide) follower.setStartingPose(new Pose(123, 124.4, Math.toRadians(90)));
+            else follower.setStartingPose(new Pose(88, 8.65, Math.toRadians(90)));
+        }
+    }
+
+    // =================================================================
+    // ACTUAL AUTONOMOUS OPMODES
+    // =================================================================
+
+    @Disabled
+    @Autonomous(name = "Blue Far (Auto)", group = "Unified", preselectTeleOp = "Red Final")
+    public static class BlueFar extends TimeAuton {
+        @Override
+        protected void configureAuton() {
+            alliance = 1;
+            isNetSide = false;
+        }
+    }
+
+    @Disabled
+    @Autonomous(name = "Red Far (Auto)", group = "Unified", preselectTeleOp = "Red Final")
+    public static class RedFar extends TimeAuton {
+        @Override
+        protected void configureAuton() {
+            alliance = 2;
+            isNetSide = false;
+        }
+    }
+
+    @Disabled
+    @Autonomous(name = "Blue Net (Auto)", group = "Unified", preselectTeleOp = "Blue Final")
+    public static class BlueNet extends TimeAuton {
+        @Override
+        protected void configureAuton() {
+            alliance = 1;
+            isNetSide = true;
+        }
+    }
+
+    @Disabled
+    @Autonomous(name = "Red Net (Auto)", group = "Unified", preselectTeleOp = "Blue Final")
+    public static class RedNet extends TimeAuton {
+        @Override
+        protected void configureAuton() {
+            alliance = 2;
+            isNetSide = true;
+        }
+    }
+}

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

@@ -0,0 +1,134 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.pedropathing.geometry.Pose;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.subsystems.DriveTrain; // Unused but kept for reference
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
+import org.firstinspires.ftc.teamcode.subsystems.Turret;
+import org.firstinspires.ftc.teamcode.subsystems.Turret.TurretState;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys.TransferState;
+import org.firstinspires.ftc.teamcode.subsystems.Intake.IntakeState;
+
+import org.firstinspires.ftc.teamcode.util.AutoTransfer;
+
+public class pedroStateMachine {
+
+    public enum AutonState {
+        IDLE,
+        INTAKING,
+        SCORING
+    }
+
+    private AutonState currentState = AutonState.IDLE;
+    private final Intake m_intake;
+    private final TransferSys m_transfer;
+    private final Turret m_turret;
+
+
+
+    private int alliance = 0;
+
+    public pedroStateMachine(hwMap.IntakeHwMap h_intake, hwMap.TransferHwMap h_transfer, hwMap.TurretHwMap h_turret) {
+        this.m_intake = new Intake(h_intake);
+        this.m_transfer = new TransferSys(h_transfer);
+        this.m_turret = new Turret(h_turret);
+
+        setAutonState(AutonState.IDLE);
+        AutoTransfer.reset();
+    }
+
+    public void setAlliance(int alliance) {
+        this.alliance = alliance;
+        m_turret.setAlliance(alliance);
+        AutoTransfer.initAuton(alliance);
+    }
+
+    public void setAutonState(AutonState newState) {
+        if (this.currentState == newState) return;
+
+        handleStateExit(this.currentState);
+        this.currentState = newState;
+        handleStateEntry(newState);
+    }
+
+    private void handleStateExit(AutonState oldState) {
+        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);
+                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
+                break;
+            case SCORING:
+                m_intake.setIntakeState(IntakeState.LAUNCH); // Feed rings
+                m_turret.setTurretState(TurretState.LAUNCH);
+                m_transfer.setTransferState(TransferState.LAUNCHING);
+                break;
+            case IDLE:
+                m_intake.setIntakeState(IntakeState.IDLE);
+                m_turret.setTurretState(TurretState.IDLE);
+                m_transfer.setTransferState(TransferState.IDLE);
+                break;
+        }
+    }
+
+    public void update(int allianceTarget, Pose currentPose) {
+        m_turret.updateAuton(allianceTarget, currentPose, 0.2);
+
+        if (currentState == AutonState.SCORING) {
+            m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
+        }
+
+        m_transfer.update();
+
+
+        if (currentState == AutonState.SCORING) {
+            if (m_transfer.getTransferState() != TransferSys.TransferState.LAUNCHING) {
+                setAutonState(AutonState.IDLE);
+            }
+        }
+
+        AutoTransfer.updatePose(currentPose);
+    }
+
+    public void initiateTurretSpinUp() {
+        m_turret.setTurretState(TurretState.LAUNCH);
+    }
+
+    public boolean isTurretTracking() {
+        return m_turret.hasTarget();
+    }
+
+    public boolean isScoringComplete() {
+        if (currentState == AutonState.IDLE && m_transfer.getTransferState() == TransferState.IDLE) {
+            return true;
+        }
+        return false;
+    }
+
+    public double getTurretDistance() {
+        return m_turret.getDistance();
+    }
+
+    // Getters
+    public Intake getIntake() { return m_intake; }
+    public Turret getTurret() { return m_turret; }
+    public TransferSys getTransfer() { return m_transfer; }
+    public AutonState getCurrentState() { return currentState; }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/todo.txt

@@ -0,0 +1,29 @@
+Issues:
+ Therefore, the revised list of remaining handoff and connecting logic issues is:
+
+   1. `hwMap.java ` - LED/Flicker Servo Port Conflict: LEDA, LEDB, and LEDC are mapped to the same hardware servo names as flickA, flickB, and flickC. This creates an alias, meaning controlling one set of servos will inadvertently control the other, leading to unintended behavior and potential conflicts.
+   DONE (diff thing but i did what was needed) - 2. `hwMap.java` - Incomplete `getAprilTagPose()` Implementation: The getAprilTagPose() method returns an empty double[0], rendering getDistanceFromAprilTag() (which relies on getAprilTagPose()) ineffective, always returning -1. This impacts the accuracy of turret aiming calculations.
+   DONE - 3. `hwMap.java` - `isTurretReady()` Placeholder: The isTurretReady(double turretPower) method always returns true. This is a placeholder and does not reflect actual turret readiness, potentially causing finalTeleOp to proceed with scoring prematurely.
+   DONE -  i just removed the if alliance = 0 4. `subsystems/Turret.java` - `motif` Update Restriction: The motif (AprilTag pattern) in Turret.java is only updated via hardware.getMotif() when alliance == 0 within the update() method. If alliance is set to a non-zero value (e.g., 1 or 2 by finalTeleOp), the motif will not be updated from new Limelight detections in
+      teleop, potentially using an outdated pattern for TransferSys.
+   5. `subsystems/Turret.java` - `updateAuton()` Limelight Pipeline Management: The updateAuton method (used in autonomous) calls hardware.setPipeline(allianceTarget). If allianceTarget (which is 1 from pedroRedNet.java) does not correspond to the correct Limelight pipeline index for AprilTags, the Limelight may switch to
+      an incorrect pipeline, hindering accurate AprilTag tracking.
+   6. `subsystems/Turret.java` - Unused `apriltags` array: The apriltags array is declared in Turret.java but is never used, indicating potentially vestigial or incomplete logic.
+   7. `subsystems/TransferSys.java` - Incorrect `flickTimer` Logic in `processFlickSequence()`: The flickTimer is reset when liftCurrentItem() is called, and then FLICK_RESET_MS is measured from that same flickTimer to determine when to dropCurrentItem(). This means the FLICK_RESET_MS duration is not correctly measured from
+      the moment the item is flicked up, but from the moment it started moving up, leading to potentially incorrect timing for the drop sequence.
+   8. `subsystems/TransferSys.java` - Redundant `indexAllArtifacts()` Calls: indexAllArtifacts() is called when setTransferState(TransferState.INDEXING) and at the beginning of startFlickSequence(). While not a critical error, it could be redundant if the artifacts have already been indexed.
+   9. `subsystems/TransferSys.java` - Simplistic `updateMotif` Logic: The updateMotif method creates a motif where only one slot can be color 2 (Green), and the rest are 1 (Purple), based on a single pattern input. This might be too simplistic or inflexible if more complex motif patterns are needed.
+   10. `teleOp/StateMachine.java` - Direct `TurretState.LAUNCH` in `handleGameStateEntry(GameState.SCORING)`: The handleGameStateEntry for SCORING directly sets m_turret.setTurretState(Turret.TurretState.LAUNCH). This immediately commands the turret to launch without checking if it has acquired a target or is otherwise
+       ready, potentially leading to inaccurate shots.
+   11. `teleOp/StateMachine.java` - `isMotifEdited` Flag Never Resets: The isMotifEdited flag in StateMachine.java (which mirrors the one in TransferSys.java) is set to true when m_transfer.isMotifEdited() is true but is never reset. This means that once a motif is detected and updated once, isMotifEdited() will always
+       return true, making the alliance detection logic in finalTeleOp.java trigger only once.
+   12. `teleOp/StateMachine.java` - Unused `handleStatePeriodic()`: The handleStatePeriodic() method is defined but never called, indicating incomplete or unused functionality.
+   13. `teleOp/finalTeleOp.java` - `alliance` and `isMotifEdited()` Fragility: The initial detection and setting of alliance = 1 based on stateMachine.isMotifEdited() is fragile due to the isMotifEdited flag never being reset. This can lead to alliance being stuck at 0 or 1 incorrectly if motif detection doesn't happen as
+       expected or if the flag is not reset.
+   14. `teleOp/finalTeleOp.java` - Conflicting `hoodServo` Control: finalTeleOp.java directly initializes hoodServo and controls its position via hoodPosition. However, the Turret subsystem also has a hoodservo and a setHoodPos() method. This creates a potential conflict where finalTeleOp might be directly controlling the
+       servo while the Turret subsystem also tries to manage it, or the Turret's setHoodPos is being bypassed for manual control. Control should be consolidated through the Turret subsystem.
+   15. `teleOp/finalTeleOp.java` - Redundant `stateMachine.getTransfer().indexAllArtifacts()`: indexAllArtifacts() is called at the very end of the while (opModeIsActive()) loop. This re-indexes artifacts on every loop iteration, which might be inefficient if indexing is a resource-intensive operation and not always needed.
+   16. `teleOp/finalTeleOp.java` - Coupled `manualSAM` and `manualFlicker`: manualSAM = manualFlicker; directly couples manual flicker control with manual Shot Adjustment Module control. This might not always be the desired behavior and could limit flexibility.
+   17. `teleOp/finalTeleOp.java` - Blocking `sleep()` Calls in `manualFlicker`: The use of sleep() calls within the manualFlicker block in the main while (opModeIsActive()) loop will block the entire robot's operation, preventing other subsystems from updating or responding to input for the duration of the sleep. Servo
+       operations should ideally be managed with non-blocking timers within a state machine.
+

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

@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.pedroPathing;
+
+import com.pedropathing.control.FilteredPIDFCoefficients;
+import com.pedropathing.control.PIDFCoefficients;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.ftc.drivetrains.MecanumConstants;
+import com.pedropathing.ftc.FollowerBuilder;
+import com.pedropathing.ftc.localization.Encoder;
+import com.pedropathing.ftc.localization.constants.ThreeWheelConstants;
+import com.pedropathing.ftc.localization.constants.ThreeWheelIMUConstants;
+import com.pedropathing.ftc.localization.constants.TwoWheelConstants;
+import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants;
+
+public class Constants {
+    public static FollowerConstants followerConstants = new FollowerConstants()
+            .mass(12);
+
+    public static MecanumConstants driveConstants = new MecanumConstants()
+            .maxPower(0.8)
+            .rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR)
+            .rightRearMotorName(DriveConstants.BACK_RIGHT_MOTOR)
+            .leftRearMotorName(DriveConstants.BACK_LEFT_MOTOR)
+            .leftFrontMotorName(DriveConstants.FRONT_LEFT_MOTOR)
+            .leftFrontMotorDirection(DriveConstants.FRONT_LEFT_DIRECTION)
+            .leftRearMotorDirection(DriveConstants.BACK_LEFT_DIRECTION)
+            .rightFrontMotorDirection(DriveConstants.FRONT_RIGHT_DIRECTION)
+            .rightRearMotorDirection(DriveConstants.BACK_RIGHT_DIRECTION);
+
+    public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
+
+
+    public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants()
+            .forwardTicksToInches(0.001978956)
+            .strafeTicksToInches(0.001978956)
+            .turnTicksToInches(0.001978956)
+            .leftPodY(3.75)
+            .rightPodY(-3.25)
+            .strafePodX(-6.25)
+            .leftEncoder_HardwareMapName("intake")
+            .rightEncoder_HardwareMapName("fl")
+            .strafeEncoder_HardwareMapName("fr")
+            .leftEncoderDirection(Encoder.FORWARD)
+            .rightEncoderDirection(Encoder.FORWARD)
+            .strafeEncoderDirection(Encoder.FORWARD);
+
+
+    public static Follower createFollower(HardwareMap hardwareMap) {
+        return new FollowerBuilder(followerConstants, hardwareMap)
+                .pathConstraints(pathConstraints)
+                .mecanumDrivetrain(driveConstants)
+                .threeWheelLocalizer(localizerConstants)
+                .build();
+    }
+}

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

@@ -0,0 +1,69 @@
+package org.firstinspires.ftc.teamcode.pedroPathing;
+
+import com.pedropathing.control.FilteredPIDFCoefficients;
+import com.pedropathing.control.PIDFCoefficients;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.ftc.drivetrains.MecanumConstants;
+import com.pedropathing.ftc.FollowerBuilder;
+import com.pedropathing.ftc.localization.Encoder;
+import com.pedropathing.ftc.localization.constants.TwoWheelConstants; // Changed import
+import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants;
+
+public class Constants {
+
+    public static FollowerConstants followerConstants = new FollowerConstants()
+            .mass(12.27)
+            .forwardZeroPowerAcceleration(-36.474)
+            .lateralZeroPowerAcceleration(-75.984)
+            .translationalPIDFCoefficients(new PIDFCoefficients(0.08, 0, 0, 0.019))
+            .headingPIDFCoefficients(new PIDFCoefficients(0.932, 0, 0, 0.025))
+            .drivePIDFCoefficients(new FilteredPIDFCoefficients(.025,0,0.00001, 0.8, .01));
+
+    public static MecanumConstants driveConstants = new MecanumConstants()
+            .maxPower(0.8)
+            .rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR)
+            .rightRearMotorName(DriveConstants.BACK_RIGHT_MOTOR)
+            .leftRearMotorName(DriveConstants.BACK_LEFT_MOTOR)
+            .leftFrontMotorName(DriveConstants.FRONT_LEFT_MOTOR)
+            .leftFrontMotorDirection(DriveConstants.FRONT_LEFT_DIRECTION)
+            .leftRearMotorDirection(DriveConstants.BACK_LEFT_DIRECTION)
+            .rightFrontMotorDirection(DriveConstants.FRONT_RIGHT_DIRECTION)
+            .rightRearMotorDirection(DriveConstants.BACK_RIGHT_DIRECTION)
+            .xVelocity(80.106)
+            .yVelocity(64.8032);
+
+    public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1.1, 1);
+
+    public static TwoWheelConstants localizerConstants = new TwoWheelConstants()
+            .forwardTicksToInches(0.0016334)
+            .strafeTicksToInches(0.00200734)
+            //.forwardPodY(5.2)
+            //.forwardEncoder_HardwareMapName("fl")
+            .forwardPodY(5.51)
+            .forwardEncoder_HardwareMapName("br")
+
+            .strafePodX(6.43)
+            .strafeEncoder_HardwareMapName("fl")
+
+            .forwardEncoderDirection(Encoder.REVERSE)
+            .strafeEncoderDirection(Encoder.REVERSE)
+
+            .IMU_HardwareMapName("imu")
+            .IMU_Orientation(new RevHubOrientationOnRobot(
+                    RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
+                    RevHubOrientationOnRobot.UsbFacingDirection.UP
+            ));
+
+    public static Follower createFollower(HardwareMap hardwareMap) {
+        return new FollowerBuilder(followerConstants, hardwareMap)
+                .pathConstraints(pathConstraints)
+                .mecanumDrivetrain(driveConstants)
+                .twoWheelLocalizer(localizerConstants)
+                .build();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md

@@ -0,0 +1,131 @@
+## TeamCode Module
+
+Welcome!
+
+This module, TeamCode, is the place where you will write/paste the code for your team's
+robot controller App. This module is currently empty (a clean slate) but the
+process for adding OpModes is straightforward.
+
+## Creating your own OpModes
+
+The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
+
+Sample opmodes exist in the FtcRobotController module.
+To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
+
+Expand the following tree elements:
+ FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples
+
+### Naming of Samples
+
+To gain a better understanding of how the samples are organized, and how to interpret the
+naming system, it will help to understand the conventions that were used during their creation.
+
+These conventions are described (in detail) in the sample_conventions.md file in this folder.
+
+To summarize: A range of different samples classes will reside in the java/external/samples.
+The class names will follow a naming convention which indicates the purpose of each class.
+The prefix of the name will be one of the following:
+
+Basic:  	This is a minimally functional OpMode used to illustrate the skeleton/structure
+            of a particular style of OpMode.  These are bare bones examples.
+
+Sensor:    	This is a Sample OpMode that shows how to use a specific sensor.
+            It is not intended to drive a functioning robot, it is simply showing the minimal code
+            required to read and display the sensor values.
+
+Robot:	    This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
+            It may be used to provide a common baseline driving OpMode, or
+            to demonstrate how a particular sensor or concept can be used to navigate.
+
+Concept:	This is a sample OpMode that illustrates performing a specific function or concept.
+            These may be complex, but their operation should be explained clearly in the comments,
+            or the comments should reference an external doc, guide or tutorial.
+            Each OpMode should try to only demonstrate a single concept so they are easy to
+            locate based on their name.  These OpModes may not produce a drivable robot.
+
+After the prefix, other conventions will apply:
+
+* Sensor class names are constructed as:    Sensor - Company - Type
+* Robot class names are constructed as:     Robot - Mode - Action - OpModetype
+* Concept class names are constructed as:   Concept - Topic - OpModetype
+
+Once you are familiar with the range of samples available, you can choose one to be the
+basis for your own robot.  In all cases, the desired sample(s) needs to be copied into
+your TeamCode module to be used.
+
+This is done inside Android Studio directly, using the following steps:
+
+ 1) Locate the desired sample class in the Project/Android tree.
+
+ 2) Right click on the sample class and select "Copy"
+
+ 3) Expand the  TeamCode/java folder
+
+ 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
+
+ 5) You will be prompted for a class name for the copy.
+    Choose something meaningful based on the purpose of this class.
+    Start with a capital letter, and remember that there may be more similar classes later.
+
+Once your copy has been created, you should prepare it for use on your robot.
+This is done by adjusting the OpMode's name, and enabling it to be displayed on the
+Driver Station's OpMode list.
+
+Each OpMode sample class begins with several lines of code like the ones shown below:
+
+```
+ @TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
+ @Disabled
+```
+
+The name that will appear on the driver station's "opmode list" is defined by the code:
+ ``name="Template: Linear OpMode"``
+You can change what appears between the quotes to better describe your opmode.
+The "group=" portion of the code can be used to help organize your list of OpModes.
+
+As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
+  ``@Disabled`` annotation which has been included.
+This line can simply be deleted , or commented out, to make the OpMode visible.
+
+
+
+## ADVANCED Multi-Team App management:  Cloning the TeamCode Module
+
+In some situations, you have multiple teams in your club and you want them to all share
+a common code organization, with each being able to *see* the others code but each having
+their own team module with their own code that they maintain themselves.
+
+In this situation, you might wish to clone the TeamCode module, once for each of these teams.
+Each of the clones would then appear along side each other in the Android Studio module list,
+together with the FtcRobotController module (and the original TeamCode module).
+
+Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
+prior to clicking to the green Run arrow.
+
+Warning:  This is not for the inexperienced Software developer.
+You will need to be comfortable with File manipulations and managing Android Studio Modules.
+These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
+ 
+Also.. Make a full project backup before you start this :)
+
+To clone TeamCode, do the following:
+
+Note: Some names start with "Team" and others start with "team".  This is intentional.
+
+1)  Using your operating system file management tools, copy the whole "TeamCode"
+    folder to a sibling folder with a corresponding new name, eg: "Team0417".
+
+2)  In the new Team0417 folder, delete the TeamCode.iml file.
+
+3)  the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
+    to a matching name with a lowercase 'team' eg:  "team0417".
+
+4)  In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
+         package="org.firstinspires.ftc.teamcode"
+    to be
+         package="org.firstinspires.ftc.team0417"
+
+5)  Add:    include ':Team0417' to the "/settings.gradle" file.
+    
+6)  Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""

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

@@ -0,0 +1,192 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+public class DriveTrain {
+    private final hwMap.DriveHwMap h_driveTrain;
+
+    // --- Anchor Mode State ---
+    private boolean isAnchorActive = false;
+
+    private double targetFwdPod = 0; // Uses frontLeft encoder
+    private double targetStrafePod = 0; // Uses backLeft encoder
+    private double targetHeading = 0; // Uses IMU (Degrees)
+
+    // PID Storage
+
+    private double driveP = -0.001;
+    private double driveI = 0.0;
+    private double driveD = -0.00015;
+
+    private double strafeP = -0.002;
+    private double strafeI = 0.0;
+    private double strafeD = -0.00008;
+
+    private double turnP = -0.08;
+    private double turnI = 0.0;
+    private double turnD = -0.0001; // Tuned for Degrees
+
+    private double anchorMaxPower = 0.7;
+
+    private double lastFwdErr = 0, lastStrErr = 0, lastTurnErr = 0;
+    private double sumFwd = 0, sumStr = 0, sumTurn = 0;
+    private long lastLoopTime = System.currentTimeMillis();
+
+    public enum DriveState {
+        NORMAL, TURBO, PRECISION, STOP
+    }
+    private DriveState currentState = DriveState.NORMAL;
+    private double speedMultiplier = 1.0;
+
+    public DriveTrain(hwMap.DriveHwMap hardware) {
+        this.h_driveTrain = hardware;
+    }
+
+    public void update() {
+        if (isAnchorActive) {
+            runAnchorControlLoop();
+        }
+    }
+
+    private void runAnchorControlLoop() {
+        long currentTime = System.currentTimeMillis();
+        double dt = (currentTime - lastLoopTime) / 1000.0;
+        if(dt == 0) dt = 0.001;
+        lastLoopTime = currentTime;
+
+        double currentFwd = h_driveTrain.backRightMotor.getCurrentPosition();
+        double currentStrafe = h_driveTrain.frontLeftMotor.getCurrentPosition();
+        double currentHeading = getHeadingDegrees();
+
+        // 2. Calculate Errors
+        double errorForward = targetFwdPod - currentFwd;
+        double errorStrafe = targetStrafePod - currentStrafe;
+
+        // Heading Error (Wrapped -180 to 180)
+        double errorTurn = targetHeading - currentHeading;
+        while (errorTurn > 180) errorTurn -= 360;
+        while (errorTurn < -180) errorTurn += 360;
+
+        // 3. PID Calculations
+
+        // Forward
+        sumFwd += errorForward * dt;
+        double dFwd = (errorForward - lastFwdErr) / dt;
+        double powerFwd = (errorForward * driveP) + (sumFwd * driveI) + (dFwd * driveD);
+
+        // Strafe
+        sumStr += errorStrafe * dt;
+        double dStr = (errorStrafe - lastStrErr) / dt;
+        double powerStr = (errorStrafe * strafeP) + (sumStr * strafeI) + (dStr * strafeD);
+
+        // Turn
+        sumTurn += errorTurn * dt;
+        double dTurn = (errorTurn - lastTurnErr) / dt;
+        double powerTurn = (errorTurn * turnP) + (sumTurn * turnI) + (dTurn * turnD);
+
+        lastFwdErr = errorForward;
+        lastStrErr = errorStrafe;
+        lastTurnErr = errorTurn;
+
+        powerFwd = Range.clip(powerFwd, -anchorMaxPower, anchorMaxPower);
+        powerStr = Range.clip(powerStr, -anchorMaxPower, anchorMaxPower);
+        powerTurn = Range.clip(powerTurn, -anchorMaxPower, anchorMaxPower);
+
+        double fl = powerFwd + powerStr + powerTurn;
+        double bl = powerFwd - powerStr + powerTurn;
+        double fr = powerFwd - powerStr - powerTurn;
+        double br = powerFwd + powerStr - powerTurn;
+
+        double max = Math.max(Math.abs(fl), Math.max(Math.abs(bl), Math.max(Math.abs(fr), Math.abs(br))));
+        if (max > 1.0) {
+            fl /= max; bl /= max; fr /= max; br /= max;
+        }
+
+        h_driveTrain.setMotorPowers(fl, fr, bl, br);
+    }
+
+    public void startAnchor() {
+        if (!isAnchorActive) {
+            h_driveTrain.resetEncoders();
+            targetFwdPod = h_driveTrain.frontLeftMotor.getCurrentPosition();
+            targetStrafePod = h_driveTrain.backLeftMotor.getCurrentPosition();
+            targetHeading = getHeadingDegrees();
+
+            sumFwd = 0; sumStr = 0; sumTurn = 0;
+            lastFwdErr = 0; lastStrErr = 0; lastTurnErr = 0;
+            lastLoopTime = System.currentTimeMillis();
+
+            isAnchorActive = true;
+            this.currentState = DriveState.STOP;
+        }
+    }
+
+    public void stopAnchor() {
+        if (isAnchorActive) {
+            isAnchorActive = false;
+            stop();
+            this.currentState = DriveState.NORMAL;
+        }
+    }
+
+    private double getHeadingDegrees() {
+        YawPitchRollAngles orientation = h_driveTrain.imu.getRobotYawPitchRollAngles();
+        return orientation.getYaw(AngleUnit.DEGREES);
+    }
+
+    public void updateAnchorPID(double dP, double dI, double dD,
+                                double sP, double sI, double sD,
+                                double tP, double tI, double tD,
+                                double maxP) {
+        this.driveP = dP; this.driveI = dI; this.driveD = dD;
+        this.strafeP = sP; this.strafeI = sI; this.strafeD = sD;
+        this.turnP = tP; this.turnI = tI; this.turnD = tD;
+        this.anchorMaxPower = maxP;
+    }
+
+    public void teleopDrive(double x, double y, double rx) {
+        if (isAnchorActive) {
+            if (Math.abs(x) > 0.1 || Math.abs(y) > 0.1 || Math.abs(rx) > 0.1) {
+                stopAnchor();
+            } else {
+                return;
+            }
+        }
+
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+        double frontLeftPower = (((y + x + rx) / denominator) * speedMultiplier);
+        double backLeftPower = (((y - x + rx) / denominator) * speedMultiplier);
+        double frontRightPower = (((y - x - rx) / denominator) * speedMultiplier);
+        double backRightPower = (((y + x - rx) / denominator) * speedMultiplier);
+
+
+        h_driveTrain.setMotorPowers(frontLeftPower, frontRightPower, backLeftPower, backRightPower);
+    }
+
+    // Getters for Tuning Telemetry
+    public boolean isAnchorActive() { return isAnchorActive; }
+    public double getLastForwardError() { return lastFwdErr; }
+    public double getLastStrafeError() { return lastStrErr; }
+    public double getLastTurnError() { return lastTurnErr; }
+
+    public void stop() {
+        if (currentState != DriveState.STOP) setDriveState(DriveState.STOP);
+        h_driveTrain.stopMotors();
+    }
+
+    public void setDriveState(DriveState state) {
+        this.currentState = state;
+        switch (state) {
+            case NORMAL: speedMultiplier = Constants.DriveConstants.NORMAL_SPEED_MULTIPLIER; break;
+            case PRECISION: speedMultiplier = Constants.DriveConstants.PRECISION_SPEED_MULTIPLIER; break;
+            case TURBO: speedMultiplier = Constants.DriveConstants.TURBO_SPEED_MULTIPLIER; break;
+            case STOP: speedMultiplier = Constants.DriveConstants.STOP_SPEED_MULTIPLIER; stop(); break;
+        }
+    }
+    public DriveState getDriveState() { return currentState; }
+}

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

@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public class Intake {
+    private double runPower = 1;
+    private final hwMap.IntakeHwMap h_intake;
+    public enum IntakeState {
+        INTAKE,
+        EXTAKE,
+        IDLE,
+        LAUNCH,
+        STOP
+    }
+
+    private IntakeState currentState = IntakeState.IDLE;
+    public Intake(hwMap.IntakeHwMap hardware) {
+        this.h_intake = hardware;
+    }
+    public void setIntakeState(IntakeState state) {
+        this.currentState = state;
+
+        switch (state) {
+            case INTAKE:
+                h_intake.frontIntakeMotor.setPower(Constants.IntakeConstants.INTAKE_POWER);
+                //h_intake.backIntakeMotor.setPower(Constants.IntakeConstants.INTAKE_POWER);
+                break;
+            case EXTAKE:
+                h_intake.frontIntakeMotor.setPower(Constants.IntakeConstants.EXTAKE_POWER);
+                //h_intake.backIntakeMotor.setPower(Constants.IntakeConstants.EXTAKE_POWER);
+                break;
+            case LAUNCH:
+                h_intake.frontIntakeMotor.setPower(Constants.IntakeConstants.LAUNCH_POWER);
+                //h_intake.backIntakeMotor.setPower(Constants.IntakeConstants.LAUNCH_POWER);
+                break;
+            case IDLE:
+                h_intake.frontIntakeMotor.setPower(Constants.IntakeConstants.IDLE_POWER);
+                //h_intake.backIntakeMotor.setPower(Constants.IntakeConstants.IDLE_POWER);
+                break;
+            case STOP:
+                break;
+        }
+    }
+
+    public IntakeState getCurrentState() {
+        return this.currentState;
+    }
+}

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

@@ -0,0 +1,76 @@
+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;
+
+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
+
+    public LED(hwMap.LedHwMap hardware) {
+        this.hardware = hardware;
+
+        solidRed.setBrightness(50);
+        solidOrange.setBrightness(50);
+        solidGreen.setBrightness(50);
+        solidBlue.setBrightness(50);
+        solidPurple.setBrightness(50);
+    }
+
+    public void update(boolean isScoring, boolean isTurretReady, boolean hasTarget, boolean isAnchorActive) {
+    /*
+        int newLed1State = 0;
+        if (isScoring) {
+            if (isTurretReady) {
+                newLed1State = 2;
+            } else {
+                newLed1State = 1;
+            }
+        }
+
+        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;
+        }
+
+        int newLed2State = 0;
+        if (hasTarget && isAnchorActive) {
+            newLed2State = 3;
+        } else if (hasTarget) {
+            newLed2State = 1;
+        } else if (isAnchorActive) {
+            newLed2State = 2;
+        }
+
+        if (newLed2State != currentLed2State) {
+            if (newLed2State == 0) {
+                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
+            } else if (newLed2State == 1) {
+                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
+            } else if (newLed2State == 2) {
+                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidBlue);
+            } else if (newLed2State == 3) {
+                hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidPurple);
+            }
+            currentLed2State = newLed2State;
+        }*/
+    }
+}

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

@@ -0,0 +1,77 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public class Lift {
+
+    private final hwMap.LiftHwMap h_lift;
+
+    public enum LiftState {
+        BEGIN_LIFT,  // Engages PTO and pulls up
+        HOLD_LIFT,   // Keeps PTO engaged but reduces power to hold position
+        END_LIFT,    // Stops motors and Disengages PTO
+        MANUAL_DOWN  // Optional: In case you need to reverse/lower
+    }
+
+    private LiftState currentState = LiftState.END_LIFT;
+
+    public Lift(hwMap.LiftHwMap hardware) {
+        this.h_lift = hardware;
+    }
+
+    public void setLiftState(LiftState state) {
+        this.currentState = state;
+
+        switch (state) {
+            case BEGIN_LIFT:
+                // Engage servos, run motors at lift power
+                setPtoState(
+                        Constants.LiftConstants.POS_ENGAGED,
+                        Constants.LiftConstants.POWER_LIFTING
+                );
+                break;
+
+            case HOLD_LIFT:
+                // Keep servos engaged, run motors at holding power (gravity compensation)
+                setPtoState(
+                        Constants.LiftConstants.POS_ENGAGED,
+                        Constants.LiftConstants.POWER_HOLDING
+                );
+                break;
+
+            case MANUAL_DOWN:
+                // Engage servos, reverse motors
+                setPtoState(
+                        Constants.LiftConstants.POS_ENGAGED,
+                        -Constants.LiftConstants.POWER_LIFTING
+                );
+                break;
+
+            case END_LIFT:
+            default:
+                // Disengage servos, cut power
+                setPtoState(
+                        Constants.LiftConstants.POS_DISENGAGED,
+                        0
+                );
+                break;
+        }
+    }
+    private void setPtoState(double servoPos, double motorPower) {
+        // Set Servo Positions
+        /*h_lift.ptoLeft.setPosition(servoPos);
+        h_lift.ptoRight.setPosition(servoPos);
+
+        // Set Drive Motor Powers
+        // Note: You might need to invert the right side depending on your drive train config
+        h_lift.leftFront.setPower(motorPower);
+        h_lift.leftRear.setPower(motorPower);
+        h_lift.rightFront.setPower(motorPower);
+        h_lift.rightRear.setPower(motorPower);*/
+    }
+
+    public LiftState getState() {
+        return currentState;
+    }
+}

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

@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+
+public class TransferSys {
+    private final hwMap.TransferHwMap hardware;
+
+    private boolean isTurretReady = false;
+    private boolean limiterClosed = true;
+
+    public double launchDuration = 2; // CHANGEABLE TODO
+
+    public enum TransferState {
+        LAUNCHING, // Gate OPEN
+        IDLE,      // Gate CLOSED
+        STOP       // Reset
+    }
+
+    private TransferState currentState = TransferState.IDLE;
+    private final ElapsedTime timer = new ElapsedTime();
+    private boolean isLaunchSequenceActive = false;
+
+    public TransferSys(hwMap.TransferHwMap hardware) {
+        this.hardware = hardware;
+        timer.reset();
+    }
+
+    public void setTransferState(TransferState state) {
+        if (this.currentState == state) return;
+        this.currentState = state;
+
+        switch (state) {
+            case LAUNCHING:
+                isLaunchSequenceActive = false;
+                break;
+            case IDLE:
+                closeLimiter();
+                isLaunchSequenceActive = false;
+                break;
+            case STOP:
+                hardware.resetLimiter();
+                limiterClosed = false;
+                break;
+        }
+    }
+
+
+    public void update() {
+        switch (currentState) {
+            case LAUNCHING:
+                if (!isLaunchSequenceActive) {
+                    if (isTurretReady) {
+                        openLimiter();
+                        timer.reset();
+                        isLaunchSequenceActive = true;
+                    } else {
+                        closeLimiter();
+                    }
+                }
+                else {
+                    openLimiter();
+
+                    if (timer.seconds() >= launchDuration) {
+                        setTransferState(TransferState.IDLE);
+                    }
+                }
+                break;
+
+            case IDLE:
+            case STOP:
+                if (currentState == TransferState.IDLE && !limiterClosed) {
+                    closeLimiter();
+                }
+                break;
+        }
+    }
+
+    public void updateTurretReady(boolean turretReady) {
+        this.isTurretReady = turretReady;
+    }
+
+    public void closeLimiter() {
+        hardware.setLimiter(true);
+        limiterClosed = true;
+    }
+
+    public void openLimiter() {
+        hardware.setLimiter(false);
+        limiterClosed = false;
+    }
+
+    public boolean getLimiterState() {
+        return limiterClosed;
+    }
+
+    public TransferState getTransferState() {
+        return currentState;
+    }
+
+    public boolean hasArtifacts() {
+        return hardware.detectArtifactColor(1) != 0 ||
+                hardware.detectArtifactColor(2) != 0 ||
+                hardware.detectArtifactColor(3) != 0;
+    }
+}

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

@@ -0,0 +1,317 @@
+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;
+
+public class Turret {
+
+    private final hwMap.TurretHwMap hardware;
+    private double turretPower = Constants.TurretConstants.TURRET_POWER_MID;
+    private int alliance = 0;
+    private double distance = 0;
+
+    private double targetVelocity = 0;
+
+    // PID Variables
+    private double integralSum = 0;
+    private double lastError = 0;
+
+    // Targeting Variables
+    private double targetCorrectionOffsetTicks = 0;
+    private double fusedTargetTicks = 0;
+    public double turretPowerRotation;
+    public static double offsetTicks = 0.0;
+
+    public double offsetHood = 0;
+
+    private Pose robotPose;
+
+    private final ElapsedTime loopTimer = new ElapsedTime();
+
+    public enum TurretState {
+        LAUNCH,
+        IDLE,
+        EXTAKE,
+        INTAKING
+    }
+    private TurretState currentState = TurretState.IDLE;
+
+    public Turret(hwMap.TurretHwMap hardware) {
+        this.hardware = hardware;
+        loopTimer.reset();
+    }
+
+    public void setAlliance(int alliance) {
+        this.alliance = alliance;
+        if (alliance == 2) {
+            hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
+        } else if(alliance == 1) {
+            hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_BLUE_TARGET);
+        } else {
+            hardware.setPipeline(0);
+        }
+    }
+
+    public void stop() {
+        if (currentState != TurretState.IDLE) {
+            setTurretState(TurretState.IDLE);
+        }
+        hardware.turretOff();
+        hardware.stopTurretRotation();
+    }
+
+    public void setTurretState(TurretState state) {
+        this.currentState = state;
+
+        switch (state) {
+            case IDLE:
+                hardware.turretOff();
+                hardware.stopTurretRotation();
+                integralSum = 0;
+                lastError = 0;
+                break;
+            case EXTAKE:
+                hardware.setTurretPower(Constants.TurretConstants.EXTAKE_POWER);
+                break;
+            case INTAKING:
+                hardware.setTurretPower(Constants.TurretConstants.INTAKE_POWER);
+                break;
+        }
+    }
+
+
+    public TurretState getTurretState() {
+        return currentState;
+    }
+
+    public boolean hasTarget() {
+        return hardware.hasTarget();
+    }
+
+    public void update(boolean manualTracking, boolean manualSAM, Pose currentPose) {
+        double dt = loopTimer.seconds();
+        loopTimer.reset();
+        if (dt < 0.001) dt = 0.001;
+
+        updatePose(currentPose);
+
+        double rawTicks = hardware.getTurretRotationPosition();
+        double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
+
+        distance = getDistance();
+
+        if (!manualTracking) {
+            calculateHybridTarget(currentPose, currentDegrees, rawTicks);
+            runPIDControl(rawTicks, dt);
+        } else {
+            hardware.stopTurretRotation();
+        }
+
+        if (currentState == TurretState.LAUNCH) {
+            handleLaunchLogic();
+        }
+    }
+
+    public void updateAuton(int alliance, Pose currentPose, double offset) {
+        updatePose(currentPose);
+        double dt = loopTimer.seconds();
+        loopTimer.reset();
+        if (dt < 0.001) dt = 0.001;
+
+        setAlliance(alliance);
+
+        double rawTicks = hardware.getTurretRotationPosition();
+        double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
+
+        distance = getDistance();
+
+        calculateHybridTarget(currentPose, currentDegrees, rawTicks);
+        runPIDControl(rawTicks, dt);
+        handleLaunchLogic(-offset);
+    }
+
+    private void handleLaunchLogic() {
+        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 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) {
+        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 rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+        fusedTargetTicks = Range.clip(rawTarget, Constants.TurretConstants.SOFT_LIMIT_NEG, Constants.TurretConstants.SOFT_LIMIT_POS);
+    }
+
+    private double calculateOdometryTargetTicks(Pose robotPose) {
+        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 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 = (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;
+    }
+
+    private void runPIDControl(double currentTicks, double dt) {
+        fusedTargetTicks = fusedTargetTicks + offsetTicks;
+        double error = fusedTargetTicks - currentTicks;
+        double derivative = (error - lastError) / dt;
+
+        if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) {
+            integralSum += (error * dt);
+        } else {
+            integralSum = 0;
+        }
+
+        double integralTerm = Range.clip(Constants.TurretConstants.KI * integralSum, -Constants.TurretConstants.MAX_INTEGRAL, Constants.TurretConstants.MAX_INTEGRAL);
+        double output = (Constants.TurretConstants.KP * error) + integralTerm + (Constants.TurretConstants.KD * derivative);
+        output = Range.clip(output, -Constants.TurretConstants.MAX_POWER, Constants.TurretConstants.MAX_POWER);
+
+        hardware.setTurretRotationPower(output);
+        turretPowerRotation = output;
+        lastError = error;
+    }
+
+    public boolean hasReachedVelocity() {
+        double currentVel = hardware.getFlywheelVelocities()[0];
+
+        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;
+
+            return error <= tolerance;
+        }
+        return false;
+    }
+
+    public boolean isTurretReady() {
+        return hasReachedVelocity();
+    }
+
+
+    public void updatePose(Pose robotPose) {
+        this.robotPose = robotPose;
+    }
+
+    public double getDistance() {
+        if (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;
+
+            return Math.hypot(targetX - robotPose.getX(), targetY - robotPose.getY());
+        }
+        return 0.0;
+    }
+
+    private double calculateHoodOffset(double target, double current) {
+        return (current - target) * 0.0002;
+    }
+
+    private double getFlywheelVelocity(double dist) {
+        return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
+    }
+
+    private double getHoodPOS(double dist) {
+        return ((-(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);
+    }
+
+    public void setHoodPos(double pos) {
+        hardware.setHoodPos(Range.clip(pos, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+    }
+
+    public void setLaunchPower(double power) {
+        this.turretPower = power;
+    }
+
+    public void updateOffsetTicks(double change) {
+        offsetTicks += change;
+    }
+
+    public double getTurretPower() {
+        return turretPower;
+    }
+
+    public double getTargetVelocity() {
+        return targetVelocity;
+    }
+}

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

@@ -0,0 +1,156 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.bylazar.configurables.annotations.Configurable;
+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 {
+        // Red Basket (Top Right)
+        public static final Pose RED_BASKET = new Pose(136, 139, 0);
+        // Blue Basket (Top Left)
+        public static final Pose BLUE_BASKET = new Pose(8, 139, 0);
+    }
+
+    // Drive Constants
+    public static class DriveConstants {
+        public static final String FRONT_LEFT_MOTOR = "fl";
+        public static final String FRONT_RIGHT_MOTOR = "fr";
+        public static final String BACK_LEFT_MOTOR = "bl";
+        public static final String BACK_RIGHT_MOTOR = "br";
+        public static final DcMotorSimple.Direction FRONT_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE;
+        public static final DcMotorSimple.Direction FRONT_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD;
+        public static final DcMotorSimple.Direction BACK_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE;
+        public static final DcMotorSimple.Direction BACK_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD;
+        public static final double TRACK_WIDTH = 13.5;
+        public static final double WHEEL_BASE = 13.5;
+        public static final double WHEEL_DIAMETER = 4.0;
+        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 IntakeConstants {
+        public static final String FRONT_INTAKE_MOTOR = "intake";
+
+        public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.REVERSE;
+
+        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 EXTAKE_POWER = -0.8;
+        public static final double LAUNCH_POWER = 1;
+        public static final double IDLE_POWER = 0;
+    }
+
+    public static class LiftConstants {
+        // Hardware Map Names
+        public static final String PTO_LEFT = "ptoLeft";
+        public static final String PTO_RIGHT = "ptoRight";
+
+        // Directions
+        public static final Servo.Direction PTO_LEFT_DIR = Servo.Direction.FORWARD;
+        public static final Servo.Direction PTO_RIGHT_DIR = Servo.Direction.REVERSE;
+
+        public static final double POS_ENGAGED = 0.8;
+        public static final double POS_DISENGAGED = 0.2;
+
+        // Motor Powers
+        public static final double POWER_LIFTING = 0.9;
+        public static final double POWER_HOLDING = 0.1;
+    }
+
+    public static class TransferConstants {
+        public static final String LIMITER_SERVO = "activeTransfer";
+
+        public static final Servo.Direction LIMITER_SERVO_DIR = Servo.Direction.FORWARD;
+
+
+        public static final String INDEX_SENSOR_A = "colorA";
+        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 UNLOCK_POS = 0.81;
+
+    }
+
+    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 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 final double MAX_INTEGRAL = 0.5;
+
+        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 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 final double SOFT_LIMIT_NEG = -100.0;
+        public static final double SOFT_LIMIT_POS = 250.0;
+
+        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 double HOOD_OFFSET = 0.3;
+
+        public static final double MANUAL_TURRET_SPEED_DEG = 1.0;
+
+        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 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 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_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 final double SERVO_MAX = 0.7;
+        public static final double SERVO_MIN = 0.26;
+    }
+
+
+
+}

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

@@ -0,0 +1,224 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+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;
+public class SOLOTeleOP {
+
+    @TeleOp(name = "Red SOLO", group = "FINAL")
+    public static class RedSOLO extends BaseSoloTeleOp {
+        public RedSOLO() {
+            super(2);
+        }
+    }
+    @TeleOp(name = "Blue SOLO", group = "FINAL")
+    public static class BlueSOLO extends BaseSoloTeleOp {
+        public BlueSOLO() {
+            super(1);
+        }
+    }
+
+    public static abstract class BaseSoloTeleOp extends LinearOpMode {
+
+        private StateMachine stateMachine;
+        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();
+
+        public BaseSoloTeleOp(int alliance) {
+            this.initialAlliance = alliance;
+            this.currentAlliance = 0;
+        }
+
+        @Override
+        public void runOpMode() throws InterruptedException {
+            initializeRobot();
+
+            if (AutoTransfer.isAutonRan) {
+                currentAlliance = AutoTransfer.alliance;
+                if (currentAlliance != initialAlliance) {
+                    telemetry.addLine("!!! WARNING: AUTON ALLIANCE DOES NOT MATCH TELEOP ALLIANCE!");
+                }
+                follower.setStartingPose(AutoTransfer.transferPose);
+                telemetry.addLine("AUTON DATA LOADED");
+            } else {
+                currentAlliance = initialAlliance;
+                if (currentAlliance == 2) follower.setStartingPose(DEFAULT_RED_START);
+                else follower.setStartingPose(DEFAULT_BLUE_START);
+                telemetry.addLine("MANUAL START - NO AUTON DATA");
+            }
+
+            stateMachine.getTurret().setAlliance(currentAlliance);
+
+            telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
+            telemetry.update();
+
+            waitForStart();
+            configureStartSettings();
+
+            fpsCounter.reset();
+
+            while (opModeIsActive()) {
+                fpsCounter.startLoop();
+
+                for (LynxModule hub : allHubs) {
+                    hub.clearBulkCache();
+                }
+
+
+                stateMachine.getDriveTrain().teleopDrive(
+                        gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.right_stick_x
+                );
+
+                follower.update();
+                Pose currentPose = follower.getPose();
+
+                //Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
+                //if (healingPose != null) {
+                   // follower.setPose(healingPose);
+                //}
+
+                handleGlobalLogic();
+                handleDriverControls();
+
+                stateMachine.update(false, false, currentPose);
+
+
+                updateTelemetry(currentPose);
+            }
+            stateMachine.setRobotState(RobotState.ESTOP);
+        }
+
+        private void initializeRobot() {
+            follower = Constants.createFollower(hardwareMap);
+
+            allHubs = hardwareMap.getAll(LynxModule.class);
+            for (LynxModule hub : allHubs) {
+                hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+            }
+
+            stateMachine = new StateMachine(
+                    new hwMap.DriveHwMap(hardwareMap),
+                    new hwMap.IntakeHwMap(hardwareMap),
+                    new hwMap.LiftHwMap(hardwareMap),
+                    new hwMap.TransferHwMap(hardwareMap),
+                    new hwMap.TurretHwMap(hardwareMap),
+                    new hwMap.LedHwMap(hardwareMap),
+                    AutoTransfer.motifPattern,
+                    currentAlliance
+
+            );
+            stateMachine.getTurret().setAlliance(0);
+
+            stateMachine.getTransfer().setTransferState(TransferSys.TransferState.STOP);
+
+
+            telemetry.addLine("DSS (Single Player) Initialized.");
+            telemetry.update();
+        }
+
+        private void configureStartSettings() {
+            stateMachine.setRobotState(RobotState.TELEOP);
+            stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+            stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
+            stateMachine.getTransfer().closeLimiter();
+            stateMachine.initUpdate();
+        }
+
+        private void handleGlobalLogic() {
+            if (currentAlliance == 0 && stateMachine.isMotifEdited()) {
+                currentAlliance = initialAlliance;
+                stateMachine.getTurret().setAlliance(currentAlliance);
+            }
+
+            if (gamepad1.startWasPressed() && currentAlliance != 0) {
+                currentAlliance = (currentAlliance == 1) ? 2 : 1;
+                stateMachine.getTurret().setAlliance(currentAlliance);
+            }
+
+            if (gamepad1.backWasPressed()) {
+                stateMachine.emergencyStop();
+            }
+
+            if (stateMachine.getCurrentGameState() != GameState.SCORING) {
+                if (!stateMachine.getTurret().hasReachedVelocity()) {
+                    int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
+                    gamepad1.rumble(rumbleDuration);
+                }
+            }
+
+            if (gamepad1.psWasPressed()) {
+                stateMachine.toggleAnchor();
+            }
+        }
+
+        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 (stateMachine.getCurrentGameState() != GameState.SCORING) {
+                if (gamepad1.right_trigger > 0.1) {
+                    stateMachine.getIntake().setIntakeState(Intake.IntakeState.INTAKE);
+                } else if (gamepad1.left_trigger > 0.1) {
+                    stateMachine.getIntake().setIntakeState(Intake.IntakeState.EXTAKE);
+                } else {
+                    if (stateMachine.getIntake().getCurrentState() == Intake.IntakeState.INTAKE ||
+                            stateMachine.getIntake().getCurrentState() == Intake.IntakeState.EXTAKE) {
+                        stateMachine.getIntake().setIntakeState(Intake.IntakeState.IDLE);
+                    }
+                }
+            }
+
+            if (gamepad1.yWasPressed()) {
+                isScoringActive = !isScoringActive;
+                if (isScoringActive) stateMachine.setGameState(GameState.SCORING);
+                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);
+
+
+        }
+
+        private void updateTelemetry(Pose p) {
+            telemetry.addData("OpMod ev", "SOLO TeleOp");
+            telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps());
+
+            telemetry.addLine("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
+            telemetry.addData("State", stateMachine.getCurrentRobotState());
+            telemetry.addData("Game Mode", stateMachine.getCurrentGameState());
+            telemetry.addData("Scoring", isScoringActive ? "ACTIVE" : "IDLE");
+            telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE");
+            telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
+            telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState());
+
+            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.update();
+        }
+    }
+}

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

@@ -0,0 +1,192 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.pedropathing.geometry.Pose; // ADDED
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
+import org.firstinspires.ftc.teamcode.subsystems.LED;
+import org.firstinspires.ftc.teamcode.subsystems.Lift;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Turret;
+
+enum RobotState {
+    INIT, TELEOP, ESTOP
+}
+enum GameState {
+    INTAKING, EXTAKING, SCORING, LIFTING, IDLE
+}
+
+public class StateMachine {
+
+    private RobotState currentRobotState = RobotState.INIT;
+    private GameState currentGameState = GameState.IDLE;
+    private final DriveTrain m_driveTrain;
+    private final Intake m_intake;
+    private final TransferSys m_transfer;
+    private final Turret m_turret;
+    private final Lift m_lift;
+
+    private final LED m_led;
+
+    boolean isMotifEdited = false;
+    private long lastIndexAllArtifactsTime = 0;
+
+    public boolean isIntakeLaunching = false;
+
+    public StateMachine(hwMap.DriveHwMap h_driveTrain, hwMap.IntakeHwMap h_intake, hwMap.LiftHwMap h_lift, hwMap.TransferHwMap h_transfer, hwMap.TurretHwMap h_turret, hwMap.LedHwMap h_led, int initialMotif, int alliance) {
+        this.m_driveTrain = new DriveTrain(h_driveTrain);
+        this.m_intake = new Intake(h_intake);
+        this.m_lift = new Lift(h_lift);
+        this.m_transfer = new TransferSys(h_transfer);
+        this.m_turret = new Turret(h_turret);
+        this.m_led = new LED(h_led);
+
+
+        setRobotState(RobotState.INIT);
+        setGameState(GameState.IDLE);
+    }
+
+    public void setRobotState(RobotState newState) {
+        if (this.currentRobotState == newState) return;
+        handleRobotStateExit(this.currentRobotState);
+        this.currentRobotState = newState;
+        handleRobotStateEntry(newState);
+    }
+
+    public void setGameState(GameState newState) {
+        if (this.currentGameState == newState) return;
+        handleGameStateExit(this.currentGameState);
+        this.currentGameState = newState;
+        handleGameStateEntry(newState);
+    }
+
+    private void handleGameStateExit(GameState oldState) {
+        switch (oldState) {
+            case SCORING:
+                m_transfer.setTransferState(TransferSys.TransferState.IDLE);
+                m_turret.setTurretState(Turret.TurretState.IDLE);
+                m_intake.setIntakeState(Intake.IntakeState.IDLE);
+                isIntakeLaunching = false;
+                //m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+                m_driveTrain.stopAnchor();
+                break;
+            case LIFTING:
+                //m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+                break;
+        }
+    }
+
+    public void update(boolean manualSAM, boolean manualTracking, Pose currentPose) {
+        m_driveTrain.update();
+
+        m_turret.update(manualTracking, manualSAM, currentPose);
+
+        if (currentGameState == GameState.SCORING) {
+            m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
+            if (m_turret.hasReachedVelocity()) {
+                isIntakeLaunching = true;
+                m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
+            }
+        }
+
+        m_transfer.update();
+
+        m_led.update(
+                currentGameState == GameState.SCORING,
+                m_turret.hasReachedVelocity(),
+                m_turret.hasTarget(),
+                m_driveTrain.isAnchorActive()
+        );
+
+        if (currentGameState == GameState.SCORING) {
+            if (m_transfer.getTransferState() != TransferSys.TransferState.LAUNCHING) {
+                setGameState(GameState.IDLE);
+            }
+        }
+    }
+
+    public void initUpdate() {
+        m_led.update(false, false, false, false);
+    }
+
+    private void handleGameStateEntry(GameState newState) {
+        switch (newState) {
+            case INTAKING:
+                //m_driveTrain.setDriveState(DriveTrain.DriveState.TURBO);
+                m_intake.setIntakeState(Intake.IntakeState.INTAKE);
+                break;
+            case EXTAKING:
+                //m_driveTrain.setDriveState(DriveTrain.DriveState.TURBO);
+                m_intake.setIntakeState(Intake.IntakeState.EXTAKE);
+                break;
+            case SCORING:
+                m_transfer.setTransferState(TransferSys.TransferState.LAUNCHING);
+                m_turret.setTurretState(Turret.TurretState.LAUNCH);
+                break;
+            case LIFTING:
+                //m_driveTrain.setDriveState(DriveTrain.DriveState.STOP);
+                m_intake.setIntakeState(Intake.IntakeState.IDLE);
+                m_transfer.setTransferState(TransferSys.TransferState.IDLE);
+                m_turret.setTurretState(Turret.TurretState.IDLE);
+                m_lift.setLiftState(Lift.LiftState.BEGIN_LIFT);
+                break;
+            case IDLE:
+                m_intake.setIntakeState(Intake.IntakeState.IDLE);
+                //m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+                break;
+        }
+    }
+
+    public void holdLift() {
+        if (currentGameState == GameState.LIFTING) {
+            m_lift.setLiftState(Lift.LiftState.HOLD_LIFT);
+        }
+    }
+
+    public void toggleAnchor() {
+        if (m_driveTrain.isAnchorActive()) {
+            m_driveTrain.stopAnchor();
+        } else {
+            m_driveTrain.startAnchor();
+        }
+    }
+
+
+    private void handleRobotStateExit(RobotState oldState) {
+        switch (oldState) {
+            case TELEOP:
+                m_driveTrain.stop();
+                break;
+            case ESTOP:
+                break;
+        }
+    }
+
+    private void handleRobotStateEntry(RobotState newState) {
+        switch (newState) {
+            case TELEOP:
+                m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+                m_intake.setIntakeState(Intake.IntakeState.IDLE);
+                break;
+            case ESTOP:
+                m_driveTrain.setDriveState(DriveTrain.DriveState.STOP);
+                m_intake.setIntakeState(Intake.IntakeState.STOP);
+                break;
+        }
+    }
+
+    public void emergencyStop() {
+        setRobotState(RobotState.ESTOP);
+        setGameState(GameState.IDLE);
+    }
+
+    public DriveTrain getDriveTrain() { return m_driveTrain; }
+    public Intake getIntake() { return m_intake; }
+    public Turret getTurret() { return m_turret; }
+    public TransferSys getTransfer() { return m_transfer; }
+    public RobotState getCurrentRobotState() { return currentRobotState; }
+    public GameState getCurrentGameState() { return currentGameState; }
+    public boolean isMotifEdited() { return isMotifEdited; }
+}

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

@@ -0,0 +1,289 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+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.util.AutoTransfer;
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+import com.qualcomm.hardware.lynx.LynxModule;
+import java.util.List;
+
+public class finalTeleOp {
+
+    @TeleOp(name = "Red Final", group = "FINAL")
+    public static class RedFinal extends BaseFinalTeleOp {
+        public RedFinal() {
+            super(2);
+        }
+    }
+
+    @TeleOp(name = "Blue Final", group = "FINAL")
+    public static class BlueFinal extends BaseFinalTeleOp {
+        public BlueFinal() {
+            super(1);
+        }
+    }
+
+    public static abstract class BaseFinalTeleOp extends LinearOpMode {
+
+        protected StateMachine stateMachine;
+        protected Follower follower;
+
+        protected int currentAlliance;
+        protected final int initialAlliance;
+        protected boolean launching = false;
+        protected boolean manualTracking = false;
+        protected boolean manualSAM = false;
+
+        protected double hoodPosition = 0.5;
+
+        protected List<LynxModule> allHubs;
+
+        // Default Start Poses for manual testing (if Auton didn't run)
+
+        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();
+
+        public BaseFinalTeleOp(int alliance) {
+            this.initialAlliance = alliance;
+            this.currentAlliance = 0;
+        }
+
+        @Override
+        public void runOpMode() throws InterruptedException {
+            initializeRobot();
+
+            if (AutoTransfer.isAutonRan) {
+                currentAlliance = AutoTransfer.alliance;
+                if (currentAlliance != initialAlliance) {
+                    telemetry.addLine("WARNING: AUTON ALLIANCE DOES NOT MATCH TELEOP ALLIANCE!");
+                }
+                follower.setStartingPose(AutoTransfer.transferPose);
+                telemetry.addLine("AUTON DATA LOADED");
+            } else {
+                currentAlliance = initialAlliance;
+                if (currentAlliance == 2) follower.setStartingPose(DEFAULT_RED_START);
+                else follower.setStartingPose(DEFAULT_BLUE_START);
+                telemetry.addLine("MANUAL START - NO AUTON DATA");
+            }
+
+            stateMachine.getTurret().setAlliance(currentAlliance);
+
+            telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
+            telemetry.update();
+
+            waitForStart();
+            configureStartSettings();
+
+            fpsCounter.reset();
+
+            while (opModeIsActive()) {
+                fpsCounter.startLoop();
+
+                if (stateMachine.getCurrentGameState() != GameState.SCORING) {
+                    stateMachine.getTransfer().closeLimiter();
+                } else if (stateMachine.getCurrentGameState() == GameState.SCORING && stateMachine.getTurret().isTurretReady()) {
+                    stateMachine.getTransfer().openLimiter();
+                }
+
+                for (LynxModule hub : allHubs) {
+                    hub.clearBulkCache();
+                }
+
+                follower.update();
+                Pose currentPose = follower.getPose();
+
+                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();
+                    }
+                }
+
+
+                updateTelemetry(currentPose);
+            }
+
+            stateMachine.setRobotState(RobotState.ESTOP);
+        }
+
+        private void initializeRobot() {
+            follower = Constants.createFollower(hardwareMap);
+
+            allHubs = hardwareMap.getAll(LynxModule.class);
+            for (LynxModule hub : allHubs) {
+                hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+            }
+
+            stateMachine = new StateMachine(
+                    new hwMap.DriveHwMap(hardwareMap),
+                    new hwMap.IntakeHwMap(hardwareMap),
+                    new hwMap.LiftHwMap(hardwareMap),
+                    new hwMap.TransferHwMap(hardwareMap),
+                    new hwMap.TurretHwMap(hardwareMap),
+                    new hwMap.LedHwMap(hardwareMap),
+                    AutoTransfer.motifPattern,
+                    currentAlliance
+            );
+
+
+            stateMachine.getTransfer().closeLimiter();
+
+            stateMachine.getTurret().setAlliance(currentAlliance);
+        }
+
+        private void configureStartSettings() {
+            stateMachine.setRobotState(RobotState.TELEOP);
+            stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+            stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
+            stateMachine.getTransfer().closeLimiter();
+            stateMachine.initUpdate();
+        }
+
+        private void handleGlobalLogic() {
+
+            if (gamepad1.backWasPressed()) {
+                stateMachine.emergencyStop();
+            }
+
+            if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity()) {
+                int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
+                gamepad2.rumble(rumbleDuration);
+            }
+
+            if (gamepad1.psWasPressed()) {
+                stateMachine.toggleAnchor();
+            }
+
+            if (gamepad2.psWasPressed()) {
+                if (currentAlliance == 2) { // RED
+                    follower.setPose(new Pose(135, 9, 0));
+                } else if (currentAlliance == 1) {
+                    follower.setPose(new Pose(9, 9, Math.toRadians(180)));
+
+                }
+            }
+
+        }
+
+        private void handleDriverInput() {
+            stateMachine.getDriveTrain().teleopDrive(
+                    gamepad1.left_stick_x,
+                    -gamepad1.left_stick_y,
+                    gamepad1.right_stick_x
+            );
+
+            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 && !stateMachine.isIntakeLaunching) {
+                if (gamepad1.right_trigger > 0.1 || gamepad2.right_trigger > 0.1) {
+                    stateMachine.getIntake().setIntakeState(Intake.IntakeState.INTAKE);
+                } else if (gamepad1.left_trigger > 0.1 || gamepad2.left_trigger > 0.1) {
+                    stateMachine.getIntake().setIntakeState(Intake.IntakeState.EXTAKE);
+                } else {
+                    if (stateMachine.getIntake().getCurrentState() == Intake.IntakeState.INTAKE ||
+                            stateMachine.getIntake().getCurrentState() == Intake.IntakeState.EXTAKE) {
+                        stateMachine.getIntake().setIntakeState(Intake.IntakeState.IDLE);
+                    }
+                }
+            }
+
+
+
+            if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.6);
+            if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.6);
+        }
+
+        private void handleOperatorInput() {
+            if (gamepad2.yWasPressed()) {
+                launching = !launching;
+                if (launching) {
+                    stateMachine.setGameState(GameState.SCORING);
+                } else if (stateMachine.getCurrentGameState() == GameState.SCORING) {
+                    stateMachine.setGameState(GameState.IDLE);
+                }
+            }
+
+            if (gamepad2.backWasPressed()) {
+                manualSAM = !manualSAM;
+            }
+
+            if (gamepad2.psWasPressed()) {
+                manualTracking = !manualTracking;
+            }
+
+            if (manualSAM) {
+                handleManualSAM();
+            }
+        }
+
+        private void handleManualSAM() {
+            if (gamepad2.dpad_up) {
+                stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MAX);
+                stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_FAR);
+            } else if (gamepad2.dpad_left) {
+                stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
+                stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_MID);
+            } else if (gamepad2.dpad_down) {
+                stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_LOW);
+                stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_CLOSE);
+            }
+
+            if (Math.abs(gamepad2.left_stick_y) > 0.13) {
+                hoodPosition += -gamepad2.left_stick_y * 0.05;
+                hoodPosition = Range.clip(hoodPosition, 0.0, 1.0);
+            }
+            stateMachine.getTurret().setHoodPos(hoodPosition);
+        }
+
+        private void updateTelemetry(Pose p) {
+            telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps());
+
+            telemetry.addLine("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
+            telemetry.addData("State", stateMachine.getCurrentRobotState());
+            telemetry.addData("Game Mode", stateMachine.getCurrentGameState());
+            telemetry.addData("Scoring", stateMachine.getCurrentGameState() == GameState.SCORING ? "ACTIVE" : "IDLE");
+            telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE");
+            telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
+            telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState());
+
+            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("Hood Offset", stateMachine.getTurret().offsetHood);
+
+
+
+            telemetry.update();
+        }
+    }
+}

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

@@ -0,0 +1,113 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import android.graphics.Color;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+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;
+@TeleOp(name="Color Sensor Tuning", group="Diagnostics")
+public class ColorSensorTuning extends LinearOpMode {
+
+    hwMap.TransferHwMap transfer;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+         transfer = new hwMap.TransferHwMap(hardwareMap);
+
+        telemetry.addLine("═══════════════════════════════");
+        telemetry.addLine("  DUAL COLOR SENSOR TUNING");
+        telemetry.addLine("═══════════════════════════════");
+        telemetry.addLine("\nEach slot has 2 sensors for redundancy");
+        telemetry.addLine("Uses both sensors for best accuracy");
+        telemetry.update();
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            telemetry.clearAll();
+            telemetry.addLine("═══════════════════════════════");
+            telemetry.addLine("  DUAL SENSOR READINGS");
+            telemetry.addLine("═══════════════════════════════");
+
+            // Display all three slots with both sensors
+            for (int slot = 1; slot <= 3; slot++) {
+                displayDualSensorData(transfer, slot);
+            }
+
+            telemetry.addLine("\n═══════════════════════════════");
+            telemetry.addLine("  FINAL DETECTION RESULTS");
+            telemetry.addLine("═══════════════════════════════");
+            telemetry.addData("Slot A", getColorName(detectArtifactColor(1)));
+            telemetry.addData("Slot B", getColorName(detectArtifactColor(2)));
+            telemetry.addData("Slot C", getColorName(detectArtifactColor(3)));
+
+            telemetry.update();
+            sleep(100);
+        }
+    }
+
+    /**
+     * Detect color using BOTH sensors and pick the best result
+     */
+    public int detectArtifactColor(int index) {
+        if(index < 1 || index > 3) return 0;
+        int i = index - 1;
+
+        NormalizedRGBA colors2 = transfer.sensors[i].getNormalizedColors();
+
+        float[] hsv = new float[3];
+        Color.colorToHSV(colors2.toColor(), hsv);
+
+        int color = detectColor(hsv[0], hsv[1], hsv[2]);
+
+        double confidence = hsv[1] * hsv[2];
+
+        // Decision Matrix
+        if (color == 0 && confidence == 0) return 0;
+        else {
+            return color;
+        }
+
+    }
+
+    private int detectColor(float hue, float sat, float val) {
+        if (sat < 0.15 || val < 0.1) return 0;
+
+        if (hue > 170 && hue < 310) return 1;
+        if (hue > 80 && hue < 170) return 2;
+
+        return 0;
+    }
+    private void displayDualSensorData(hwMap.TransferHwMap transfer, int slot) {
+        int index = slot - 1;
+        String slotName = (slot == 1) ? "A" : (slot == 2) ? "B" : "C";
+
+        NormalizedRGBA colors = transfer.sensors[index].getNormalizedColors();
+
+        float[] hsv = new float[3];
+        Color.colorToHSV(colors.toColor(), hsv);
+
+        telemetry.addLine("\n───── Slot " + slotName + " ─────");
+
+        // Sensor 1 (Wall)
+        telemetry.addLine( "Sensors:");
+        telemetry.addData("  HSV", "H:%.0f S:%.2f V:%.2f", hsv[0], hsv[1], hsv[2]);
+        telemetry.addData("  Detects", getColorName(detectColor(hsv[0], hsv[1], hsv[2])));
+        telemetry.addData("  Confidence", String.format("%.2f", hsv[1] * hsv[2]));
+
+        // Show which sensor is being used
+        int color1 = detectColor(hsv[0], hsv[1], hsv[2]);
+        double conf1 = hsv[1] * hsv[2];
+    }
+
+    private String getColorName(int colorCode) {
+        switch (colorCode) {
+            case 0: return "NONE";
+            case 1: return "PURPLE";
+            case 2: return "GREEN";
+            default: return "UNKNOWN";
+        }
+    }
+}

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

@@ -0,0 +1,94 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.telemetry.TelemetryManager;
+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.DcMotorEx;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import com.bylazar.telemetry.PanelsTelemetry;
+
+import com.bylazar.configurables.annotations.Configurable;
+
+
+@Configurable
+@TeleOp(name = "Limelight Distance", group = "Sensor")
+public class GetDistanceTuning extends OpMode {
+
+    private Limelight3A limelight;
+    private double distance;
+    public static double CAMERA_HEIGHT_INCHES = 12.5;
+    public static double GOAL_HEIGHT_INCHES = 29.5;
+    public static int pipeline = 1;
+    public static double CAMERA_MOUNT_ANGLE_DEGREES = 14.3;
+
+    private TelemetryManager telemetryM;
+
+
+
+    @Override
+    public void init() {
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        limelight.pipelineSwitch(pipeline);
+
+        telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+        limelight.start();
+
+        telemetryM.debug("Status", "Initialized");
+        telemetryM.update(telemetry);
+    }
+
+    @Override
+    public void loop() {
+        LLResult result = limelight.getLatestResult();
+
+        if (result != null && result.isValid()) {
+            double targetY = result.getTy();
+            double targetArea = result.getTa();
+
+
+            distance = getTrigBasedDistance(targetY);
+
+            double areaDistance = getDistanceFromTag(targetArea);
+
+            telemetryM.debug("Method", "Trigonometry");
+            telemetryM.debug("Target Y (ty)", targetY);
+            telemetryM.debug("Calculated Distance", distance);
+
+            telemetryM.debug("Area Algo Distance", areaDistance);
+
+            telemetryM.debug("Flywheel Velocity", getFlywheelVelocity(distance));
+            telemetryM.debug("Hood Position", getHoodPOS(distance));
+
+        } else {
+            telemetryM.debug("Limelight", "No Target Found");
+        }
+
+        telemetryM.update(telemetry);
+    }
+
+    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) {
+        return (74.34095 * Math.pow(x, -0.518935));
+    }
+
+    private double getFlywheelVelocity(double dist) {
+        return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
+    }
+
+    private double getHoodPOS(double dist) {
+        return Range.clip(((-(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), Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX);
+    }
+}

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

@@ -0,0 +1,201 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+@Disabled
+@TeleOp(name="Limelight Turret", group="Tuning")
+public class LimelightEncoderTurret extends LinearOpMode {
+
+    private Limelight3A limelight;
+    private CRServo turretServo;
+    private DcMotor turretEncoder;
+
+    // Encoder specs
+    private final double ENCODER_TICKS_PER_REV = 8392; // Rev Through Bore
+    private final double ENCODER_TICKS_PER_DEGREE = ENCODER_TICKS_PER_REV / 360.0;
+
+    // ONLY tunable parameter - adjust this multiplier
+    private double rotationMultiplier = 1.0; // Tune this if turret over/under rotates
+
+    private double maxSpeed = 1.0; // Full speed
+    private double deadzone = 2.0; // Stop when within 2 degrees
+
+    private int targetTicks = 0;
+    private boolean trackingActive = false;
+
+    // Debouncing
+    private boolean lastDpadUp = false;
+    private boolean lastDpadDown = false;
+    private boolean lastA = false;
+    private boolean lastB = false;
+    private boolean lastX = false;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        // Initialize hardware
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        turretServo = hardwareMap.get(CRServo.class, "turretservo");
+        turretEncoder = hardwareMap.get(DcMotor.class, "lturret");
+
+        // Setup encoder
+        turretEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        turretEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        // Start Limelight on pipeline 8
+        limelight.pipelineSwitch(8);
+        limelight.start();
+
+        telemetry.addLine("═══════════════════════════════");
+        telemetry.addLine("  SIMPLE LIMELIGHT TURRET");
+        telemetry.addLine("═══════════════════════════════");
+        telemetry.addLine("\nCamera → Calculate ticks → Spin");
+        telemetry.addLine("\nControls:");
+        telemetry.addLine("  D-Pad Up: Increase Multiplier");
+        telemetry.addLine("  D-Pad Down: Decrease Multiplier");
+        telemetry.addLine("  A: Aim at Target (One-Shot)");
+        telemetry.addLine("  B: Manual Control (Right Stick)");
+        telemetry.addLine("  X: Reset Encoder Zero");
+        telemetry.addLine("\nPress START when ready");
+        telemetry.update();
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+
+            // Adjust rotation multiplier
+            if (gamepad1.dpad_up && !lastDpadUp) {
+                rotationMultiplier += 0.05;
+            }
+            lastDpadUp = gamepad1.dpad_up;
+
+            if (gamepad1.dpad_down && !lastDpadDown) {
+                rotationMultiplier -= 0.05;
+                if (rotationMultiplier < 0.1) rotationMultiplier = 0.1;
+            }
+            lastDpadDown = gamepad1.dpad_down;
+
+            // Trigger aiming
+            if (gamepad1.a && !lastA) {
+                aimAtTarget();
+            }
+            lastA = gamepad1.a;
+
+            // Manual control
+            if (gamepad1.b) {
+                turretServo.setPower(gamepad1.right_stick_x * maxSpeed);
+                trackingActive = false;
+            } else if (trackingActive) {
+                moveToTarget();
+            } else {
+                turretServo.setPower(0);
+            }
+
+            // Reset encoder
+            if (gamepad1.x && !lastX) {
+                turretEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                turretEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                targetTicks = 0;
+                trackingActive = false;
+            }
+            lastX = gamepad1.x;
+
+            // Display telemetry
+            displayTelemetry();
+        }
+
+        turretServo.setPower(0);
+        limelight.stop();
+    }
+
+    /**
+     * Use Limelight to calculate target and start moving
+     */
+    private void aimAtTarget() {
+        LLResult result = limelight.getLatestResult();
+
+        if (result == null || !result.isValid()) {
+            telemetry.addLine("\n✗ NO TARGET FOUND");
+            telemetry.update();
+            return;
+        }
+
+        // Get angle offset from camera
+        double txDegrees = result.getTx();
+
+        // Convert to ticks (apply multiplier for tuning)
+        double ticksToRotate = txDegrees * ENCODER_TICKS_PER_DEGREE * rotationMultiplier;
+
+        // Set target relative to current position
+        int currentTicks = turretEncoder.getCurrentPosition();
+        targetTicks = currentTicks + (int)ticksToRotate;
+
+        trackingActive = true;
+    }
+
+    private void moveToTarget() {
+        int currentTicks = turretEncoder.getCurrentPosition();
+        int error = targetTicks - currentTicks;
+
+        // Convert error to degrees for deadzone check
+        double errorDegrees = error / ENCODER_TICKS_PER_DEGREE;
+
+        if (Math.abs(errorDegrees) < deadzone) {
+            // Close enough - stop
+            turretServo.setPower(0);
+            trackingActive = false;
+        } else {
+            // Spin at max speed in correct direction
+            double power = (error > 0) ? maxSpeed : -maxSpeed;
+            turretServo.setPower(power); // Negative if servo is reversed
+        }
+    }
+
+    private void displayTelemetry() {
+        LLResult result = limelight.getLatestResult();
+        int currentTicks = turretEncoder.getCurrentPosition();
+        double currentDegrees = currentTicks / ENCODER_TICKS_PER_DEGREE;
+
+        telemetry.clearAll();
+        telemetry.addLine("═══════════════════════════════");
+        telemetry.addLine("  🎯 SIMPLE TURRET AIMING");
+        telemetry.addLine("═══════════════════════════════");
+
+        telemetry.addLine("\n───── Status ─────");
+        telemetry.addData("Tracking", trackingActive ? "→ MOVING" : "⊙ IDLE");
+        telemetry.addData("Camera", (result != null && result.isValid()) ? "✓ TARGET" : "✗ NO TARGET");
+
+        if (result != null && result.isValid()) {
+            telemetry.addData("Camera Angle", String.format("%.2f°", result.getTx()));
+        }
+
+        telemetry.addLine("\n───── Encoder ─────");
+        telemetry.addData("Current Ticks", currentTicks);
+        telemetry.addData("Current Angle", String.format("%.1f°", currentDegrees));
+        telemetry.addData("Target Ticks", targetTicks);
+        telemetry.addData("Error", String.format("%d ticks", targetTicks - currentTicks));
+
+        telemetry.addLine("\n───── Tuning ─────");
+        telemetry.addData(">>> Rotation Multiplier", String.format("%.2f <<<", rotationMultiplier));
+        telemetry.addLine("");
+        telemetry.addLine("If turret OVERSHOOTS:");
+        telemetry.addLine("  → Press D-Pad DOWN");
+        telemetry.addLine("If turret UNDERSHOOTS:");
+        telemetry.addLine("  → Press D-Pad UP");
+
+        telemetry.addLine("\n───── How to Use ─────");
+        telemetry.addLine("1. Point camera at AprilTag");
+        telemetry.addLine("2. Press A to aim");
+        telemetry.addLine("3. Turret rotates at full speed");
+        telemetry.addLine("4. Stops when close enough");
+
+        telemetry.addLine("\n───── Copy to Code ─────");
+        telemetry.addLine(String.format("ROTATION_MULTIPLIER = %.3f;", rotationMultiplier));
+
+        telemetry.update();
+    }
+}

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

@@ -0,0 +1,84 @@
+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.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 curTargetVelocity = 0;
+    public static double targetPOS = 0;
+    public static int pipeline = 1;
+
+    double distance = 0;
+
+    @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(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();
+
+        telemetry.addLine("Init complete");
+    }
+
+    @Override
+    public void loop() {
+        limelight.pipelineSwitch(pipeline);
+
+        LLResult result = limelight.getLatestResult();
+
+        if (result.isValid()) {
+            double targetY = result.getTy();
+            telemetry.addData("Ty", targetY);
+            distance = getTrigBasedDistance(targetY);
+        }
+
+        if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
+        flywheelMotorL.setVelocity(curTargetVelocity);}
+
+        hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+
+
+
+        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.update();
+    }
+
+    private double getTrigBasedDistance(double targetOffsetAngleVertical) {
+        return (14) / Math.tan(Math.toRadians(2.0 + targetOffsetAngleVertical));
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismArtboardExample.java

@@ -0,0 +1,105 @@
+/*   MIT License
+ *   Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.tuning.Prism;
+
+
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.Artboard;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver;
+
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This example shows how to recall previously created Artboards on the goBILDA Prism RGB Driver.
+ * Recalling these animations is all we recommend that your main robot OpMode do. We prefer to
+ * create these Artboards once and store them on the device, allowing for a short I²C write to
+ * set the current Artboard.
+ *
+ * It also shows how to enable or disable the default boot animation on power up. When enabled,
+ * as soon as the Prism gets power it will display the Artboard saved in Artboard slot 0. If you'd
+ * instead like to wait until you explicitly set an Artboard, disable this setting. This setting is
+ * saved and will only need to be set once.
+ *
+ */
+
+@TeleOp(name="Prism Artboard Example", group="Linear OpMode")
+//@Disabled
+
+public class GoBildaPrismArtboardExample extends LinearOpMode {
+
+    GoBildaPrismDriver prism;
+
+    @Override
+    public void runOpMode() {
+
+        /*
+         * Initialize the hardware variables. Note that the strings used here must correspond
+         * to the names assigned during the robot configuration step on the driver's station.
+         */
+        prism = hardwareMap.get(GoBildaPrismDriver.class, "prism");
+
+        telemetry.addData("Device ID: ", prism.getDeviceID());
+        telemetry.addData("Firmware Version: ", prism.getFirmwareVersionString());
+        telemetry.addData("Hardware Version: ", prism.getHardwareVersionString());
+        telemetry.addData("Power Cycle Count: ", prism.getPowerCycleCount());
+        telemetry.addData("Run Time (Minutes): ", prism.getRunTime(TimeUnit.MINUTES));
+        telemetry.update();
+
+        // Wait for the game to start (driver presses START)
+        waitForStart();
+        resetRuntime();
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+            if(gamepad1.aWasPressed()){
+                prism.loadAnimationsFromArtboard(Artboard.ARTBOARD_0);
+            } else if(gamepad1.bWasPressed()){
+                prism.loadAnimationsFromArtboard(Artboard.ARTBOARD_1);
+            } else if(gamepad1.yWasPressed()){
+                prism.loadAnimationsFromArtboard(Artboard.ARTBOARD_2);
+            } else if(gamepad1.xWasPressed()){
+                prism.loadAnimationsFromArtboard(Artboard.ARTBOARD_3);
+            }
+
+            if(gamepad1.dpadLeftWasPressed()){prism.enableDefaultBootArtboard(false);}
+            if(gamepad1.dpadRightWasPressed()){prism.enableDefaultBootArtboard(true);}
+
+            telemetry.addLine("Press A to recall Artboard #0");
+            telemetry.addLine("Press B to recall Artboard #1");
+            telemetry.addLine("Press Y to recall Artboard #2");
+            telemetry.addLine("Press X to recall Artboard #3");
+            telemetry.addLine("");
+            telemetry.addLine("By default the Prism will wait for an I²C signal to " +
+                    " start showing an animation.The prism can enable a Default Boot animation" +
+                    " which will play the animation stored at Artboard #0 automatically on power up.");
+            telemetry.addLine("Press D-Pad Right to enable the default boot animation." +
+                    " Press D-Pad Left to disable it.");
+            telemetry.update();
+            sleep(20);
+
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismExample.java

@@ -0,0 +1,148 @@
+/*   MIT License
+ *   Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ *   Permission is hereby granted, free of charge, to any person obtaining a copy
+ *   of this software and associated documentation files (the "Software"), to deal
+ *   in the Software without restriction, including without limitation the rights
+ *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ *   copies of the Software, and to permit persons to whom the Software is
+ *   furnished to do so, subject to the following conditions:
+
+ *   The above copyright notice and this permission notice shall be included in all
+ *   copies or substantial portions of the Software.
+
+ *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *   SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.tuning.Prism;
+
+
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Prism.Color;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This example file shows how to create a couple of different Animations on the Prism, and save
+ * them to an Artboard on the device.
+ * The example file called "GoBildaPrismConfigurator" is designed to create a user-interface
+ * through your driver's station. This is a great way to create Artboards from Animations, but
+ * doesn't let you control quite everything about the available Animations. If you'd like to
+ * do something more complex, or would just prefer to create your Artboard in Java, this
+ * program shows you how.
+ * The example file called "GoBildaPrismArtboardExample" shows how to recall different Artboards
+ * that you have already saved to the device. That file shows the code you should consider adding
+ * to your OpMode if you would like to dynamically change the Artboard shown by your LEDs during
+ * the match.
+ *
+ * Core to understanding how to use this product is knowing these three terms:
+ * Animations: (Like RAINBOW or BLINK) - These have properties you can configure, like their color.
+ * They can have unique start and end points!
+ * Layers: There are 10 layers, each of which can store an animation. These are hierarchical.
+ * So an Animation on layer 5 will cover an animation on layer 2 if they overlap.
+ * You can use start and end points to have layers overlap to create new patterns! Or show multiple
+ * animations at once on different LEDs.
+ * Artboards: An Artboard is a set of 10 layers which is stored on the Prism.
+ * you can have up to 8 unique Artboards. Artboards are easy and computationally fast to switch between.
+ */
+
+@TeleOp(name="Prism Animations Example", group="Linear OpMode")
+//@Disabled
+
+public class GoBildaPrismExample extends LinearOpMode {
+
+    GoBildaPrismDriver prism;
+
+    PrismAnimations.Solid solid = new PrismAnimations.Solid(Color.BLUE);
+    PrismAnimations.RainbowSnakes rainbowSnakes = new PrismAnimations.RainbowSnakes();
+
+    @Override
+    public void runOpMode() {
+        /*
+         * Initialize the hardware variables. Note that the strings used here must correspond
+         * to the names assigned during the robot configuration step on the driver's station.
+         */
+        prism = hardwareMap.get(GoBildaPrismDriver.class,"prism");
+
+        /*
+         * Here you can customize the specifics of different animations. Each animation has it's
+         * own set of parameters that you can customize to create something unique! Each animation
+         * has carefully selected default parameters. So you do not need to set each parameter
+         * for every animation!
+         */
+        solid.setBrightness(50);
+        solid.setStartIndex(0);
+        solid.setStopIndex(12);
+
+        rainbowSnakes.setNumberOfSnakes(2);
+        rainbowSnakes.setSnakeLength(3);
+        rainbowSnakes.setSpacingBetween(6);
+        rainbowSnakes.setSpeed(0.5f);
+
+        telemetry.addData("Device ID: ", prism.getDeviceID());
+        telemetry.addData("Firmware Version: ", prism.getFirmwareVersionString());
+        telemetry.addData("Hardware Version: ", prism.getHardwareVersionString());
+        telemetry.addData("Power Cycle Count: ", prism.getPowerCycleCount());
+        telemetry.update();
+
+        // Wait for the game to start (driver presses START)
+        waitForStart();
+        resetRuntime();
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+
+
+            if(gamepad1.aWasPressed()){
+                /*
+                 * Here we insert and update the animation to the Prism, this by default does not
+                 * save it to an Artboard, it just starts the Animation playing. If you have
+                 * already inserted an animation at a layer height, you can instead call
+                 * .updateAnimationFromIndex(LayerHeight.LAYER_0) to update an animation at a
+                 * specific layer height without overwriting it completely.
+                 */
+                prism.insertAndUpdateAnimation(LayerHeight.LAYER_0, solid);
+                prism.insertAndUpdateAnimation(LayerHeight.LAYER_1,rainbowSnakes);
+            }
+
+            if(gamepad1.xWasPressed()){
+                /*
+                 * Clearing the animation doesn't erase any saved Artboards, but it removes all the
+                 * currently displayed animations.
+                 */
+                prism.clearAllAnimations();
+            }
+
+            if(gamepad1.dpadDownWasPressed()){
+                /*
+                 * Here we save the animation we are currently displaying to Artboard 0.
+                 */
+                prism.saveCurrentAnimationsToArtboard(GoBildaPrismDriver.Artboard.ARTBOARD_0);
+            }
+
+            telemetry.addLine("Press A to insert and update the created animations.");
+            telemetry.addLine("Press X to clear current animations.");
+            telemetry.addLine("Press D-Pad Down to save current animations to Artboard #0");
+            telemetry.addLine();
+            telemetry.addData("Run Time (Hours): ",prism.getRunTime(TimeUnit.HOURS));
+            telemetry.addData("Run Time (Minutes): ",prism.getRunTime(TimeUnit.MINUTES));
+            telemetry.addData("Number of LEDS: ", prism.getNumberOfLEDs());
+            telemetry.addData("Current FPS: ", prism.getCurrentFPS());
+            telemetry.update();
+            sleep(50);
+        }
+    }
+
+}

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

@@ -0,0 +1,291 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.bylazar.configurables.annotations.Configurable;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Configurable
+@TeleOp(name = "TestBench", group = "Maintenance")
+public class TestBench extends LinearOpMode {
+
+    public static String SERVO_NAME = "servo";
+    public static boolean IS_CR_SERVO = false;
+
+    public static String MOTOR_NAME = "motor";
+    public static boolean IS_MOTOR_EX = false;
+
+    public static String ODO_LEFT_NAME = "intake";
+    public static String ODO_RIGHT_NAME = "fl";
+    public static String ODO_STRAFE_NAME = "fr";
+    public static boolean REVERSE_LEFT_ENCODER = false;
+    public static boolean REVERSE_RIGHT_ENCODER = false;
+    public static boolean REVERSE_STRAFE_ENCODER = false;
+
+    private enum Mode {
+        SERVO_TESTER,
+        MOTOR_TESTER,
+        ODOMETRY_TUNER
+    }
+
+    @Override
+    public void runOpMode() {
+        Mode[] modes = Mode.values();
+        int modeIndex = 0;
+        boolean locked = false;
+        boolean lastUp = false;
+        boolean lastDown = false;
+
+        while (!isStopRequested() && !locked) {
+            boolean currentUp = gamepad1.dpad_up;
+            boolean currentDown = gamepad1.dpad_down;
+
+            if (currentUp && !lastUp) {
+                modeIndex--;
+                if (modeIndex < 0) modeIndex = modes.length - 1;
+            }
+            if (currentDown && !lastDown) {
+                modeIndex++;
+                if (modeIndex >= modes.length) modeIndex = 0;
+            }
+            lastUp = currentUp;
+            lastDown = currentDown;
+
+            if (gamepad1.a) {
+                locked = true;
+            }
+
+            telemetry.addLine("▰▰▰▰▰▰▰ SYSTEM CONFIG MENU ▰▰▰▰▰▰▰");
+            telemetry.addLine("Use DPAD to scroll, A to select");
+            telemetry.addLine();
+
+            for (int i = 0; i < modes.length; i++) {
+                if (i == modeIndex) {
+                    telemetry.addData(">>", modes[i].name());
+                } else {
+                    telemetry.addData("  ", modes[i].name());
+                }
+            }
+
+            telemetry.addLine();
+            telemetry.addLine("Current Settings:");
+            if (modes[modeIndex] == Mode.SERVO_TESTER) {
+                telemetry.addData("Target", SERVO_NAME);
+                telemetry.addData("Type", IS_CR_SERVO ? "Continuous" : "Positional");
+            } else if (modes[modeIndex] == Mode.MOTOR_TESTER) {
+                telemetry.addData("Target", MOTOR_NAME);
+                telemetry.addData("Mode", IS_MOTOR_EX ? "Velocity (Ex)" : "Power (Std)");
+            } else {
+                telemetry.addData("Left", ODO_LEFT_NAME);
+                telemetry.addData("Right", ODO_RIGHT_NAME);
+                telemetry.addData("Strafe", ODO_STRAFE_NAME);
+            }
+            telemetry.update();
+        }
+
+        if (isStopRequested()) return;
+
+        Mode selectedMode = modes[modeIndex];
+        Servo posServo = null;
+        CRServo crServo = null;
+        DcMotor simpleMotor = null;
+        DcMotorEx exMotor = null;
+        DcMotor leftOdo = null, rightOdo = null, strafeOdo = null;
+
+        try {
+            switch (selectedMode) {
+                case SERVO_TESTER:
+                    if (IS_CR_SERVO) {
+                        crServo = hardwareMap.get(CRServo.class, SERVO_NAME);
+                    } else {
+                        posServo = hardwareMap.get(Servo.class, SERVO_NAME);
+                    }
+                    break;
+                case MOTOR_TESTER:
+                    if (IS_MOTOR_EX) {
+                        exMotor = hardwareMap.get(DcMotorEx.class, MOTOR_NAME);
+                        exMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                        exMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                    } else {
+                        simpleMotor = hardwareMap.get(DcMotor.class, MOTOR_NAME);
+                        simpleMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                        simpleMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                    }
+                    break;
+                case ODOMETRY_TUNER:
+                    leftOdo = hardwareMap.get(DcMotor.class, ODO_LEFT_NAME);
+                    rightOdo = hardwareMap.get(DcMotor.class, ODO_RIGHT_NAME);
+                    strafeOdo = hardwareMap.get(DcMotor.class, ODO_STRAFE_NAME);
+
+                    leftOdo.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                    rightOdo.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                    strafeOdo.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+                    leftOdo.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                    rightOdo.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                    strafeOdo.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                    break;
+            }
+        } catch (Exception e) {
+            telemetry.addData("INIT ERROR", "Hardware not found. Check names.");
+            telemetry.update();
+            sleep(5000);
+            return;
+        }
+
+        waitForStart();
+
+        double servoPos = 0.5;
+        double velocityTarget = 0.0;
+        double velocityIncrement = 10.0;
+        boolean directionReverse = false;
+        boolean lastB = false;
+        boolean lastLeft = false, lastRight = false, lastUpVal = false, lastDownVal = false;
+
+        double prevLeft = 0, prevRight = 0, prevStrafe = 0;
+
+        while (opModeIsActive()) {
+            boolean currentB = gamepad1.b;
+            if (currentB && !lastB) {
+                directionReverse = !directionReverse;
+            }
+            lastB = currentB;
+
+            switch (selectedMode) {
+                case SERVO_TESTER:
+                    if (IS_CR_SERVO && crServo != null) {
+                        double power = gamepad1.right_trigger - gamepad1.left_trigger;
+                        if (directionReverse) power *= -1;
+                        crServo.setPower(power);
+                        telemetry.addData("Type", "CR Servo");
+                        telemetry.addData("Power", "%.2f", power);
+                        telemetry.addData("Direction", directionReverse ? "REVERSED" : "FORWARD");
+                    } else if (posServo != null) {
+                        boolean dUp = gamepad1.dpad_up;
+                        boolean dDown = gamepad1.dpad_down;
+
+                        if (dUp && !lastUpVal) servoPos = Math.min(1.0, servoPos + 0.01);
+                        if (dDown && !lastDownVal) servoPos = Math.max(0.0, servoPos - 0.01);
+
+                        lastUpVal = dUp;
+                        lastDownVal = dDown;
+
+                        double outputPos = servoPos;
+                        if (directionReverse) {
+                            posServo.setDirection(Servo.Direction.REVERSE);
+                        } else {
+                            posServo.setDirection(Servo.Direction.FORWARD);
+                        }
+
+                        posServo.setPosition(outputPos);
+                        telemetry.addData("Type", "Position Servo");
+                        telemetry.addData("Target Pos", "%.2f", servoPos);
+                        telemetry.addData("Direction", directionReverse ? "REVERSED" : "FORWARD");
+                    }
+                    break;
+
+                case MOTOR_TESTER:
+                    if (IS_MOTOR_EX && exMotor != null) {
+                        boolean dLeft = gamepad1.dpad_left;
+                        boolean dRight = gamepad1.dpad_right;
+                        boolean dUp = gamepad1.dpad_up;
+                        boolean dDown = gamepad1.dpad_down;
+
+                        if (dRight && !lastRight) {
+                            if (velocityIncrement == 0.1) velocityIncrement = 1;
+                            else if (velocityIncrement == 1) velocityIncrement = 10;
+                            else if (velocityIncrement == 10) velocityIncrement = 100;
+                        }
+                        if (dLeft && !lastLeft) {
+                            if (velocityIncrement == 100) velocityIncrement = 10;
+                            else if (velocityIncrement == 10) velocityIncrement = 1;
+                            else if (velocityIncrement == 1) velocityIncrement = 0.1;
+                        }
+
+                        if (dUp && !lastUpVal) velocityTarget += velocityIncrement;
+                        if (dDown && !lastDownVal) velocityTarget -= velocityIncrement;
+
+                        lastRight = dRight;
+                        lastLeft = dLeft;
+                        lastUpVal = dUp;
+                        lastDownVal = dDown;
+
+                        double finalVel = directionReverse ? -velocityTarget : velocityTarget;
+                        exMotor.setVelocity(finalVel);
+
+                        telemetry.addData("Type", "DcMotor Ex");
+                        telemetry.addData("Target Vel", "%.1f", finalVel);
+                        telemetry.addData("Actual Vel", "%.1f", exMotor.getVelocity());
+                        telemetry.addData("Position", exMotor.getCurrentPosition());
+                        telemetry.addData("Increment", velocityIncrement);
+                    } else if (simpleMotor != null) {
+                        double power = gamepad1.right_trigger - gamepad1.left_trigger;
+                        if (Math.abs(power) > 1.0) power = Math.signum(power);
+                        if (directionReverse) power *= -1;
+
+                        simpleMotor.setPower(power);
+
+                        telemetry.addData("Type", "DcMotor Standard");
+                        telemetry.addData("Power", "%.2f", power);
+                        telemetry.addData("Direction", directionReverse ? "REVERSED" : "FORWARD");
+                        telemetry.addData("Position", simpleMotor.getCurrentPosition());
+                    }
+                    break;
+
+                case ODOMETRY_TUNER:
+                    double rLeft = leftOdo.getCurrentPosition();
+                    double rRight = rightOdo.getCurrentPosition();
+                    double rStrafe = strafeOdo.getCurrentPosition();
+
+                    double vLeft = REVERSE_LEFT_ENCODER ? -rLeft : rLeft;
+                    double vRight = REVERSE_RIGHT_ENCODER ? -rRight : rRight;
+                    double vStrafe = REVERSE_STRAFE_ENCODER ? -rStrafe : rStrafe;
+
+                    double dLeft = vLeft - prevLeft;
+                    double dRight = vRight - prevRight;
+                    double dStrafe = vStrafe - prevStrafe;
+
+                    prevLeft = vLeft;
+                    prevRight = vRight;
+                    prevStrafe = vStrafe;
+
+                    telemetry.addLine("▰▰▰▰▰▰▰ READINGS ▰▰▰▰▰▰▰");
+                    telemetry.addData("L", "%.0f (Raw: %.0f)", vLeft, rLeft);
+                    telemetry.addData("R", "%.0f (Raw: %.0f)", vRight, rRight);
+                    telemetry.addData("S", "%.0f (Raw: %.0f)", vStrafe, rStrafe);
+
+                    telemetry.addLine("\n▰▰▰▰▰▰▰ DELTAS ▰▰▰▰▰▰▰");
+                    telemetry.addData("L_Delta", "%.0f", dLeft);
+                    telemetry.addData("R_Delta", "%.0f", dRight);
+                    telemetry.addData("S_Delta", "%.0f", dStrafe);
+
+                    telemetry.addLine("\n▰▰▰▰▰▰▰ ANALYSIS ▰▰▰▰▰▰▰");
+                    if (Math.abs(dLeft) > 5 && Math.abs(dRight) > 5) {
+                        if (Math.signum(dLeft) == Math.signum(dRight)) {
+                            telemetry.addData("Motion", "FORWARD/BACK");
+                            if (Math.abs(dStrafe) > Math.abs(dLeft) * 0.5) {
+                                telemetry.addData("WARNING", "Strafe Pod Leaking/Crooked");
+                            }
+                        } else {
+                            telemetry.addData("Motion", "TURNING");
+                        }
+                    } else if (Math.abs(dStrafe) > 5) {
+                        telemetry.addData("Motion", "STRAFING");
+                        if (Math.abs(dLeft) > 5 || Math.abs(dRight) > 5) {
+                            telemetry.addData("WARNING", "Drive Pods Leaking");
+                        }
+                    } else {
+                        telemetry.addData("Motion", "STATIONARY");
+                    }
+                    break;
+            }
+            telemetry.update();
+        }
+    }
+}

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

@@ -0,0 +1,225 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+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.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+
+@Configurable
+@TeleOp(name = "Turret Motor Tuning", group = "Turret")
+public class TurretRotationTuning extends LinearOpMode {
+
+    // --- PID GAINS ---
+    public static double kP = 0.02;
+    public static double kI = 0;
+    public static double kD = 0.001;
+
+    public static double MAX_POWER = 0.5;
+    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 boolean MOTOR_REVERSED = true;
+
+    // --- CALIBRATION SETTINGS ---
+    public static boolean ENABLE_AUTO_RATIO = false;
+    public static double CALIBRATION_SWEEP_DEGREES = 180.0;
+
+    // --- TARGETING SETTINGS ---
+    public static double TARGET_OFFSET_DEGREES = -2;
+    public static double SOFT_LIMIT_NEG = -230;
+    public static double SOFT_LIMIT_POS = 230;
+    public static double WRAP_THRESHOLD_DEGREES = 180.0;
+
+    public static int targetFPS = 140;
+
+    private Limelight3A limelight;
+    private DcMotorEx turretMotor;
+    private TelemetryManager telemetryM;
+
+    private double integralSum = 0;
+    private double lastError = 0;
+
+    private double zeroOffsetTicks = 0;
+    private double startSweepTicks = 0;
+    private boolean isCalibratingSweep = false;
+    private boolean lastReverseState = false;
+
+    private double targetTicks = 0;
+    private double ticksPerDegree = 0;
+
+    private enum TuningMode {
+        CALIBRATION,
+        PID_ACTIVE
+    }
+
+    private TuningMode currentMode = TuningMode.PID_ACTIVE;
+    private final ElapsedTime timer = new ElapsedTime();
+    FPSCounter fps = new FPSCounter();
+
+    @Override
+    public void runOpMode() {
+        
+        telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+        turretMotor = hardwareMap.get(DcMotorEx.class, "turretRotation");
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+        turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        turretMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        turretMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+        applyMotorDirection();
+
+        // Initial calculation based on provided static ratio
+        ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
+
+        limelight.pipelineSwitch(1);
+        limelight.start();
+
+        telemetryM.debug("Status", "Initialized");
+        telemetryM.update(telemetry);
+
+        waitForStart();
+        timer.reset();
+
+        while (opModeIsActive()) {
+            fps.startLoop();
+            double dt = timer.seconds();
+            timer.reset();
+            if (dt < 0.001) dt = 0.001;
+
+            if (MOTOR_REVERSED != lastReverseState) {
+                applyMotorDirection();
+            }
+
+            double rawTicks = turretMotor.getCurrentPosition();
+            double currentTicks = rawTicks - zeroOffsetTicks;
+            double currentDegrees = currentTicks / ticksPerDegree;
+
+            switch (currentMode) {
+                case CALIBRATION:
+                    handleCalibrationMode(rawTicks);
+                    targetTicks = currentTicks; // Keep target at current position to avoid jumps
+                    break;
+
+                case PID_ACTIVE:
+                    handlePidMode(currentTicks, dt);
+                    break;
+            }
+
+            sleep(fps.getSyncTime(targetFPS));
+            
+            // --- TELEMETRY ---
+            telemetryM.debug("▰▰▰▰▰ MODE ▰▰▰▰▰", currentMode.toString());
+            telemetryM.debug("Gear Ratio", EXTERNAL_GEAR_RATIO);
+            telemetryM.debug("Ticks/Deg", ticksPerDegree);
+            telemetryM.debug("Adj Ticks", currentTicks);
+            telemetryM.debug("Degrees", currentDegrees);
+            telemetryM.debug("Target Deg", targetTicks / ticksPerDegree);
+            telemetry.addData("FPS", fps.getAverageFps());
+            telemetryM.update(telemetry);
+        }
+        limelight.stop();
+    }
+
+    private void handleCalibrationMode(double rawTicks) {
+        // Slow manual control for alignment
+        double manualPower = -gamepad1.left_stick_y * 0.4;
+        turretMotor.setPower(manualPower);
+
+        // 1. Reset Local Zero
+        if (gamepad1.x) {
+            zeroOffsetTicks = rawTicks;
+            telemetryM.debug("ACTION", "Zero Point Reset!");
+        }
+
+        // 2. Start 180 Degree Sweep
+        if (gamepad1.y && ENABLE_AUTO_RATIO && !isCalibratingSweep) {
+            startSweepTicks = rawTicks;
+            isCalibratingSweep = true;
+        }
+
+        // 3. End Sweep and Calculate Ratio
+        if (gamepad1.b && isCalibratingSweep) {
+            double deltaTicks = Math.abs(rawTicks - startSweepTicks);
+            double measuredTicksPerDegree = deltaTicks / CALIBRATION_SWEEP_DEGREES;
+            EXTERNAL_GEAR_RATIO = (measuredTicksPerDegree * 360.0) / TICKS_PER_REV_MOTOR;
+            
+            ticksPerDegree = measuredTicksPerDegree;
+            isCalibratingSweep = false;
+            telemetryM.debug("ACTION", "New Ratio Calculated!");
+        }
+
+        // 4. Switch back to PID
+        if (gamepad1.a) {
+            currentMode = TuningMode.PID_ACTIVE;
+        }
+
+        telemetryM.debug("STATUS", isCalibratingSweep ? "RECORDING SWEEP..." : "IDLE");
+        telemetryM.debug("INSTRUCTIONS", "LS: Move | X: Zero | Y: Start Sweep | B: Stop Sweep | A: Done");
+    }
+
+    private void handlePidMode(double currentTicks, double dt) {
+        // Emergency stop/switch to manual
+        if (gamepad1.back || (gamepad1.b && !isCalibratingSweep)) {
+            turretMotor.setPower(0);
+            currentMode = TuningMode.CALIBRATION;
+            return;
+        }
+
+        double wrapLimitTicks = WRAP_THRESHOLD_DEGREES * ticksPerDegree;
+        LLResult result = limelight.getLatestResult();
+
+        if (result != null && result.isValid()) {
+            double tx = result.getTx();
+            double offsetTicks = (tx + TARGET_OFFSET_DEGREES) * ticksPerDegree;
+            double potentialTarget = currentTicks + offsetTicks;
+
+            // Anti-Wrap and Soft Limit Logic
+            if (potentialTarget < -wrapLimitTicks) {
+                targetTicks = SOFT_LIMIT_POS;
+            } else if (potentialTarget > wrapLimitTicks) {
+                targetTicks = SOFT_LIMIT_NEG;
+            } else {
+                targetTicks = Math.max(SOFT_LIMIT_NEG * ticksPerDegree, 
+                             Math.min(SOFT_LIMIT_POS * ticksPerDegree, potentialTarget));
+            }
+        }
+
+        double error = targetTicks - 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 = Math.max(-MAX_POWER, Math.min(MAX_POWER, output));
+
+        turretMotor.setPower(output);
+        lastError = error;
+    }
+
+    private void applyMotorDirection() {
+        turretMotor.setDirection(MOTOR_REVERSED ? 
+            DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+        lastReverseState = MOTOR_REVERSED;
+    }
+}

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

@@ -0,0 +1,465 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import android.graphics.Color;
+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.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.SwitchableLight;
+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 = "HOLY GODFATHER OF TESTING", group = "All")
+public class allInOne 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 kP = 0.035; // Default to ODO roughly
+    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;
+
+    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 = 4;
+    public static double BLUE_TARGET_OFFSET_DEGREES = 4;
+
+    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;
+
+    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;
+
+    public static double SERVO_MIN = 0.48;
+    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 DcMotor frontLeft, frontRight, backLeft, backRight;
+    private Follower follower;
+
+    public DcMotor frontIntakeMotor;
+    private NormalizedColorSensor indexA, indexB, indexC;
+    private NormalizedColorSensor[] sensors;
+
+    private TelemetryManager telemetryM;
+
+    private double integralSum = 0;
+    private double lastError = 0;
+    private double targetAngle = 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 long lastIndexAllArtifactsTime = 0;
+    private int[] artifactColors = {0, 0, 0};
+
+    List<LynxModule> allHubs;
+
+    private enum Alliance { RED, BLUE }
+    private Alliance currentAlliance = Alliance.BLUE;
+
+    // Tracking State
+    private enum TrackingSource { LIMELIGHT, ODOMETRY }
+    private TrackingSource activeSource = TrackingSource.ODOMETRY;
+    private TrackingSource lastActiveSource = TrackingSource.ODOMETRY;
+
+    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");
+
+        frontLeft = hardwareMap.get(DcMotor.class, "fl");
+        frontRight = hardwareMap.get(DcMotor.class, "fr");
+        backLeft = hardwareMap.get(DcMotor.class, "bl");
+        backRight = hardwareMap.get(DcMotor.class, "br");
+
+        frontIntakeMotor = hardwareMap.dcMotor.get("intake");
+        frontIntakeMotor.setDirection(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_DIRECTION);
+        frontIntakeMotor.setZeroPowerBehavior(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_ZERO_POWER_BEHAVIOR);
+        frontIntakeMotor.setMode(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_RUNMODE);
+
+        // --- Sensor Setup ---
+        indexA = hardwareMap.get(NormalizedColorSensor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.TransferConstants.INDEX_SENSOR_A);
+        indexB = hardwareMap.get(NormalizedColorSensor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.TransferConstants.INDEX_SENSOR_B);
+        indexC = hardwareMap.get(NormalizedColorSensor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.TransferConstants.INDEX_SENSOR_C);
+        sensors = new NormalizedColorSensor[]{indexA, indexB, indexC};
+
+        for (NormalizedColorSensor s : sensors) {
+            if (s instanceof SwitchableLight) ((SwitchableLight) s).enableLight(true);
+        }
+
+        // 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);
+
+        frontLeft.setDirection(DcMotor.Direction.REVERSE);
+        backLeft.setDirection(DcMotor.Direction.REVERSE);
+
+        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;
+
+        // Drive Control
+        double y = -gamepad1.left_stick_y;
+        double x = gamepad1.left_stick_x * 1.1;
+        double rx = gamepad1.right_stick_x;
+        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+
+        frontLeft.setPower((y + x + rx) / denominator);
+        backLeft.setPower((y - x + rx) / denominator);
+        frontRight.setPower((y - x - rx) / denominator);
+        backRight.setPower((y + x - rx) / denominator);
+
+        // Pass movement to follower for pose updates
+        follower.setTeleOpDrive(y, x, rx, true);
+
+        // Alliance Toggle
+        if (gamepad1.psWasPressed()) {
+            currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
+        }
+
+        if (gamepad1.startWasPressed()) {
+            pipelineIndex = (pipelineIndex + 1) % 3;
+            limelight.pipelineSwitch(pipelineIndex);
+        }
+
+        if (gamepad1.yWasPressed()) launch = !launch;
+
+        // Intake Logic
+        if (gamepad1.right_trigger > 0.01) {
+            frontIntakeMotor.setPower(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_POWER);
+        } else if (gamepad1.left_trigger > 0.01) {
+            frontIntakeMotor.setPower(-org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_POWER);
+        } else {
+            if (!launch) frontIntakeMotor.setPower(0);
+        }
+
+        // Color Sensor Logic
+        long currentTime = System.currentTimeMillis();
+        if (currentTime - lastIndexAllArtifactsTime >= 350) {
+            detectArtifacts();
+            lastIndexAllArtifactsTime = currentTime;
+        }
+
+        // --- TURRET TRACKING LOGIC ---
+
+        // Update Pipeline based on Alliance
+        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: Default to LL, fall back to ODO
+        calculatedTargetTicks = calculateLimelightTargetTicks(result, currentDegrees, rawTicks);
+
+        if (Double.isNaN(calculatedTargetTicks)) {
+            // Limelight failed/no target
+            if (limelightFailureTimer.seconds() > 0.5) {
+                // If failed for > 0.5s, use ODO
+                calculatedTargetTicks = calculateOdometryTargetTicks();
+                activeSource = TrackingSource.ODOMETRY;
+            } else {
+                // Hold last valid position (short flicker)
+                calculatedTargetTicks = targetTicks;
+            }
+        } else {
+            // Limelight valid
+            limelightFailureTimer.reset();
+            activeSource = TrackingSource.LIMELIGHT;
+        }
+
+        // Apply Target
+        if (!Double.isNaN(calculatedTargetTicks)) {
+            targetTicks = calculatedTargetTicks;
+        }
+
+        // PID Reset on Source Switch
+        if (activeSource != lastActiveSource) {
+            integralSum = 0;
+            lastError = 0;
+        }
+        lastActiveSource = activeSource;
+
+        // PID Calculation
+        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 ---
+        // Uses raw LL result if valid, regardless of whether it's controlling the turret
+        double calculatedDistance = 0;
+        double calculatedVelocity = 0;
+        double calculatedHoodPos = 0;
+
+        if (result != null && result.isValid()) {
+            double ta = result.getTa();
+            calculatedDistance = getDistanceFromTag(ta);
+            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) {
+                frontIntakeMotor.setPower(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.LAUNCH_POWER);
+            }
+        }
+
+        // Telemetry
+        telemetryM.debug("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
+        telemetryM.debug("Alliance", currentAlliance);
+        telemetryM.debug("Source", activeSource);
+        telemetryM.debug("Detected", getArtifactString());
+        telemetryM.debug("Turret Angle", currentDegrees);
+        telemetryM.debug("Target Angle", targetTicks / ticksPerDegree);
+        telemetryM.debug("Hood Target", calculatedHoodPos);
+        telemetryM.debug("Distance", calculatedDistance);
+
+        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();
+
+            // Logic Multiplier handles coordinate system differences without switching motor direction
+            double calculatedTargetAngle = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
+
+            TARGET_OFFSET_DEGREES = LL_TARGET_OFFSET_DEGREES;
+            double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
+            double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
+
+            // Teleport Wrap Logic
+            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 void detectArtifacts() {
+        for (int i = 0; i < 3; i++) {
+            NormalizedRGBA c1 = sensors[i].getNormalizedColors();
+            float[] hsv = new float[3];
+            Color.colorToHSV(c1.toColor(), hsv);
+            int color1 = detectColor(hsv[0], hsv[1], hsv[2]);
+            artifactColors[i] = color1;
+        }
+    }
+
+    private int detectColor(float hue, float sat, float val) {
+        if (hue > 170 && hue < 310) return 1; // Purple
+        if (hue > 80 && hue < 170) return 2;  // Green
+        if (sat < 0.1 || val < 0.1) return 0;
+        return 0;
+    }
+
+    private String getArtifactString() {
+        StringBuilder sb = new StringBuilder();
+        for (int i : artifactColors) {
+            sb.append(i == 2 ? "G " : (i == 1 ? "P " : "- "));
+        }
+        return sb.toString();
+    }
+
+    private double getDistanceFromTag(double x) {
+        return (74.34095 * Math.pow(x, -0.518935));
+    }
+
+    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/anchorTuning.java

@@ -0,0 +1,86 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
+
+@Configurable
+@TeleOp(name = "Anchor Mode Tuner", group = "Tuning")
+public class anchorTuning extends LinearOpMode {
+
+    public static double driveP = -0.001;
+    public static double driveI = 0.0;
+    public static double driveD = -0.00015;
+
+    public static double strafeP = -0.002;
+    public static double strafeI = 0.0;
+    public static double strafeD = -0.00008;
+
+    public static double turnP = -0.08;
+    public static double turnI = 0.0;
+    public static double turnD = -0.0001; // Tuned for Degrees
+
+
+    public static double MAX_POWER = 0.2;
+
+
+    private DriveTrain driveTrain;
+    private TelemetryManager telemetryM;
+    private boolean lastPS = false;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        // Initialize Telemetry
+        telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+
+        // Initialize Hardware (This now includes the IMU from hwMap)
+        hwMap.DriveHwMap driveHw = new hwMap.DriveHwMap(hardwareMap);
+        driveTrain = new DriveTrain(driveHw);
+
+        telemetryM.debug("Status", "Initialized. Press PS/Guide to Toggle Anchor.");
+        telemetryM.update(telemetry);
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            // Update PID values from Dashboard to Subsystem in real-time
+            driveTrain.updateAnchorPID(
+                    driveP, driveI, driveD,
+                    strafeP, strafeI, strafeD,
+                    turnP, turnI, turnD,
+                    MAX_POWER
+            );
+
+            // Toggle Anchor with PS Button
+            if (gamepad1.ps && !lastPS) {
+                if (driveTrain.isAnchorActive()) {
+                    driveTrain.stopAnchor();
+                    gamepad1.rumble(200);
+                } else {
+                    driveTrain.startAnchor();
+                    gamepad1.rumble(500);
+                }
+            }
+            lastPS = gamepad1.ps;
+
+            driveTrain.update();
+
+            if (!driveTrain.isAnchorActive()) {
+                driveTrain.teleopDrive(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.right_stick_x);
+            }
+
+            telemetryM.debug("Anchor Active", driveTrain.isAnchorActive());
+
+            telemetryM.debug("Error FWD (Ticks)", driveTrain.getLastForwardError());
+            telemetryM.debug("Error STR (Ticks)", driveTrain.getLastStrafeError());
+            telemetryM.debug("Error TRN (Degs)", driveTrain.getLastTurnError());
+
+            telemetryM.update(telemetry);
+        }
+    }
+}

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

@@ -0,0 +1,130 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.telemetry.TelemetryManager;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+
+import com.bylazar.telemetry.PanelsTelemetry;
+
+
+@TeleOp(name = "Encoders Test", group = "Test")
+public class encoderTest extends OpMode {
+
+    // Define Motor Objects
+    private DcMotor leftPodMotor;
+    private DcMotor rightPodMotor;
+    private DcMotor strafePodMotor;
+
+    // Previous values for calculating delta (speed)
+    private double prevLeft = 0;
+    private double prevRight = 0;
+    private double prevStrafe = 0;
+
+    private TelemetryManager telemetryM;
+
+    @Override
+    public void init() {
+
+        telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+        try {
+            leftPodMotor = hardwareMap.get(DcMotor.class, "fl");
+        } catch (Exception e) {
+            telemetryM.debug("Error", "Could not find 'frontLeft' in config");
+        }
+
+        try {
+            rightPodMotor = hardwareMap.get(DcMotor.class, "fr");
+        } catch (Exception e) {
+            telemetryM.debug("Error", "Could not find 'backRight' in config");
+        }
+
+        try {
+            strafePodMotor = hardwareMap.get(DcMotor.class, "br");
+        } catch (Exception e) {
+            telemetryM.debug("Error", "Could not find 'backLeft' in config");
+        }
+
+        if (leftPodMotor != null) {
+            leftPodMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            leftPodMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        }
+        if (rightPodMotor != null) {
+            rightPodMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            rightPodMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        }
+        if (strafePodMotor != null) {
+            strafePodMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+            strafePodMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        }
+
+        telemetryM.debug("Status", "Initialized");
+        telemetryM.debug("Note", "Manually move robot to check directions");
+        telemetryM.update(telemetry);
+    }
+
+    @Override
+    public void loop() {
+        // Read current positions (Safeguard against null if config is wrong)
+        double rawLeft = (leftPodMotor != null) ? leftPodMotor.getCurrentPosition() : 0;
+        double rawRight = (rightPodMotor != null) ? rightPodMotor.getCurrentPosition() : 0;
+        double rawStrafe = (strafePodMotor != null) ? strafePodMotor.getCurrentPosition() : 0;
+
+        // Apply the negation logic found in your DriveTrain file
+        // Your Anchor code used: -currentPosition
+        double anchorLeft = -rawLeft;
+        double anchorRight = -rawRight;
+        double anchorStrafe = -rawStrafe;
+
+        // Calculate Deltas (Movement since last loop)
+        double deltaLeft = anchorLeft - prevLeft;
+        double deltaRight = anchorRight - prevRight;
+        double deltaStrafe = anchorStrafe - prevStrafe;
+
+        // Update Previous
+        prevLeft = anchorLeft;
+        prevRight = anchorRight;
+        prevStrafe = anchorStrafe;
+
+        // ----------------------------------
+        // TELEMETRY DISPLAY
+        // ----------------------------------
+        telemetryM.debug("--- RAW ENCODER TICKS ---");
+        telemetryM.debug("Left Pod (FL Port)", "%.0f", rawLeft);
+        telemetryM.debug("Right Pod (BR Port)", "%.0f", rawRight);
+        telemetryM.debug("Strafe Pod (BL Port)", "%.0f", rawStrafe);
+
+        telemetryM.debug("\n--- ANCHOR LOGIC (Negated) ---");
+        telemetryM.debug("Left", "%.0f", anchorLeft);
+        telemetryM.debug("Right", "%.0f", anchorRight);
+        telemetryM.debug("Strafe", "%.0f", anchorStrafe);
+
+        telemetry.addLine("\n--- DIAGNOSTICS ---");
+
+        // Check Forward Movement
+        if (Math.abs(deltaLeft) > 5 && Math.abs(deltaRight) > 5) {
+            if (Math.signum(deltaLeft) == Math.signum(deltaRight)) {
+                telemetryM.debug("Motion", "FORWARD / BACKWARD");
+                if (Math.abs(anchorStrafe) > 1000) {
+                    telemetryM.debug("WARNING: Strafe pod is increasing while moving forward.");
+                    telemetryM.debug("Check if Strafe pod is crooked.");
+                }
+            } else {
+                telemetryM.debug("Motion", "TURNING");
+            }
+        }
+        // Check Strafe Movement
+        else if (Math.abs(deltaStrafe) > 5) {
+            telemetryM.debug("Motion", "STRAFING");
+            if (Math.abs(deltaLeft) < 5 && Math.abs(deltaRight) < 5) {
+                telemetryM.debug("GOOD: Left/Right pods stationary during strafe.");
+            }
+        } else {
+            telemetryM.debug("Motion", "STATIONARY");
+        }
+
+        telemetryM.update(telemetry);
+    }
+}

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

@@ -0,0 +1,370 @@
+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/odoTracking.java

@@ -0,0 +1,302 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.configurables.annotations.Configurable;
+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.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 = "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 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;
+
+    // ================= Limits & Logic =================
+    public static double LL_LOGIC_MULTIPLIER = 1.0;
+    public static double SOFT_LIMIT_NEG = -230;
+    public static double SOFT_LIMIT_POS = 230;
+
+    // ================= 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;
+
+    // ================= 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;
+
+    // ================= Sensor Fusion Settings =================
+    public static double OFFSET_SMOOTHING = 0.2;
+
+    public static int targetFPS = 100;
+
+    // ================= Hardware Objects =================
+    private Limelight3A limelight;
+    private DcMotorEx turretMotor;
+    private Follower follower;
+
+    // ================= State Variables =================
+    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 TuningMode { CALIBRATION, TRACKING }
+    private TuningMode currentMode = TuningMode.CALIBRATION;
+
+    private enum Alliance { RED, BLUE }
+    private Alliance currentAlliance = Alliance.BLUE;
+    private Alliance lastAlliance = null;
+
+    private final ElapsedTime timer = new ElapsedTime();
+    protected FPSCounter fpsCounter = new FPSCounter();
+
+    public static double start_x = 72;
+    public static double start_y = 8.5;
+    public static double start_heading = 90;
+
+
+    protected List<LynxModule> allHubs;
+
+    @Override
+    public void runOpMode() {
+        turretMotor = hardwareMap.get(DcMotorEx.class, "turretRotation");
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+        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);
+
+        allHubs = hardwareMap.getAll(LynxModule.class);
+        for (LynxModule hub : allHubs) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+        }
+
+        applyMotorDirection();
+        recalculateTicksPerDegree();
+
+        limelight.pipelineSwitch(1);
+        limelight.start();
+
+        telemetry.addData("Status", "Initialized");
+        telemetry.update();
+
+        waitForStart();
+        timer.reset();
+
+        follower.startTeleopDrive(true);
+        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;
+
+            handleInput();
+            updateAlliancePipeline();
+
+            double rawTicks = turretMotor.getCurrentPosition();
+            double currentTicks = rawTicks - zeroOffsetTicks;
+            double currentDegrees = currentTicks / ticksPerDegree;
+
+            switch (currentMode) {
+                case CALIBRATION:
+                    handleCalibrationMode(rawTicks);
+                    targetCorrectionOffsetTicks = 0;
+                    fusedTargetTicks = currentTicks;
+                    break;
+
+                case TRACKING:
+                    calculateHybridTarget(currentTicks, currentDegrees);
+                    runPidControl(currentTicks, dt);
+
+                    follower.setTeleOpDrive(
+                            -gamepad1.left_stick_y,
+                            -gamepad1.left_stick_x,
+                            -gamepad1.right_stick_x,
+                            true
+                    );
+                    break;
+            }
+
+            sleep(fpsCounter.getSyncTime(targetFPS));
+            updateTelemetry(currentTicks, currentDegrees);
+        }
+        limelight.stop();
+    }
+
+    private void calculateHybridTarget(double currentTicks, double currentDegrees) {
+        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);
+    }
+
+    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 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 void handleInput() {
+        if (gamepad1.x) {
+            fusedTargetTicks = 0;
+            return;
+        }
+
+        if (gamepad1.psWasPressed()) {
+            currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
+        }
+
+        if (gamepad1.aWasPressed() && currentMode == TuningMode.CALIBRATION) {
+            currentMode = TuningMode.TRACKING;
+        } else if (gamepad1.bWasPressed() && currentMode == TuningMode.TRACKING) {
+            turretMotor.setPower(0);
+            currentMode = TuningMode.CALIBRATION;
+        }
+    }
+
+    private void updateAlliancePipeline() {
+        if (lastAlliance != currentAlliance) {
+            if (currentAlliance == Alliance.RED) {
+                limelight.pipelineSwitch(2);
+            } else {
+                limelight.pipelineSwitch(1);
+            }
+            lastAlliance = currentAlliance;
+        }
+    }
+
+    private void handleCalibrationMode(double rawTicks) {
+        double manualPower = -gamepad1.left_stick_y * 0.3;
+        turretMotor.setPower(manualPower);
+
+    }
+
+    private void updateTelemetry(double currentTicks, double currentDegrees) {
+        telemetry.addData("Mode", currentMode);
+        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("Applied Offset (Ticks)", targetCorrectionOffsetTicks);
+        telemetry.addData("Applied Offset (Deg)", targetCorrectionOffsetTicks / ticksPerDegree);
+        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/util/AutoTransfer.java

@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.pedropathing.geometry.Pose;
+
+public class AutoTransfer {
+
+    public static Pose transferPose = new Pose(0, 0, 0);
+
+    public static int alliance = 0;
+
+    public static int motifPattern = 0;
+
+    public static boolean isAutonRan = false;
+
+    public static void reset() {
+        transferPose = new Pose(0, 0, 0);
+        alliance = 0;
+        motifPattern = 0;
+        isAutonRan = false;
+    }
+
+    public static void initAuton(int allianceColor) {
+        reset();
+        alliance = allianceColor;
+        isAutonRan = true;
+    }
+
+    public static void setMotifPattern(int pattern) {
+        motifPattern = pattern;
+    }
+
+    public static void updatePose(Pose pose) {
+        transferPose = pose;
+    }
+
+    public static String getAllianceString() {
+        switch (alliance) {
+            case 1: return "BLUE";
+            case 2: return "RED";
+            default: return "UNKNOWN";
+        }
+    }
+    public static String getMotifString() {
+        switch (motifPattern) {
+            case 1: return "Pattern 1 (GPP)";
+            case 2: return "Pattern 2 (PGP)";
+            case 3: return "Pattern 3 (PPG)";
+            default: return "UNKNOWN";
+        }
+    }
+
+    public static boolean hasValidData() {
+        return isAutonRan && alliance != 0;
+    }
+}

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

@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class FPSCounter {
+    private ElapsedTime loopTimer = new ElapsedTime();
+    private double loopTimeSum = 0;
+    private int loopCount = 0;
+    private double currentLoopTime = 0;
+    private double lastTime = 0;
+
+    public FPSCounter() {
+        reset();
+    }
+
+    public void startLoop() {
+        double now = loopTimer.milliseconds();
+        if (loopCount > 0) {
+            currentLoopTime = now - lastTime;
+            loopTimeSum += currentLoopTime;
+        }
+        lastTime = now;
+        loopCount++;
+    }
+
+    public long getSyncTime(int targetFps) {
+        double targetMs = 1000.0 / targetFps;
+
+        double elapsedWorkMs = loopTimer.milliseconds() - lastTime;
+        double remainingMs = targetMs - elapsedWorkMs;
+
+        return (remainingMs > 0) ? (long) remainingMs : 0;
+    }
+
+    public double getCurrentFps() {
+        return (currentLoopTime > 0) ? 1000.0 / currentLoopTime : 0;
+    }
+
+    public double getAverageFps() {
+        if (loopCount <= 1) return 0;
+        return 1000.0 / (loopTimeSum / (loopCount - 1));
+    }
+
+    public double getCurrentLoopTime() {
+        return currentLoopTime;
+    }
+
+    public double getAverageLoopTime() {
+        if (loopCount <= 1) return 0;
+        return loopTimeSum / (loopCount - 1);
+    }
+
+    public void reset() {
+        loopTimer.reset();
+        lastTime = 0;
+        loopTimeSum = 0;
+        loopCount = 0;
+        currentLoopTime = 0;
+    }
+}

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

@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.bylazar.field.FieldManager;
+import com.bylazar.field.PanelsField;
+import com.bylazar.field.Style;
+import com.pedropathing.geometry.*;
+import com.pedropathing.math.*;
+import com.pedropathing.paths.*;
+
+public class OATDrawing {
+    public static final double ROBOT_RADIUS = 9;
+    private static final FieldManager panelsField = PanelsField.INSTANCE.getField();
+
+    private static final Style robotLook = new Style("", "#3F51B5", 0.75);
+
+    private static final Style goalLook = new Style("", "#F44336", 0.9);
+
+    private static final Style vectorLook = new Style("", "#4CAF50", 0.8);
+
+    private static final Style visionLook = new Style("", "#FFD700", 0.9);
+
+    public static void init() {
+        panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING());
+    }
+    public static void drawOATState(Pose robotPose, Pose goalPose, boolean hasVisionLock) {
+        if (robotPose == null || goalPose == null) return;
+
+        drawRobot(robotPose, robotLook);
+
+        panelsField.setStyle(goalLook);
+        panelsField.moveCursor(goalPose.getX(), goalPose.getY());
+        panelsField.circle(4);
+
+        panelsField.setStyle(hasVisionLock ? visionLook : vectorLook);
+        panelsField.moveCursor(robotPose.getX(), robotPose.getY());
+        panelsField.line(goalPose.getX(), goalPose.getY());
+
+        panelsField.update();
+    }
+
+    private static void drawRobot(Pose pose, Style style) {
+        if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) {
+            return;
+        }
+
+        panelsField.setStyle(style);
+        panelsField.moveCursor(pose.getX(), pose.getY());
+        panelsField.circle(ROBOT_RADIUS);
+
+        Vector v = pose.getHeadingAsUnitVector();
+        v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);
+        double x2 = pose.getX() + v.getXComponent();
+        double y2 = pose.getY() + v.getYComponent();
+
+        panelsField.moveCursor(pose.getX(), pose.getY());
+        panelsField.line(x2, y2);
+    }
+}