# ftc-lib

## Yes, ts was generated... No I do not believe in an AI takeover

**ftc-lib** is a high-performance, state-machine-driven framework designed for FTC teams who prioritize clean code, modularity, and hardware optimization. It provides a robust abstraction layer over standard FTC OpModes, integrating **PedroPathing** for precision movement and **Panels (Sloth)** for live configuration and telemetry.

---

## 🚀 Key Features

*   **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.

---

## 📂 Project Structure

```text
teamcode/
├── 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. Enhanced Gamepad & States
The backend automatically updates `g1` and `g2`. Use edge detection for clean state transitions.

```java
@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());

    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); 
}
```

---

## ⚡ The "Tick" Lifecycle

Every loop, `ftc-lib` executes in this strict sequence:
1. **Hardware Sync:** Clears Bulk Cache on all Hubs.
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`.

---

## 📦 Dependencies
*   [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 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.