ftc-lib/doc/ftc-lib/docs.md

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

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

// 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

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.

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.

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