Update README.md

Added Pedro Support, Pedro Drive to Pose, and Megatag Support
docs
2026-03-24 02:49:10 +00:00 · 2026-03-23 21:45:54 -05:00 · 2026-03-23 19:09:42 -05:00 · 2026-03-23 18:57:24 -05:00 · 2026-03-23 18:56:32 -05:00
32 changed files with 976 additions and 237 deletions
--- a/README.md
+++ b/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
-    }
-}
-```
-
-### 3. Build your TeleOp
-Inherit from `BaseStateOpMode`. This gives you the `stateMachineUpdate()` hook where you map gamepad inputs to states.
-
-```java
-@TeleOp
-public class MainTeleOp extends BaseStateOpMode {
-    
-    @Override
-    protected void setupSubsystems() {
-        manager.register(new DriveSubsystem(), new LiftSubsystem());
-    }
-
-    @Override
-    protected void stateMachineUpdate() {
-        if (gamepad1.aWasPressed()) {
+    if (g1.aWasPressed()) {
        robotState = GlobalState.SCORING;
    }
-    }
+}
+```
+
+### 2. Writing a Routine
+Routines allow you to script the robot without using `sleep()`.
+
+```java
+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))
+    )
+);
+```
+
+### 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.
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java
+++ b/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,14 +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();
@@ -33,16 +49,41 @@ 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", "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();
-        PIDController.currentSystemVoltage = batterySensor.getVoltage();
+        BaseController.currentSystemVoltage = batterySensor.getVoltage();

        allHubs = hardwareMap.getAll(LynxModule.class);
        for (LynxModule hub : allHubs) {
@@ -50,6 +91,9 @@ public abstract class BaseOpMode extends OpMode {
        }


+        limelight = new LLUtil(hardwareMap);
+
+
        g1 = new EnhancedGamepad(gamepad1);
        g2 = new EnhancedGamepad(gamepad2);

@@ -69,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) {
-            PIDController.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", PIDController.currentSystemVoltage);
-                unifiedTelemetry.addData("Loop Time (ms)", fps.getCurrentLoopTime());
-                unifiedTelemetry.addData("FPS", fps.getAverageFps());
-            }
-
            unifiedTelemetry.update();
            telemetryTimer.reset();
        }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java
@@ -1,57 +0,0 @@
-package org.firstinspires.ftc.teamcode.lib;
-
-import com.qualcomm.robotcore.util.ElapsedTime;
-import org.firstinspires.ftc.teamcode.teleOp.Constants;
-
-public class PIDController {
-    public static double currentSystemVoltage = 12.0;
-
-    private double kP, kI, kD;
-    private double lastError = 0;
-    private double integralSum = 0;
-    private final ElapsedTime timer = new ElapsedTime();
-    private boolean hasRunOnce = false;
-
-    public PIDController(double kP, double kI, double kD) {
-        setCoefficients(kP, kI, kD);
-    }
-
-    public void setCoefficients(double kP, double kI, double kD) {
-        this.kP = kP;
-        this.kI = kI;
-        this.kD = kD;
-    }
-
-    public double calculate(double target, double current) {
-        double error = target - current;
-
-        if (!hasRunOnce) {
-            timer.reset();
-            lastError = error;
-            hasRunOnce = true;
-            return kP * error;
-        }
-
-        double dt = timer.seconds();
-        timer.reset();
-
-        if (dt == 0.0) return 0.0;
-
-        double derivative = (error - lastError) / dt;
-        integralSum += (error * dt);
-        lastError = error;
-
-        double rawOutput = (kP * error) + (kI * integralSum) + (kD * derivative);
-
-        double compensationMultiplier = Constants.PIDController.VOLTAGE_REFERENCE / currentSystemVoltage;
-
-        return rawOutput * compensationMultiplier;
-    }
-
-    public void reset() {
-        integralSum = 0;
-        lastError = 0;
-        hasRunOnce = false;
-        timer.reset();
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/SubsysManager.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/SubsysManager.java
@@ -31,4 +31,18 @@ public class SubsysManager {
            sub.publishTelemetry(telemetry);
        }
    }
+
+    public boolean isRobotHealthy() {
+        for (Subsystem sub : subsystems) {
+            if (!sub.isHealthy()) return false;
+        }
+        return true;
+    }
+
+    public void health(Telemetry telemetry) {
+        for (Subsystem sub : subsystems) {
+            String status = sub.isHealthy() ? "[ OK ]" : "[ ERROR ]";
+            telemetry.addData(sub.getClass().getSimpleName(), status);
+        }
+    }
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Subsystem.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Subsystem.java
@@ -3,10 +3,12 @@ package org.firstinspires.ftc.teamcode.lib;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import org.firstinspires.ftc.robotcore.external.Telemetry;

+import java.util.List;
+
 public abstract class Subsystem {
    public abstract void init(HardwareMap hwMap);
-
    public abstract void update();
-
    public void publishTelemetry(Telemetry telemetry) {}
+
+    public boolean isHealthy() { return true; }
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/lib/PedroDriveAction.java
+++ b/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();
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CMotor.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CMotor.java
@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.lib.hardware;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+public class CMotor {
+    protected DcMotor motor;
+
+    // Cached States
+    protected double lastPower = 0.0;
+    protected DcMotor.RunMode lastMode = null;
+    protected DcMotor.Direction lastDirection = null;
+    protected DcMotor.ZeroPowerBehavior lastZeroBehavior = null;
+    protected int lastTargetPos = 0;
+
+    protected final double CHANGE_TRES = 0.005;
+
+    public CMotor(DcMotor motor) {
+        this.motor = motor;
+    }
+
+    public void setPower(double power) {
+        // Optimization: Only write if power changed significantly
+        // Always write if power is exactly 0 or 1 (boundary cases)
+        if (Math.abs(power - lastPower) > CHANGE_TRES || (power == 0 && lastPower != 0) || (power == 1 && lastPower != 1)) {
+            motor.setPower(power);
+            lastPower = power;
+        }
+    }
+
+    public void setMode(DcMotor.RunMode mode) {
+        if (mode != lastMode) {
+            motor.setMode(mode);
+            lastMode = mode;
+        }
+    }
+
+    public void setDirection(DcMotor.Direction direction) {
+        if (direction != lastDirection) {
+            motor.setDirection(direction);
+            lastDirection = direction;
+        }
+    }
+
+    public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior behavior) {
+        if (behavior != lastZeroBehavior) {
+            motor.setZeroPowerBehavior(behavior);
+            lastZeroBehavior = behavior;
+        }
+    }
+
+    public void setTargetPosition(int pos) {
+        if (pos != lastTargetPos) {
+            motor.setTargetPosition(pos);
+            lastTargetPos = pos;
+        }
+    }
+
+    // Passthrough for reads (Bulk caching handles the speed here)
+    public int getCurrentPosition() { return motor.getCurrentPosition(); }
+    public boolean isBusy() { return motor.isBusy(); }
+    public DcMotor getInternalMotor() { return motor; }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CMotorEx.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CMotorEx.java
@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.lib.hardware;
+
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+public class CMotorEx extends CMotor {
+    private DcMotorEx motorEx;
+
+    private double lastVelocity = 0;
+
+    public CMotorEx(DcMotorEx motor) {
+        super(motor);
+        this.motorEx = motor;
+    }
+
+    public void setVelocity(double angularRate) {
+        // Velocity often fluctuates slightly, so we use a small epsilon
+        if (Math.abs(angularRate - lastVelocity) > 0.1) {
+            motorEx.setVelocity(angularRate);
+            lastVelocity = angularRate;
+        }
+    }
+
+    public void setPIDFCoefficients(DcMotorEx.RunMode mode, PIDFCoefficients coefficients) {
+        // We always set PIDF as it's usually done in init, and caching it
+        // adds more complexity than it saves time.
+        motorEx.setPIDFCoefficients(mode, coefficients);
+    }
+
+    // Extended Read Methods
+    public double getVelocity() { return motorEx.getVelocity(); }
+    public double getCurrentAmps() { return motorEx.getCurrent(org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit.AMPS); }
+
+    @Override
+    public DcMotorEx getInternalMotor() { return motorEx; }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CServo.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CServo.java
@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.lib.hardware;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class CServo {
+    protected final Servo servo;
+    protected double lastPos = -1.0;
+    protected Servo.Direction lastDir = null;
+
+    protected final double EPSILON = 0.001;
+
+    public CServo(Servo servo) {
+        this.servo = servo;
+    }
+
+    public void setPosition(double pos) {
+        if (Math.abs(pos - lastPos) > EPSILON) {
+            servo.setPosition(pos);
+            lastPos = pos;
+        }
+    }
+
+    public void setDirection(Servo.Direction direction) {
+        if (direction != lastDir) {
+            servo.setDirection(direction);
+            lastDir = direction;
+        }
+    }
+
+    public double getPosition() { return servo.getPosition(); }
+    public Servo getInternalServo() { return servo; }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CServoEx.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CServoEx.java
@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.lib.hardware;
+
+import com.qualcomm.robotcore.hardware.PwmControl;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+
+public class CServoEx extends CServo {
+    private final ServoImplEx servoEx;
+    private boolean lastPwmEnabled = true;
+
+    public CServoEx(ServoImplEx servo) {
+        super(servo);
+        this.servoEx = servo;
+    }
+
+    public void setPwmEnable(boolean enable) {
+        if (enable != lastPwmEnabled) {
+            if (enable) servoEx.setPwmEnable();
+            else servoEx.setPwmDisable();
+            lastPwmEnabled = enable;
+        }
+    }
+
+    public void setPwmRange(PwmControl.PwmRange range) {
+        servoEx.setPwmRange(range);
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/motion/smooth.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/motion/smooth.java
@@ -0,0 +1,78 @@
+package org.firstinspires.ftc.teamcode.lib.motion;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class smooth {
+    private double maxV, maxA;
+    private double dAccel, dCruise, dDecel;
+    private double tAccel, tCruise, tDecel, tTotal;
+    private double startPos, targetPos;
+    private final ElapsedTime timer = new ElapsedTime();
+
+    public smooth(double maxV, double maxA) {
+        this.maxV = maxV;
+        this.maxA = maxA;
+    }
+
+    public void generate(double currentPos, double targetPos) {
+        this.startPos = currentPos;
+        this.targetPos = targetPos;
+        double distance = Math.abs(targetPos - currentPos);
+
+        // Calculate time to reach max velocity
+        tAccel = maxV / maxA;
+        dAccel = 0.5 * maxA * Math.pow(tAccel, 2);
+
+        // If distance is too short to reach maxV, it becomes a triangular profile
+        if (dAccel * 2 > distance) {
+            tAccel = Math.sqrt(distance / maxA);
+            dAccel = 0.5 * maxA * Math.pow(tAccel, 2);
+            tCruise = 0;
+            dCruise = 0;
+        } else {
+            dCruise = distance - (2 * dAccel);
+            tCruise = dCruise / maxV;
+        }
+
+        tDecel = tAccel;
+        dDecel = dAccel;
+        tTotal = tAccel + tCruise + tDecel;
+        timer.reset();
+    }
+
+    public State calculate() {
+        double t = timer.seconds();
+        double dir = Math.signum(targetPos - startPos);
+        double p, v;
+
+        if (t < tAccel) {
+            // Acceleration Phase
+            v = maxA * t;
+            p = 0.5 * maxA * Math.pow(t, 2);
+        } else if (t < tAccel + tCruise) {
+            // Cruise Phase
+            v = maxV;
+            p = dAccel + maxV * (t - tAccel);
+        } else if (t < tTotal) {
+            // Deceleration Phase
+            double tDecelLocal = t - tAccel - tCruise;
+            v = maxV - (maxA * tDecelLocal);
+            p = dAccel + dCruise + (maxV * tDecelLocal) - (0.5 * maxA * Math.pow(tDecelLocal, 2));
+        } else {
+            // Finished
+            v = 0;
+            p = Math.abs(targetPos - startPos);
+        }
+
+        return new State(startPos + (p * dir), v * dir);
+    }
+
+    public boolean isFinished() {
+        return timer.seconds() >= tTotal;
+    }
+
+    public static class State {
+        public double pos, vel;
+        public State(double p, double v) { pos = p; vel = v; }
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/BaseController.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/BaseController.java
@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public abstract class BaseController {
+    // Voltage is updated globally by BaseOpMode
+    public static double currentSystemVoltage = 12.0;
+
+    protected double target = 0;
+    protected double tolerance = 0;
+    protected double settleTimeMs = 0;
+    protected final ElapsedTime settleTimer = new ElapsedTime();
+    protected final ElapsedTime dtTimer = new ElapsedTime();
+    
+    protected boolean hasRun = false;
+
+    public void setTarget(double target) {
+        this.target = target;
+    }
+
+    public void setTolerance(double tolerance, double settleTimeMs) {
+        this.tolerance = tolerance;
+        this.settleTimeMs = settleTimeMs;
+    }
+
+    /**
+     * Returns true if the error is within tolerance for the required settle time.
+     */
+    public boolean atSetpoint(double current) {
+        if (Math.abs(target - current) <= tolerance) {
+            if (settleTimer.milliseconds() >= settleTimeMs) {
+                return true;
+            }
+        } else {
+            settleTimer.reset();
+        }
+        return false;
+    }
+
+    protected double getVoltageMultiplier() {
+        return Constants.PIDController.VOLTAGE_REFERENCE / currentSystemVoltage;
+    }
+
+    public abstract double calculate(double current);
+    public abstract void reset();
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PController.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PController.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+public class PController extends BaseController {
+    public double kP;
+
+    public PController(double kP) {
+        this.kP = kP;
+    }
+
+    @Override
+    public double calculate(double current) {
+        double error = target - current;
+        return (error * kP) * getVoltageMultiplier();
+    }
+
+    @Override
+    public void reset() {
+        hasRun = false;
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PDController.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PDController.java
@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+public class PDController extends PController {
+    public double kD;
+    protected double lastError = 0;
+    protected double lastDerivative = 0;
+    
+    public double lowPassCoeff = 0.0; 
+
+    public PDController(double kP, double kD) {
+        super(kP);
+        this.kD = kD;
+    }
+
+    @Override
+    public double calculate(double current) {
+        double error = target - current;
+        double dt = dtTimer.seconds();
+        dtTimer.reset();
+
+        if (!hasRun || dt == 0) {
+            hasRun = true;
+            lastError = error;
+            return super.calculate(current);
+        }
+
+        double rawDerivative = (error - lastError) / dt;
+        
+        // Low-Pass Filter: smooths out noisy encoder data
+        double filteredDerivative = (lowPassCoeff * lastDerivative) + ((1 - lowPassCoeff) * rawDerivative);
+        
+        lastError = error;
+        lastDerivative = filteredDerivative;
+
+        double out = (error * kP) + (filteredDerivative * kD);
+        return out * getVoltageMultiplier();
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PIDController.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PIDController.java
@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+import com.qualcomm.robotcore.util.Range;
+
+public class PIDController extends PDController {
+    public double kI;
+    protected double integralSum = 0;
+    public double maxIntegral = 0.25; // Clamps the "I" term to max 25% power
+
+    public PIDController(double kP, double kI, double kD) {
+        super(kP, kD);
+        this.kI = kI;
+    }
+
+    @Override
+    public double calculate(double current) {
+        double error = target - current;
+        double dt = dtTimer.seconds();
+        // dtTimer.reset() is handled in super.calculate
+
+        if (hasRun && dt > 0) {
+            integralSum += error * dt;
+            // Anti-Windup: Clamp the sum so it doesn't grow to infinity
+            integralSum = Range.clip(integralSum, -maxIntegral/kI, maxIntegral/kI);
+        }
+
+        double pidOut = super.calculate(current) + (integralSum * kI);
+        return pidOut; // Multiplier already applied in PD super.calculate
+    }
+
+    @Override
+    public void reset() {
+        super.reset();
+        integralSum = 0;
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PIDFController.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PIDFController.java
@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+public class PIDFController extends PIDController {
+    public double kG = 0;   // Gravity (Linear)
+    public double kCos = 0; // Gravity (Angular/Arm)
+    public double kS = 0;   // Static Friction (Stiction)
+    public double kV = 0;   // Velocity (Target-based)
+
+    public PIDFController(double kP, double kI, double kD) {
+        super(kP, kI, kD);
+    }
+
+    /**
+     * @param current The current position (ticks/degrees)
+     * @param armAngle Only required if using kCos (Radians)
+     */
+    public double calculate(double current, double armAngle) {
+        double pidOut = super.calculate(current);
+        
+        // --- FEEDFORWARD MODELS ---
+        
+        // 1. Static Friction: Jumps the motor past mechanical resistance
+        double ffStatic = Math.signum(target - current) * kS;
+        
+        // 2. Linear Gravity: Holds a vertical lift up
+        double ffGravity = kG;
+        
+        // 3. Angular Gravity: kCos * Cosine of arm angle
+        double ffArm = Math.cos(armAngle) * kCos;
+        
+        // 4. Velocity FF: Based on the target (useful for flywheels)
+        double ffVelocity = target * kV;
+
+        return pidOut + ffStatic + ffGravity + ffArm + ffVelocity;
+    }
+
+    @Override
+    public double calculate(double current) {
+        return calculate(current, 0);
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/EnhancedGamepad.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/EnhancedGamepad.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.lib;
+package org.firstinspires.ftc.teamcode.lib.util;

 import com.qualcomm.robotcore.hardware.Gamepad;
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.lib;
+package org.firstinspires.ftc.teamcode.lib.util;

 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
@@ -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();
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/health.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/health.java
@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode.lib.util;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class health {
+    private static final List<String> faults = new ArrayList<>();
+
+    public static void reportFault(String deviceName) {
+        if (!faults.contains(deviceName)) {
+            faults.add(deviceName);
+        }
+    }
+
+    public static List<String> getFaults() {
+        return faults;
+    }
+
+    public static void clear() {
+        faults.clear();
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
+++ b/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();
    }
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java
@@ -2,60 +2,62 @@ 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;

 public class Drivetrain extends Subsystem {

-    // Subsystem Micro-States
    public enum DriveState {
        NORMAL,
        PRECISION,
-        LOCKED // Safety state to halt motors instantly
+        LOCKED
    }

    private DriveState currentState = DriveState.NORMAL;
-
-    private DcMotor frontLeft, frontRight, backLeft, backRight;
+    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) {
-        frontLeft = hwMap.get(DcMotor.class, Constants.DRIVE.FL_NAME);
-        frontRight = hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME);
-        backLeft = hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME);
-        backRight = hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME);
+        try {
+            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);
        }
-
-    // Command Hooks for the OpMode to control this subsystem
-    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;
    }

    @Override
    public void update() {
+        if (follower.isBusy()) return;
+
        if (currentState == DriveState.LOCKED) {
-            frontLeft.setPower(0); frontRight.setPower(0);
-            backLeft.setPower(0); backRight.setPower(0);
+            stopMotors();
            return;
        }

@@ -68,14 +70,15 @@ public class Drivetrain extends Subsystem {
        double blPower = (driveY - driveX + driveTurn) * speedMultiplier;
        double brPower = (driveY + driveX - driveTurn) * speedMultiplier;

-        // Normalization (prevents math outputting numbers > 1.0)
        double max = Math.max(Math.abs(flPower), Math.abs(frPower));
        max = Math.max(max, Math.abs(blPower));
        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);
@@ -84,11 +87,34 @@ 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);
+        backLeft.setPower(0);
+        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("Inputs (Y/X/T)", String.format("%.2f / %.2f / %.2f", driveY, driveX, driveTurn));
+            telemetry.addData("Drive Vectors", String.format("Y:%.2f X:%.2f T:%.2f", driveY, driveX, driveTurn));
+            telemetry.addData("Pedro Busy", follower.isBusy());
        }
    }
 }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/BlueTeleOp.java
+++ b/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");
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java
+++ b/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 {
@@ -10,6 +12,12 @@ public class Constants {
        public static double TELEMETRY_DELAY_MS = 250.0;
        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);
+
    }

    public static class Gamepad {
@@ -19,6 +27,9 @@ public class Constants {
    public static class PIDController {
        public static double VOLTAGE_REFERENCE = 12.0; // Voltage PID Controller was tuned at
        public static double VOLTAGE_CHECK_INTERVAL_MS = 500.0; // Every half second
+
+        public static double MAX_VELOCITY = 2000;
+        public static double MAX_ACCELERATION = 1500;
    }

    @Configurable
@@ -26,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;
@@ -36,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;
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/RedTeleOp.java
+++ b/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");
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java
+++ b/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) {
--- a/build.dependencies.gradle
+++ b/build.dependencies.gradle
@@ -17,6 +17,5 @@ dependencies {

    implementation 'com.pedropathing:ftc:2.1.0'
    implementation 'com.pedropathing:telemetry:1.0.0'
-    implementation 'com.bylazar:fullpanels:1.0.12'
 }

--- a/doc/ftc-lib/docs.md
+++ b/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.
--- a/doc/legal/AudioBlocksSounds.txt
+++ b/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
--- a/doc/legal/Exhibit
+++ b/doc/legal/Exhibit
@@ -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.
--- a/doc/legal/LEGO
+++ b/doc/legal/LEGO
--- a/doc/media/PullRequest.PNG
+++ b/doc/media/PullRequest.PNG
--- a/gradle.properties
+++ b/gradle.properties
@@ -6,7 +6,13 @@ android.useAndroidX=true
 # We no longer need to auto-convert third-party libraries to use AndroidX, which slowed down the build
 android.enableJetifier=false

-# Allow Gradle to use up to 1 GB of RAM
-org.gradle.jvmargs=-Xmx1024M
+# Allow Gradle to use up to  2GB of RAM
+org.gradle.jvmargs=-Xmx2048M

 android.nonTransitiveRClass=false
+org.gradle.configuration-cache=true
+
+org.gradle.parallel=true
+org.gradle.caching=true
+
+org.gradle.daemon=true
Author	SHA1	Message	Date
Arkm	c706a62d20	Update README.md	2026-03-24 02:49:10 +00:00
Arkm20	d400db1b96	Added Pedro Support, Pedro Drive to Pose, and Megatag Support	2026-03-23 21:45:54 -05:00
Arkm20	32feae48c0	docs	2026-03-23 19:09:42 -05:00
Arkm20	57bf6e3ad3	Essentially Photon, which caches power and pos for Servos and Motors, Custom PID, PD, P, and PIDF Objects	2026-03-23 18:57:24 -05:00
Arkm20	545c0cbda0	move files to util	2026-03-23 18:56:32 -05:00