Update README.md

README.md

@@ -8,15 +8,18 @@
 
 ## 🚀 Key Features
 
-*   **Template Method Pattern:** A locked-down backend that handles the "boring stuff" (Bulk reads, FPS capping, Telemetry timing) so you only write robot logic.
-*   **Subsystem Architecture:** Fully isolated mechanisms with their own internal "Micro-States."
-*   **Hierarchical State Machines:** Orchestrate complex robot actions by mapping a "Global State" to specific "Subsystem States."
-*   **Automatic Hardware Optimization:** 
-    *   **Bulk Reads:** Automatically sets all expansion hubs to Manual Caching mode for the fastest possible loop times.
-    *   **FPS Capping:** Prevents CPU/Battery waste by locking loop speeds to a target (e.g., 50 FPS).
-*   **Stateful PID Control:** A built-in PID utility that handles time-deltas ($dt$) and integral sums internally.
-*   **Unified Telemetry:** A joined engine that pipes data to both the Driver Station and the **Panels** dashboard simultaneously.
-*   **Live Tuning:** Centralized `Constants.java` utilizing `@Configurable` for real-time value editing without recompiling.
+*   **Template Method Backend:** A locked-down engine that handles hardware synchronization, bulk reads, and FPS capping automatically.
+*   **Write-Caching Hardware (`CMotor` & `CServo`):** Optimized wrappers that eliminate redundant hardware writes, drastically reducing loop times (often 200+ FPS).
+*   **Universal PIDF Engine:** A comprehensive feedback hierarchy (P, PD, PID, PIDF) featuring:
+    *   **Voltage Compensation:** Consistent power output across the entire battery range.
+    *   **Low-Pass Filtering:** Smooths noisy encoder data for jitter-free movement.
+    *   **Anti-Windup:** Prevents integral "explosion" during physical stalls.
+    *   **Physics Feedforward:** Built-in models for Gravity ($kG$), Arm-Cosine ($kCos$), and Static Friction ($kS$).
+*   **Routines & Action Sequencer:** A non-blocking script engine to run complex macros (e.g., Auto-Score) in the background while the driver retains control.
+*   **Enhanced Gamepad:** Built-in rising/falling edge detection (`aWasPressed()`) and Cubic Input Scaling for high-precision driving.
+*   **Limelight MegaTag2 Pose Healing:** Periodically "heals" PedroPathing odometry drift using global AprilTag localization.
+*   **Trapezoidal Motion Profiling:** Smooths out acceleration and deceleration for heavy mechanisms like lifts and arms.
+*   **Hardware Health Monitor:** Performs safe initialization and reports hardware failures via Panels before the match starts.
 
 ---
 
@@ -24,82 +27,79 @@
 
 ```text
 teamcode/
-├── lib/                   # The Core Framework (Don't touch)
-│   ├── BaseStateOpMode.java   # The engine that runs the robot
-│   ├── Subsystem.java         # The blueprint for all mechanisms
-│   ├── SubsystemManager.java  # Automates the lifecycle of subsystems
-│   └── PIDController.java     # Stateful math utility
-├── subsystems/            # Your robot parts (Drivetrain, Lift, Intake)
-├── util/                  # Utilities (AutoTransfer, FPSCounter)
-├── Constants.java         # The "Control Panel" for the entire robot
-└── opmodes/               # Your actual TeleOp and Autonomous files
+├── lib/                   
+│   ├── actions/           # Routine & Action Sequencer
+│   ├── hardware/          # CMotor, CServo, EnhancedGamepad
+│   ├── pid/               # Universal PIDF Controller Hierarchy
+│   ├── util/              # LLUtil, BaseOpMode, SubsysManager
+│   └── Subsystem.java     # Base Subsystem template
+├── subsys/                # Robot-specific mechanisms (Drivetrain, etc.)
+├── util/                  # AutoTransfer, FPSCounter
+├── Constants.java         # Centralized @Configurable panel
+└── opmodes/               # TeleOp and Autonomous files
 ```
 
 ---
 
 ## 🛠 Usage Guide
 
