diff --git a/README.md b/README.md index cf1ead0..8624025 100644 --- 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 + 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. \ No newline at end of file +## 🤝 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. \ No newline at end of file