-### 1. Define your Constants
-Use the `Constants.java` file to store every hardware name, PID value, and speed multiplier.
+### 1. Enhanced Gamepad & States
+The backend automatically updates `g1` and `g2`. Use edge detection for clean state transitions.
 
 ```java
-@Configurable
-public static class LIFT {
-    public static double kP = 0.015;
-    public static int SCORING_POS = 2500;
-}
-```
+@Override
+protected void stateMachineUpdate() {
+    // Stick inputs are automatically cubic-scaled and deadbanded
+    drive.setDriveVectors(-g1.left_stick_y(), g1.left_stick_x(), g1.right_stick_x());
 
-### 2. Create a Subsystem
-Inherit from `Subsystem`. Define "Micro-States" for this specific mechanism.
-
-```java
-public class LiftSubsystem extends Subsystem {
-    public enum LiftState { IDLE, EXTENDING }
-    private LiftState state = LiftState.IDLE;
-
-    @Override
-    public void update() {
-        // Run PID logic here
+    if (g1.aWasPressed()) {
+        robotState = GlobalState.SCORING;
     }
 }
 ```
 
-### 3. Build your TeleOp
-Inherit from `BaseStateOpMode`. This gives you the `stateMachineUpdate()` hook where you map gamepad inputs to states.
+### 2. Writing a Routine
+Routines allow you to script the robot without using `sleep()`.
 
 ```java
-@TeleOp
-public class MainTeleOp extends BaseStateOpMode {
-    
-    @Override
-    protected void setupSubsystems() {
-        manager.register(new DriveSubsystem(), new LiftSubsystem());
-    }
+routines.run(
+    Routine.sequence(
+        Routine.instant(() -> lift.setTarget(3000)),
+        Routine.waitUntil(() -> lift.atSetpoint()),
+        Routine.instant(() -> claw.open()),
+        Routine.wait(250),
+        Routine.instant(() -> lift.setTarget(0))
+    )
+);
+```
 
-    @Override
-    protected void stateMachineUpdate() {
-        if (gamepad1.aWasPressed()) {
-            robotState = GlobalState.SCORING;
-        }
-    }
+### 3. Smart Hardware
+Use `CMotor` and `CServo` in your subsystems to save several milliseconds per loop.
+
+```java
+public void init(HardwareMap hwMap) {
+    liftMotor = new CMotorEx(hwMap.get(DcMotorEx.class, "lift"));
+}
+
+public void update() {
+    // This only writes to the hub if the power actually changes!
+    liftMotor.setPower(calculatedPID); 
 }
 ```
 
 ---
 
-## ⚡ Performance Details
+## ⚡ The "Tick" Lifecycle
 
-### The "Tick" Lifecycle
-Every loop, `ftc-lib` executes in this strict order:
+Every loop, `ftc-lib` executes in this strict sequence:
 1. **Hardware Sync:** Clears Bulk Cache on all Hubs.
-2. **Logic Tick:** Runs your `stateMachineUpdate()`.
-3. **Subsystem Tick:** All subsystems calculate PIDs and update motor powers.
-4. **Telemetry Gate:** If the `TELEMETRY_DELAY_MS` has passed, it pushes data to Panels and the Driver Station.
-5. **Sync Sleep:** Adjusts thread sleep time to maintain a consistent `TARGET_FPS`.
-
-### Auton-to-TeleOp Persistence
-Using the `AutoTransfer` utility, this library can automatically carry over the robot's end-of-auton position (from **PedroPathing**) and the Alliance color into TeleOp, ensuring your field-centric drive and automation remain seamless.
+2. **Input Update:** Takes a snapshot of `g1` and `g2`.
+3. **Localization:** Updates PedroPathing Follower and performs Limelight Pose Healing.
+4. **Routine Tick:** Progresses background Actions.
+5. **Logic Tick:** Runs your `stateMachineUpdate()` (State Machine).
+6. **Subsystem Tick:** Runs `update()` on all registered subsystems.
+7. **Telemetry Gate:** Pushes data to Panels/Driver Station based on `TELEMETRY_DELAY_MS`.
+8. **Loop Sync:** Sleeps to maintain a steady `TARGET_FPS`.
 
 ---
 
@@ -107,11 +107,11 @@ Using the `AutoTransfer` utility, this library can automatically carry over the
 *   [PedroPathing](https://github.com/pedropathing/pedro-pathing)
 *   [Panels/Configurables (Sloth)](https://panels.bylazar.com/)
 *   [Panels/Telemetry (Sloth)](https://panels.bylazar.com/)
+*   [Panels/Gamepad (Sloth)](https://panels.bylazar.com/)
 
 ---
 
-## 🤝 Contribution
-When adding new subsystems:
-1. Ensure all hardware names are in `Constants`.
-2. Ensure `publishTelemetry` only sends data when `GLOBAL.DEBUG_MODE` is true.
-3. Never use `sleep()` inside a subsystem; use state timers instead.
+## 🤝 Contribution Best Practices
+1. **Safety First:** Always include a `routines.cancelAll()` and `follower.breakFollowing()` on a panic button (e.g., `gamepad1.back`).
+2. **Null-Safety:** Use the `CMotor.exists()` check in your subsystem `isHealthy()` overrides to prevent crashes from unplugged hardware.
+3. **Physics:** Use `kG` for vertical lifts and `kCos` for arms to keep your PID coefficients small and stable.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java

@@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode.lib;
 
 import android.os.SystemClock;
 
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.util.ElapsedTime;
@@ -10,16 +12,28 @@ import com.qualcomm.robotcore.hardware.VoltageSensor;
 
 import com.bylazar.telemetry.JoinedTelemetry;
 
+
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 import org.firstinspires.ftc.teamcode.lib.actions.RoutineManager;
 import org.firstinspires.ftc.teamcode.lib.pid.BaseController;
 import org.firstinspires.ftc.teamcode.lib.util.EnhancedGamepad;
+import org.firstinspires.ftc.teamcode.lib.util.LLUtil;
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import org.firstinspires.ftc.teamcode.util.AutoTransfer;
 import org.firstinspires.ftc.teamcode.util.FPSCounter;
 
 
 import java.util.List;
 
 public abstract class BaseOpMode extends OpMode {
+
+    public enum Alliance {
+        RED, BLUE
+    }
+
+    protected final Alliance alliance;
+
+    protected Follower follower;
     protected final SubsysManager manager = new SubsysManager();
 
     protected final RoutineManager routines = new RoutineManager();
@@ -35,21 +49,39 @@ public abstract class BaseOpMode extends OpMode {
 
     protected Telemetry unifiedTelemetry;
 
+    protected LLUtil limelight;
+
     protected abstract void setupSubsystems();
     protected abstract void stateMachineUpdate();
     protected void onInit() {}
 
+    public BaseOpMode(Alliance alliance) {
+        this.alliance = alliance;
+    }
+
     @Override
     public final void init() {
 
         if (manager.isRobotHealthy()) {
-            unifiedTelemetry.addData("SYSTEM STATUS", "READY / HEALTHY");
+            unifiedTelemetry.addData("SYSTEM STATUS", "OK");
         } else {
             unifiedTelemetry.addData("SYSTEM STATUS", "!!! HARDWARE FAILURE !!!");
             manager.health(unifiedTelemetry);
         }
         unifiedTelemetry = new JoinedTelemetry(telemetry);
 
+        follower = org.firstinspires.ftc.teamcode.pedroPathing.Constants.createFollower(hardwareMap);
+
+        if (AutoTransfer.isAutonRan) {
+            follower.setStartingPose(AutoTransfer.transferPose);
+        } else {
+            if (alliance == Alliance.RED) {
+                follower.setStartingPose(Constants.GLOBAL.DEFAULT_RED_POSE);
+            } else {
+                follower.setStartingPose(Constants.GLOBAL.DEFAULT_BLUE_POSE);
+            }
+        }
+
         batterySensor = hardwareMap.voltageSensor.iterator().next();
         BaseController.currentSystemVoltage = batterySensor.getVoltage();
 
@@ -59,6 +91,9 @@ public abstract class BaseOpMode extends OpMode {
         }
 
 
+        limelight = new LLUtil(hardwareMap);
+
+
         g1 = new EnhancedGamepad(gamepad1);
         g2 = new EnhancedGamepad(gamepad2);
 
@@ -78,31 +113,36 @@ public abstract class BaseOpMode extends OpMode {
     @Override
     public final void loop() {
         fps.startLoop();
-        for (LynxModule hub : allHubs) {
-            hub.clearBulkCache();
-        }
-
-
+        for (LynxModule hub : allHubs) hub.clearBulkCache();
         g1.update();
         g2.update();
 
-        if (voltageTimer.milliseconds() >= Constants.PIDController.VOLTAGE_CHECK_INTERVAL_MS) {
-            BaseController.currentSystemVoltage = batterySensor.getVoltage();
-            voltageTimer.reset();
+        follower.update();
+
+        limelight.updateHeading(Math.toDegrees(follower.getPose().getHeading()));
+        limelight.update();
+
+        if (Constants.GLOBAL.ENABLE_POSE_HEALING && limelight.hasTarget()) {
+            Pose3D mt2Pose = limelight.getMegaTagPose();
+            if (mt2Pose != null) {
+                double llX = mt2Pose.getPosition().x * Constants.LIMELIGHT.METERS_TO_INCHES;
+                double llY = mt2Pose.getPosition().y * Constants.LIMELIGHT.METERS_TO_INCHES;
+
+                Pose currentPose = follower.getPose();
+                double healedX = currentPose.getX() + (llX - currentPose.getX()) * Constants.LIMELIGHT.POSE_HEALING_FACTOR;
+                double healedY = currentPose.getY() + (llY - currentPose.getY()) * Constants.LIMELIGHT.POSE_HEALING_FACTOR;
+
+                follower.setPose(new Pose(healedX, healedY, currentPose.getHeading()));
+            }
         }
+
+        routines.updateAll();
         stateMachineUpdate();
         manager.updateAll();
-        routines.updateAll();
 
         if (telemetryTimer.milliseconds() >= Constants.GLOBAL.TELEMETRY_DELAY_MS) {
+            unifiedTelemetry.addData("Pose", follower.getPose().toString());
             manager.publishTelemetryAll(unifiedTelemetry);
-
-            if (Constants.GLOBAL.DEBUG_MODE) {
-                unifiedTelemetry.addData("System Voltage", BaseController.currentSystemVoltage);
-                unifiedTelemetry.addData("Loop Time (ms)", fps.getCurrentLoopTime());
-                unifiedTelemetry.addData("FPS", fps.getAverageFps());
-            }
-
             unifiedTelemetry.update();
             telemetryTimer.reset();
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/lib/PedroDriveAction.java

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.lib.actions.lib;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+
+import org.firstinspires.ftc.teamcode.lib.actions.Action;
+
+public class PedroDriveAction implements Action {
+    private final Follower follower;
+    private final Pose targetPose;
+    private final boolean holdEnd;
+
+    public PedroDriveAction(Follower follower, Pose targetPose, boolean holdEnd) {
+        this.follower = follower;
+        this.targetPose = targetPose;
+        this.holdEnd = holdEnd;
+    }
+
+    @Override
+    public void start() {
+        follower.followPath(
+                follower.pathBuilder()
+                        .addPath(new BezierLine(follower::getPose, targetPose))
+                        .setLinearHeadingInterpolation(follower.getPose().getHeading(), targetPose.getHeading())
+                        .build(),
+                holdEnd
+        );
+    }
+
+    @Override
+    public boolean update() {
+        return !follower.isBusy();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java

@@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 
 public class LLUtil {
     private Limelight3A limelight;
@@ -55,7 +56,6 @@ public class LLUtil {
 
         return heightDiff / Math.tan(angleToTargetRad);
     }
-
     public double getAreaDistance() { // TA Regr
         if (!hasTarget()) return lastKnownDistance;
         return (Constants.LIMELIGHT.AREA_COEFF * Math.pow(getTa(), Constants.LIMELIGHT.AREA_EXPONENT))
@@ -65,6 +65,15 @@ public class LLUtil {
     public void setPipeline(int index) {
         limelight.pipelineSwitch(index);
     }
+    public Pose3D getMegaTagPose() {
+        if (hasTarget()) {
+            return lastResult.getBotpose_MT2();
+        }
+        return null;
+    }
+    public void updateHeading(double yawDegrees) {
+        limelight.updateRobotOrientation(yawDegrees);
+    }
 
     public void stop() {
         limelight.stop();

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

@@ -1,19 +1,65 @@
 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.DRIVE;
+
 public class Constants {
-    public static FollowerConstants followerConstants = new FollowerConstants();
+    public static FollowerConstants followerConstants = new FollowerConstants()
+            .mass(12)
+            .translationalPIDFCoefficients(new PIDFCoefficients(0.26, 0, 0.01, 0.03))
+            .headingPIDFCoefficients(new PIDFCoefficients(1.8, 0, 0.01, 0.03))
+            .forwardZeroPowerAcceleration(-39.64668514000648)
+            .lateralZeroPowerAcceleration(-76.57271150137258);
+
+    public static MecanumConstants driveConstants = new MecanumConstants()
+            .maxPower(0.7)
+            .xVelocity(68.10320673181904)
+            .yVelocity(57.52038399624941)
+            .rightFrontMotorName(DRIVE.FRONT_RIGHT_MOTOR)
+            .rightRearMotorName(DRIVE.BACK_RIGHT_MOTOR)
+            .leftRearMotorName(DRIVE.BACK_LEFT_MOTOR)
+            .leftFrontMotorName(DRIVE.FRONT_LEFT_MOTOR)
+            .leftFrontMotorDirection(DRIVE.FRONT_LEFT_DIRECTION)
+            .leftRearMotorDirection(DRIVE.BACK_LEFT_DIRECTION)
+            .rightFrontMotorDirection(DRIVE.FRONT_RIGHT_DIRECTION)
+            .rightRearMotorDirection(DRIVE.BACK_RIGHT_DIRECTION);
 
     public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
 
+
+    public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants()
+            .forwardTicksToInches(0.0020041908950982315)
+            .strafeTicksToInches(0.002004094712407555)
+            .turnTicksToInches(0.0022824233082645137)
+            .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/subsys/Drivetrain.java

@@ -2,11 +2,11 @@ package org.firstinspires.ftc.teamcode.subsys;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.pedropathing.follower.Follower;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
 import org.firstinspires.ftc.teamcode.lib.Subsystem;
-import org.firstinspires.ftc.teamcode.lib.hardware.CMotor; // Your caching wrapper
+import org.firstinspires.ftc.teamcode.lib.hardware.CMotor;
 
 public class Drivetrain extends Subsystem {
 
@@ -17,55 +17,45 @@ public class Drivetrain extends Subsystem {
     }
 
     private DriveState currentState = DriveState.NORMAL;
-
-    // Use the Cached Wrapper instead of raw DcMotor
     private CMotor frontLeft, frontRight, backLeft, backRight;
-
+    private final Follower follower;
     private boolean hardwareExists = false;
 
     private double driveY = 0.0;
     private double driveX = 0.0;
     private double driveTurn = 0.0;
 
+    public Drivetrain(Follower follower) {
+        this.follower = follower;
+    }
+
     @Override
     public void init(HardwareMap hwMap) {
         try {
-            frontLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FL_NAME));
-            frontRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME));
-            backLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME));
-            backRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME));
+            frontLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FRONT_LEFT_MOTOR));
+            frontRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FRONT_RIGHT_MOTOR));
+            backLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BACK_LEFT_MOTOR));
+            backRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BACK_RIGHT_MOTOR));
             hardwareExists = true;
         } catch (Exception e) {
             hardwareExists = false;
         }
 
-        frontRight.setDirection(DcMotor.Direction.REVERSE);
-        backRight.setDirection(DcMotor.Direction.REVERSE);
+        if (hardwareExists) {
+            frontLeft.setDirection(DcMotor.Direction.REVERSE);
+            backLeft.setDirection(DcMotor.Direction.REVERSE);
 
-        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-    }
-
-    @Override
-    public boolean isHealthy() {
-        return hardwareExists && frontLeft != null && frontRight != null
-                && backLeft != null && backRight != null;
-    }
-
-    public void setTargetState(DriveState newState) {
-        this.currentState = newState;
-    }
-
-    public void setDriveVectors(double y, double x, double turn) {
-        this.driveY = y;
-        this.driveX = x;
-        this.driveTurn = turn;
+            frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        }
     }
 
     @Override
     public void update() {
+        if (follower.isBusy()) return;
+
         if (currentState == DriveState.LOCKED) {
             stopMotors();
             return;
@@ -85,8 +75,10 @@ public class Drivetrain extends Subsystem {
         max = Math.max(max, Math.abs(brPower));
 
         if (max > 1.0) {
-            flPower /= max; frPower /= max;
-            blPower /= max; brPower /= max;
+            flPower /= max;
+            frPower /= max;
+            blPower /= max;
+            brPower /= max;
         }
 
         frontLeft.setPower(flPower);
@@ -95,6 +87,16 @@ public class Drivetrain extends Subsystem {
         backRight.setPower(brPower);
     }
 
+    public void setDriveVectors(double y, double x, double turn) {
+        this.driveY = y;
+        this.driveX = x;
+        this.driveTurn = turn;
+    }
+
+    public void setTargetState(DriveState newState) {
+        this.currentState = newState;
+    }
+
     private void stopMotors() {
         frontLeft.setPower(0);
         frontRight.setPower(0);
@@ -102,11 +104,17 @@ public class Drivetrain extends Subsystem {
         backRight.setPower(0);
     }
 
+    @Override
+    public boolean isHealthy() {
+        return hardwareExists;
+    }
+
     @Override
     public void publishTelemetry(Telemetry telemetry) {
         if (Constants.GLOBAL.DEBUG_MODE) {
             telemetry.addData("Drive State", currentState.toString());
             telemetry.addData("Drive Vectors", String.format("Y:%.2f X:%.2f T:%.2f", driveY, driveX, driveTurn));
+            telemetry.addData("Pedro Busy", follower.isBusy());
         }
     }
 }

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

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+@TeleOp(name = "Blue TeleOp", group = "Main")
+public class BlueTeleOp extends teleOp {
+    public BlueTeleOp() {
+        super(Alliance.BLUE);
+    }
+
+    @Override
+    protected void onInit() {
+        super.onInit();
+        unifiedTelemetry.addLine("Alliance: BLUE");
+    }
+}

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

@@ -1,6 +1,8 @@
 package org.firstinspires.ftc.teamcode.teleOp;
 
 import com.bylazar.configurables.annotations.Configurable;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.pedropathing.geometry.Pose;
 
 @Configurable
 public class Constants {
@@ -11,6 +13,10 @@ public class Constants {
         public static boolean DEBUG_MODE = true;
         public static int TARGET_FPS = 0; // No target FPS (UNCAPPED)
 
+        public static boolean ENABLE_POSE_HEALING = true;
+
+        public static Pose DEFAULT_RED_POSE = new Pose(72,72,0);
+        public static Pose DEFAULT_BLUE_POSE = new Pose(0,0,0);
 
     }
 
@@ -31,6 +37,8 @@ public class Constants {
         public static String NAME = "limelight";
         public static int POLL_RATE = 100;
 
+        public static double METERS_TO_INCHES = 39.3701;
+
 
         // Trig Distance
         public static double MOUNT_HEIGHT_IN = 0.0;
@@ -41,16 +49,23 @@ public class Constants {
         public static double AREA_EXPONENT = -0.518935;
         public static double AREA_OFFSET = 4.0;
 
+        public static double POSE_HEALING_FACTOR = 0.05;
         public static double STALE_DATA_TIMEOUT_SEC = 0.5;
     }
 
     @Configurable
     public static class DRIVE {
         // Hardware Names
-        public static final String FL_NAME = "fl";
-        public static final String FR_NAME = "fr";
-        public static final String BL_NAME = "bl";
-        public static final String BR_NAME = "br";
+        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;
 
         // Speed Constants
         public static double NORMAL_SPEED = 0.6;

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

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+@TeleOp(name = "Red TeleOp", group = "Main")
+public class RedTeleOp extends teleOp {
+    public RedTeleOp() {
+        super(Alliance.RED);
+    }
+
+    @Override
+    protected void onInit() {
+        super.onInit();
+        unifiedTelemetry.addLine("Alliance: RED");
+    }
+}

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

@@ -1,13 +1,14 @@
 package org.firstinspires.ftc.teamcode.teleOp;
 
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.pedropathing.geometry.Pose;
+
 import org.firstinspires.ftc.teamcode.lib.BaseOpMode;
+import org.firstinspires.ftc.teamcode.lib.actions.Routine;
+import org.firstinspires.ftc.teamcode.lib.actions.lib.PedroDriveAction;
 import org.firstinspires.ftc.teamcode.subsys.Drivetrain;
-import org.firstinspires.ftc.teamcode.util.AutoTransfer;
 
 
-@TeleOp(name = "Basic TeleOp", group = "Main")
-public class teleOp extends BaseOpMode {
+public abstract class teleOp extends BaseOpMode {
 
     public enum GlobalState {
         IDLE,
@@ -17,32 +18,41 @@ public class teleOp extends BaseOpMode {
     private GlobalState robotState = GlobalState.IDLE;
     private Drivetrain drive;
 
+    public teleOp(Alliance alliance) {
+        super(alliance);
+    }
+
     @Override
     protected void setupSubsystems() {
-        drive = new Drivetrain();
+        drive = new Drivetrain(follower);
         manager.register(drive);
     }
 
     @Override
     protected void onInit() {
-        if (AutoTransfer.hasValidData()) {
-            unifiedTelemetry.addData("Loaded Alliance", AutoTransfer.getAllianceString());
-            unifiedTelemetry.update();
-        }
+        unifiedTelemetry.addLine("Running TeleOP");
     }
 
     @Override
     protected void stateMachineUpdate() {
         if (g1.backWasPressed()) {
-
             routines.cancelAll();
+            follower.breakFollowing();
 
             robotState = GlobalState.IDLE;
-
-            drive.setTargetState(Drivetrain.DriveState.NORMAL);
-
             g1.rumbleBlips(2);
         }
+
+        if (g1.aWasPressed()) {
+            Pose scorePose = new Pose(72, 72, Math.toRadians(90));
+
+            routines.run(
+                    Routine.sequence(
+                            new PedroDriveAction(follower, scorePose, true),
+                            Routine.instant(() -> g1.rumble(500))
+                    )
+            );
+        }
         drive.setDriveVectors(-g1.left_stick_y(), g1.left_stick_x(), g1.right_stick_y());
 
         switch (robotState) {
@@ -64,4 +74,4 @@ public class teleOp extends BaseOpMode {
                 break;
         }
     }
-}
+}

doc/ftc-lib/docs.md

@@ -0,0 +1,146 @@
+# 📦 ftc-lib Documentation
+
+Welcome to **ftc-lib**, a high-performance, asynchronous framework designed for FTC robotics. This library focuses on clean architecture, hardware write-optimization (caching), and powerful motion control.
+
+---
+
+## 🏗️ 1. Subsystem Architecture
+The library uses a **Subsystem-based architecture**. Every major part of your robot (Drive, Lift, Intake) should be its own class extending `Subsystem`.
+
+### `Subsystem` Base Class
+Each subsystem must implement:
+- `init()`: Hardware mapping and initial states.
+- `update()`: Periodic logic (PID loops, state transitions).
+- `isHealthy()`: Returns `false` if a critical sensor or motor fails.
+
+### `SubsysManager`
+The manager coordinates all subsystems so your OpMode stays clean.
+```java
+SubsysManager manager = new SubsysManager();
+manager.register(drive, lift, claw);
+
+manager.initAll(hardwareMap); // Call in init()
+manager.updateAll();         // Call in every loop()
+manager.health(telemetry);   // Shows [ OK ] or [ ERROR ] for all parts
+```
+
+---
+
+## 🎬 2. Routines & Actions (Asynchronous Logic)
+The **Routines** system allows you to write complex, multi-step scripts (like an Auto-Score macro) **without using `sleep()`** or freezing your robot.
+
+### Building Blocks
+Use the `Routine` factory to create `Action` objects:
+
+| Method | Description |
+| :--- | :--- |
+| `Routine.instant(() -> ...)` | Runs code once and finishes immediately. |
+| `Routine.wait(ms)` | Pauses the sequence for a set time (non-blocking). |
+| `Routine.waitUntil(() -> ...)` | Pauses until a condition is met (e.g., sensor triggered). |
+| `Routine.sequence(a, b)` | Runs actions one after another. |
+| `Routine.parallel(a, b)` | Runs actions at the same time. |
+
+### Running in TeleOp
+```java
+// Inside your loop
+if (gamepad.yWasPressed() && !routines.isBusy()) {
+    routines.run(
+        Routine.sequence(
+            Routine.instant(() -> lift.setTarget(1000)),
+            Routine.waitUntil(() -> lift.atTarget()),
+            Routine.instant(() -> claw.open()),
+            Routine.wait(250),
+            Routine.instant(() -> lift.setTarget(0))
+        )
+    );
+}
+routines.updateAll(); // Actually runs the active actions
+```
+
+---
+
+## ⚙️ 3. Optimized Hardware (`CMotor` & `CServo`)
+Standard FTC hardware commands (like `setPower`) are expensive. `ftc-lib` uses **Cached Wrappers** to ensure hardware writes only happen when values actually change.
+
+- **`CMotor` / `CMotorEx`**: Caches power, mode, direction, and target position.
+- **`CServo` / `CServoEx`**: Caches position and PWM state.
+
+*Benefit: Significantly higher loop frequencies (Hz) and smoother PID control.*
+
+---
+
+## 📈 4. Control Theory & PIDF
+The library includes a robust hierarchy of controllers in the `lib.pid` package.
+
+### Features
+1. **Voltage Compensation**: Automatically scales power based on battery voltage (e.g., your lift moves the same at 14V as it does at 12V).
+2. **Low-Pass Filtering**: Smooths out noisy encoder data for more stable D-term calculations.
+3. **Anti-Windup**: Prevents the Integral (I) term from growing out of control.
+4. **Feedforward (PIDF)**:
+   - `kG`: Gravity compensation for vertical lifts.
+   - `kCos`: Gravity compensation for rotating arms.
+   - `kS`: Static friction (Stiction) override.
+   - `kV`: Velocity feedforward.
+
+### Example: Lift PIDF
+```java
+PIDFController controller = new PIDFController(kP, kI, kD);
+controller.kG = 0.1; // Holds the lift against gravity
+double power = controller.calculate(currentTicks);
+motor.setPower(power);
+```
+
+---
+
+## 🏎️ 5. Motion Profiling (`smooth`)
+For smooth, "robotic" movement, use the `smooth` class to generate **Trapezoidal Motion Profiles**. This prevents jerky movements that snap belts or tip the robot.
+
+```java
+smooth profile = new smooth(maxVel, maxAccel);
+profile.generate(startPos, targetPos);
+
+// In update loop
+State state = profile.calculate();
+liftController.setTarget(state.pos); 
+// state.vel can be used for kV feedforward!
+```
+
+---
+
+## 🎮 6. Enhanced Gamepad
+`EnhancedGamepad` wraps the standard `Gamepad` to provide essential features for driver control:
+- **Rising Edge Detection**: `aWasPressed()` (True only on the exact frame the button is clicked).
+- **Falling Edge Detection**: `aWasReleased()`.
+- **Stick Scaling**: Automatically applies a cubic curve and deadbands to sticks for finer precision.
+- **Trigger Buttons**: Use `left_trigger_btn()` to treat a trigger like a digital button.
+
+---
+
+## 👁️ 7. Vision Utilities (`LLUtil`)
+A wrapper for the **Limelight 3A** that simplifies targeting:
+- **Distance Estimation**: Includes two methods:
+    1. `getTrigDistance()`: Uses mounting angle and trigonometry.
+    2. `getAreaDistance()`: Uses a power-regression curve based on target area (`ta`).
+- **Data Freshness**: `isDataFresh()` checks if the target was lost recently to prevent "snapping" to old data.
+
+---
+
+## 🛠️ 8. Health Monitoring
+The `health` utility allows any part of the code to report a hardware fault.
+```java
+// Inside a subsystem
+if (motor.getCurrentAmps() > 10.0) {
+    health.reportFault("LIFT_STALL");
+}
+
+// In Telemetry
+telemetry.addData("Faults", health.getFaults());
+```
+
+---
+
+## 💡 Best Practices
+1. **Never use `Thread.sleep()`**: Use `Routines` instead.
+2. **Update the Manager**: Ensure `subsysManager.updateAll()` and `routineManager.updateAll()` are called at the very end of your loop.
+3. **Use States**: Subsystems should have internal Enums (e.g., `LiftState.INTAKING`, `LiftState.SCORING`) rather than just taking raw numbers.
+4. **Voltage Comp**: Always set `BaseController.currentSystemVoltage` once per loop from your hardware map.

doc/legal/AudioBlocksSounds.txt

@@ -1,21 +0,0 @@
-The sound files listed below in this SDK were purchased from www.audioblocks.com under the 
-following license.
-
-    http://support.audioblocks.com/customer/en/portal/topics/610636-licensing-faq-s/articles
-
-    How am I allowed to use your content?
-    Last Updated: Aug 11, 2016 01:51PM EDT
-    Our content may be used for nearly any project, commercial or otherwise, including feature
-    films, broadcast, commercial, industrial, educational video, print projects, multimedia, 
-    games, and the internet, so long as substantial value is added to the content. (For example, 
-    incorporating an audio clip into a commercial qualifies, while reposting our audio clip on 
-    YouTube with no modification or no visual component does not.) Once you download a file it is 
-    yours to keep and use forever, royalty-	free, even if you change your subscription or cancel 
-    your account.
-
-List of applicable sound files
-
-	chimeconnect.wav
-        chimedisconnect.wav
-	errormessage.wav
-	warningmessage.wav

doc/legal/Exhibit A - LEGO Open Source License Agreement.txt

@@ -1,15 +0,0 @@
-EXHIBIT A - LEGO® Open Source License Agreement
-
-The contents of the file 'nxtstartupsound.wav' contained in this SDK are subject to the 
-LEGO® Open Source License Agreement Version 1.0 (the "License"); you may not use this 
-file except in compliance with the License. You may obtain a copy of the License 
-at "LEGO Open Source License.pdf" contained in the same directory as this exhibit.
-
-Software distributed under the License is distributed on an "AS IS" basis, WITHOUT 
-WARRANTY OF ANY KIND, either express or implied. See the License for the specific 
-language governing rights and limitations under the License.
-
-The Original Code is <firmwareRoot>\AT91SAM7S256\Resource\SOUNDS\!Startup.rso.
-LEGO is the owner of the Original Code. Portions created by Robert Atkinson are 
-Copyright (C) 2015. All Rights Reserved.
-Contributor(s): Robert Atkinson.