Coding Guidelines

A Tutorial for Understanding Our FTC Robot Code

Target Audience: High school students with solid math and logic skills who are learning professional code development practices.

Reference Robot: deepthought (2024-2025 season) - demonstrates our recommended architecture patterns.

The Big Picture
Core Concept: Subsystems
The Update Loop
State Machines and Non-Blocking Code
Telemetry and Debugging
CPU Management and Loop Performance
Class Relationships
Practical Examples
Getting Started Checklist

The Big Picture

What is an FTC Robot Controller?

Your robot runs on an Android-based controller. The FTC SDK provides a framework where you write OpModes - programs that control your robot during matches. There are two types:

Autonomous OpMode: Robot runs by itself for 30 seconds
TeleOp OpMode: Driver controls the robot for 2 minutes

Why Architecture Matters

Imagine building a robot with 8 motors, 12 servos, 5 sensors, and a camera. If you put ALL the code in one file, you’d have:

Thousands of lines of tangled code
No way to test individual parts
Bugs that affect unrelated systems
Team members stepping on each other’s work

Solution: Break the robot into subsystems - independent units that each handle one responsibility.

graph TB
    subgraph "Bad: Everything in One Place"
        A[Giant OpMode.java<br/>3000+ lines of chaos]
    end

    subgraph "Good: Modular Subsystems"
        B[OpMode] --> C[Robot]
        C --> D[DriveTrain]
        C --> E[Arm]
        C --> F[Intake]
        C --> G[Sensors]
    end

Core Concept: Subsystems

The Subsystem Interface

In Java, an interface is a contract. It says “any class that implements me must have these methods.” Here’s our Subsystem interface from previous seasons, recently extended:

public interface Subsystem extends TelemetryProvider {
    void update(Canvas fieldOverlay);  // Called every loop cycle
    void stop();                        // Called when OpMode ends
}

public interface TelemetryProvider {
    Map<String, Object> getTelemetry(boolean debug);  // Return data for dashboard
    String getTelemetryName();                         // Name for dashboard section
}

What Each Method Does

Method	When It’s Called	What It Should Do
`update(Canvas)`	~50 times per second	Update motors, read sensors, run state machines
`stop()`	When OpMode ends	Stop all motors, save state if needed
`getTelemetry(debug)`	Every loop cycle	Return Map of values to display on dashboard
`getTelemetryName()`	Once at startup	Return a name like “DRIVETRAIN” or “ARM”

Subsystem Hierarchy

classDiagram
    class Subsystem {
        <<interface>>
        +update(Canvas fieldOverlay)
        +stop()
    }

    class TelemetryProvider {
        <<interface>>
        +getTelemetry(boolean debug) Map
        +getTelemetryName() String
    }

    class Robot {
        -Subsystem[] subsystems
        -DriveTrain driveTrain
        -Trident trident
        -Sensors sensors
        +update(Canvas)
        +stop()
    }

    class DriveTrain {
        -DcMotor[] motors
        -Pose2d pose
        +drive(x, y, rotation)
        +update(Canvas)
    }

    class Trident {
        -Arm sampler
        -Arm speciMiner
        +update(Canvas)
    }

    class Sensors {
        -IMU imu
        -DistanceSensor[] sensors
        +update(Canvas)
    }

    TelemetryProvider <|-- Subsystem
    Subsystem <|.. Robot
    Subsystem <|.. DriveTrain
    Subsystem <|.. Trident
    Subsystem <|.. Sensors
    Robot *-- DriveTrain
    Robot *-- Trident
    Robot *-- Sensors

Key Insight: The Robot class is ALSO a Subsystem! It contains other subsystems and coordinates them. This is called the Composite Pattern.

The Update Loop

How an OpMode Works

Every FTC OpMode follows this lifecycle:

sequenceDiagram
    participant FTC as FTC Runtime
    participant OP as Your OpMode
    participant R as Robot
    participant S as Subsystems

    FTC->>OP: init()
    OP->>R: new Robot(hardwareMap)
    R->>S: Initialize all subsystems

    loop init_loop (until Start pressed)
        FTC->>OP: init_loop()
        OP->>R: Check sensors, show status
    end

    FTC->>OP: start()
    OP->>R: robot.start()

    loop Main Loop (until Stop)
        FTC->>OP: loop()
        OP->>R: robot.update(canvas)
        R->>S: subsystem.update(canvas) for each
        OP->>FTC: Send telemetry
    end

    FTC->>OP: stop()
    OP->>R: robot.stop()
    R->>S: subsystem.stop() for each

Inside Robot.update()

This is the heart of your robot. Here’s what happens every cycle (~20ms):

public void update(Canvas fieldOverlay) {
    // 1. Update articulation state machine (robot-level coordination)
    articulate(articulation);

    // 2. Update each subsystem in order
    for (int i = 0; i < subsystems.length; i++) {
        Subsystem subsystem = subsystems[i];

        // Track how long each subsystem takes (for debugging)
        long startTime = System.nanoTime();
        subsystem.update(fieldOverlay);
        subsystemUpdateTimes[i] = System.nanoTime() - startTime;
    }

    // 3. Draw robot on field visualization
    drawRobot(fieldOverlay, driveTrain.getPose());
}

Update Order Matters!

Subsystems are updated in a specific order. If Subsystem B needs data from Subsystem A, A must update first.

flowchart LR
    A[Sensors] --> B[DriveTrain]
    B --> C[Arm/Trident]
    C --> D[Vision]

    style A fill:#e1f5fe
    style B fill:#fff3e0
    style C fill:#f3e5f5
    style D fill:#e8f5e9

Example: DriveTrain needs IMU heading from Sensors, so Sensors updates first.

Important Change

It is because of the sequence dependencies and concerns about optimizing device communications that we have recently switched to breaking the single subsystem.update() method into separate read, calc and act methods. Read more about our new 3 phase updates

State Machines and Non-Blocking Code

The Golden Rule: NEVER BLOCK THE LOOP

This is the single most important concept in robot programming. Your loop() method runs approximately 50 times per second (every 20 milliseconds). During each loop cycle:

All subsystems get updated
Sensors are read
Motors receive new commands
Telemetry is sent
The drivetrain responds to joystick input

If ANY code blocks (waits/pauses), EVERYTHING stops working.

sequenceDiagram
    participant L as loop()
    participant D as DriveTrain
    participant A as Arm
    participant S as Sensors

    Note over L: Normal Operation (~20ms per cycle)
    L->>D: update() - 2ms
    L->>A: update() - 3ms
    L->>S: update() - 1ms
    Note over L: Total: 6ms ✓ Robot responsive

    Note over L: WITH BLOCKING CODE
    L->>D: update() - 2ms
    L->>A: Thread.sleep(2000) - 2000ms!!!
    Note over A: Robot FROZEN for 2 seconds!
    Note over D: Can't steer!
    Note over S: Can't read sensors!

What “Blocking” Means

Blocking code is any code that waits for something to happen before continuing:

// ❌ ALL OF THESE ARE BLOCKING - NEVER USE IN update() OR loop()

Thread.sleep(1000);                    // Waits 1 second
while (!sensor.isPressed()) { }        // Waits until sensor pressed
motor.setTargetPosition(1000);
while (motor.isBusy()) { }             // Waits until motor reaches position
waitForStartPressed();                 // Built-in blocking wait

Why Blocking is Catastrophic

Consider what happens when your arm code blocks for 2 seconds:

Time	What Should Happen	What Actually Happens
0.0s	Driver pushes joystick forward	Nothing - loop is frozen
0.5s	Robot should be moving	Robot still frozen
1.0s	Driver sees wall, tries to stop	Robot still frozen
1.5s	Robot about to hit wall	Robot still frozen
2.0s	Blocking ends	Robot SLAMS into wall at full speed

Real consequences of blocking:

Robot becomes uncontrollable (safety hazard!)
Autonomous routines can’t adjust to obstacles
PID loops stop updating (motors overshoot)
Sensor readings become stale
Match time is wasted

The Non-Blocking Alternative: State Machines

A state machine breaks a multi-step action into discrete states. Each state:

Does ONE small thing
Checks if it’s time to move to the next state
Returns immediately (doesn’t wait)

flowchart TB
    subgraph "Blocking Approach ❌"
        B1[Start] --> B2[Move arm]
        B2 --> B3[WAIT 2 seconds]
        B3 --> B4[Close gripper]
        B4 --> B5[WAIT 0.5 seconds]
        B5 --> B6[Move arm back]
    end

    subgraph "State Machine Approach ✓"
        S1[IDLE] -->|"start()"| S2[MOVING_DOWN]
        S2 -->|"arm at position?"| S3[CLOSING_GRIPPER]
        S3 -->|"timer > 0.5s?"| S4[MOVING_UP]
        S4 -->|"arm at position?"| S1
    end

Anatomy of a State Machine

public class Arm implements Subsystem {

    // 1. Define all possible states
    public enum State {
        IDLE,           // Waiting for command
        MOVING_DOWN,    // Arm moving to pickup position
        CLOSING_GRIPPER,// Gripper closing on game piece
        MOVING_UP       // Arm returning to travel position
    }

    private State state = State.IDLE;
    private ElapsedTime timer = new ElapsedTime();

    // 2. Process current state in update() - NEVER BLOCKS
    @Override
    public void update(Canvas fieldOverlay) {
        switch (state) {
            case IDLE:
                // Do nothing, wait for startPickup() to be called
                break;

            case MOVING_DOWN:
                shoulder.setTargetPosition(PICKUP_POSITION);
                // Check transition condition - don't wait!
                if (Math.abs(shoulder.getCurrentPosition() - PICKUP_POSITION) < 10) {
                    gripper.setPosition(CLOSED);
                    timer.reset();  // Start timer for next state
                    state = State.CLOSING_GRIPPER;
                }
                break;

            case CLOSING_GRIPPER:
                // Gripper needs time to close physically
                if (timer.seconds() > 0.4) {
                    shoulder.setTargetPosition(TRAVEL_POSITION);
                    state = State.MOVING_UP;
                }
                break;

            case MOVING_UP:
                if (Math.abs(shoulder.getCurrentPosition() - TRAVEL_POSITION) < 10) {
                    state = State.IDLE;  // Sequence complete!
                }
                break;
        }
    }

    // 3. External trigger to start the sequence
    public void startPickup() {
        if (state == State.IDLE) {
            state = State.MOVING_DOWN;
        }
    }

    public boolean isIdle() {
        return state == State.IDLE;
    }
}

Common Non-Blocking Patterns

Pattern 1: Timer-Based Delays

private ElapsedTime timer = new ElapsedTime();
private boolean waitingForServo = false;

public void update() {
    if (waitingForServo) {
        // Check if enough time has passed - DON'T use Thread.sleep!
        if (timer.seconds() > 0.3) {
            waitingForServo = false;
            // Continue with next action
        }
    }
}

public void moveServoAndWait() {
    servo.setPosition(TARGET);
    timer.reset();
    waitingForServo = true;
}

Pattern 2: Condition-Based Transitions

public void update() {
    switch (state) {
        case MOVING_TO_TARGET:
            motor.setPower(0.5);
            // Transition when motor reaches target
            if (motor.getCurrentPosition() >= targetPosition) {
                motor.setPower(0);
                state = State.AT_TARGET;
            }
            break;
    }
}

Pattern 3: Sensor-Triggered Transitions

public void update() {
    switch (state) {
        case WAITING_FOR_GAMEPIECE:
            intake.setPower(1.0);
            // Transition when sensor detects piece
            if (distanceSensor.getDistance(DistanceUnit.CM) < 5) {
                intake.setPower(0);
                state = State.PIECE_ACQUIRED;
            }
            break;
    }
}

Pattern 4: Future Time Pattern

// Utility method (in your utils)
public static long futureTime(double seconds) {
    return System.currentTimeMillis() + (long)(seconds * 1000);
}

public static boolean isPast(long futureTime) {
    return System.currentTimeMillis() >= futureTime;
}

// Usage in subsystem
private long actionCompleteTime = 0;

public void update() {
    switch (state) {
        case SHOOTING:
            if (actionCompleteTime == 0) {
                shooter.setPower(1.0);
                actionCompleteTime = futureTime(0.5);  // Shoot for 0.5 seconds
            }
            if (isPast(actionCompleteTime)) {
                shooter.setPower(0);
                actionCompleteTime = 0;
                state = State.IDLE;
            }
            break;
    }
}

Debugging State Machines

Always include state information in your telemetry:

@Override
public Map<String, Object> getTelemetry(boolean debug) {
    Map<String, Object> telemetry = new LinkedHashMap<>();
    telemetry.put("State", state.name());          // Current state
    telemetry.put("Stage", stage);                  // Sub-step if using stages
    telemetry.put("Timer", timer.seconds());        // Time in current state
    return telemetry;
}

Watch for these problems:

Stuck in one state: Transition condition never becomes true
State oscillation: Rapidly switching between two states
Skipped states: Transition conditions too loose

State Machine Pattern

stateDiagram-v2
    [*] --> IDLE
    IDLE --> MOVING_TO_PICKUP: startPickup()
    MOVING_TO_PICKUP --> CLOSING_GRIPPER: arm at position
    CLOSING_GRIPPER --> MOVING_TO_DEPOSIT: gripper closed
    MOVING_TO_DEPOSIT --> OPENING_GRIPPER: arm at position
    OPENING_GRIPPER --> IDLE: gripper open

Implementation with Enums

public class Arm implements Subsystem {

    // Define all possible states
    public enum Articulation {
        MANUAL,           // Driver has direct control
        INTAKE,           // Moving to pickup position
        TRAVEL,           // Safe carrying position
        OUTTAKE,          // Moving to scoring position
        DEPOSIT           // Releasing game piece
    }

    private Articulation articulation = Articulation.MANUAL;
    private int stage = 0;  // Track sub-steps within a state

    @Override
    public void update(Canvas fieldOverlay) {
        switch (articulation) {
            case INTAKE:
                handleIntakeSequence();
                break;
            case OUTTAKE:
                handleOuttakeSequence();
                break;
            // ... other states
        }
    }

    private void handleIntakeSequence() {
        switch (stage) {
            case 0:  // Move arm down
                shoulder.setTargetPosition(INTAKE_POSITION);
                if (shoulder.isAtTarget()) stage++;
                break;
            case 1:  // Open gripper
                gripper.setPosition(OPEN);
                if (gripperTimer.seconds() > 0.3) stage++;
                break;
            case 2:  // Wait for game piece
                if (gamePieceDetected()) {
                    gripper.setPosition(CLOSED);
                    stage++;
                }
                break;
            case 3:  // Done - return to manual
                articulation = Articulation.MANUAL;
                stage = 0;
                break;
        }
    }
}

Robot-Level Coordination

The Robot class has its own state machine that coordinates multiple subsystems:

stateDiagram-v2
    [*] --> MANUAL

    MANUAL --> SAMPLER_INTAKE: button pressed
    SAMPLER_INTAKE --> TRAVEL: intake complete

    MANUAL --> SAMPLER_OUTTAKE: button pressed
    SAMPLER_OUTTAKE --> MANUAL: outtake complete

    MANUAL --> SPECIMINER_WALLTAKE: button pressed
    SPECIMINER_WALLTAKE --> TRAVEL: pickup complete

    TRAVEL --> SPECIMINER_OUTTAKE: at scoring zone
    SPECIMINER_OUTTAKE --> MANUAL: scored

Parallel Control Domain Architecture (lebot2)

When subsystems use different hardware, they should operate independently rather than being controlled by a single monolithic state machine. This is the “parallel control domain” pattern.

The Problem with Monolithic Articulation

A common anti-pattern is having Robot control everything:

// ❌ BAD: Monolithic articulation blocks unrelated subsystems
public enum Articulation {
    MANUAL,
    INTAKE,      // Intake ON, drive OFF, launcher OFF
    TARGETING,   // Intake OFF, drive to target, launcher OFF
    LAUNCHING,   // Intake OFF, drive OFF, launcher firing
    // ... combinatorial explosion!
}

switch (articulation) {
    case INTAKE:
        intake.on();
        driveTrain.stop();  // WHY? Intake doesn't need drive stopped!
        break;
}

Problems:

Drive blocked during intake (different hardware, no reason!)
Articulation enum explodes with combinations
Hard to add new behaviors
Rigid control flow

The Solution: Each Subsystem Owns Its Behavior

// ✓ GOOD: Parallel control domains
// Each subsystem manages its own state machine

// DriveTrain
public enum Behavior { MANUAL, TRAJECTORY, PID_TURN }
// Joystick input always accepted, interrupts autonomous modes

// Intake
public enum Behavior { OFF, INTAKE, LOAD_ALL, EJECT }
// Auto-completes LOAD_ALL when loader is full

// Launcher
public enum Behavior { IDLE, SPINNING }
// Pulls distance from Vision automatically when spinning

// Robot (multi-subsystem coordination ONLY)
public enum Behavior { MANUAL, TARGETING, LAUNCH_ALL }
// Only for sequences that require multiple subsystems

Control Domain Diagram

flowchart TB
    subgraph "Parallel Control Domains"
        subgraph "DriveTrain Domain"
            D1[MANUAL] --> D2[TRAJECTORY]
            D2 --> D1
            D1 --> D3[PID_TURN]
            D3 --> D1
        end

        subgraph "Intake Domain"
            I1[OFF] --> I2[LOAD_ALL]
            I2 --> I1
            I1 --> I3[EJECT]
            I3 --> I1
        end

        subgraph "Launcher Domain"
            L1[IDLE] --> L2[SPINNING]
            L2 --> L1
        end
    end

    ROBOT[Robot: Multi-Subsystem Sequences] -.->|LAUNCH_ALL| L2
    ROBOT -.->|TARGETING| D3

    style D1 fill:#4caf50
    style I1 fill:#2196f3
    style L1 fill:#ff9800

Key insight: All three domains run simultaneously. Intake can be loading while drivetrain is following a trajectory while launcher is spinning up.

Unified Behavior API

All subsystems use the same pattern:

public interface BehaviorProvider {
    enum Behavior { ... }
    void setBehavior(Behavior b);
    Behavior getBehavior();
}

// Usage in DriverControls:
if (stickyGamepad.a) {
    robot.intake.setBehavior(Intake.Behavior.LOAD_ALL);
    robot.loader.requestBeltForIntake();
}

if (stickyGamepad.x) {
    robot.launcher.setBehavior(Launcher.Behavior.SPINNING);
}

// Drive is ALWAYS called - it handles mode switching internally
robot.driveTrain.drive(throttle, strafe, turn);

Shared Resource Arbitration

When subsystems share hardware (e.g., the belt serves both intake and launcher), use an ownership model:

// Loader arbitrates belt access
public enum BeltOwner { NONE, INTAKE, LAUNCHER }

// Priority: LAUNCHER > INTAKE > NONE
public void calc() {
    if (launcherClaimsBelt) {
        currentOwner = BeltOwner.LAUNCHER;
        beltPower = launcherBeltPower;
    } else if (intakeRequestsBelt) {
        currentOwner = BeltOwner.INTAKE;
        beltPower = INTAKE_FEED_POWER;
    } else {
        currentOwner = BeltOwner.NONE;
        beltPower = 0;
    }
}

Request vs Claim:

requestBeltForIntake() - polite request, can be preempted
claimBeltForLauncher() - takes ownership, suppresses intake

When to Use Robot-Level Behaviors

Only use Robot-level coordination for sequences that require multiple subsystems working together:

Scenario	Where to Put Logic
Intake until full	Intake - single subsystem
Spin up launcher	Launcher - single subsystem
Turn to vision target	DriveTrain - single subsystem
Fire all balls (spin + feed + count)	Robot - coordinates launcher + loader
Target then launch	Robot - coordinates drivetrain + launcher

// Robot.Behavior.LAUNCH_ALL coordinates:
// 1. Launcher spins up
// 2. When ready, Launcher fires
// 3. Loader feeds next ball
// 4. Repeat until empty
// 5. Return to MANUAL

private void handleLaunchAllBehavior() {
    switch (launchAllState) {
        case SPINNING_UP:
            launcher.setBehavior(Launcher.Behavior.SPINNING);
            if (launcher.isReady()) {
                launchAllState = LaunchAllState.FIRING;
            }
            break;
        case FIRING:
            launcher.fire();  // Single ball
            // ...
    }
}

Drive Is Always Available

Critical rule: Drive input should ALWAYS be processed, never blocked.

// DriverControls.joystickDrive()
public void joystickDrive() {
    double throttle = -gamepad1.left_stick_y;
    double turn = -gamepad1.right_stick_x;

    // ALWAYS call drive - drivetrain handles mode switching internally
    // If joystick input is significant, it interrupts any automation
    robot.driveTrain.drive(throttle, 0, turn);

    // Handle buttons separately
    handleButtons();
}

Inside drivetrain, joystick input beyond deadzone switches to MANUAL:

public void drive(double throttle, double strafe, double turn) {
    if (Math.abs(throttle) > DEADZONE || Math.abs(turn) > DEADZONE) {
        if (behavior != Behavior.MANUAL) {
            behavior = Behavior.MANUAL;  // Interrupt trajectory/turn
        }
    }
    // ... set motor powers
}

Parallel Control Domain Checklist

When designing subsystem behaviors:

Does this subsystem use unique hardware? → Own behavior enum
Can this run while other subsystems run? → Parallel domain
Does this share hardware with another subsystem? → Ownership model
Does this require multiple subsystems? → Robot-level behavior
Is drive needed during this action? → Keep drive available

Missions: Composite Autonomous Operations

Missions are the highest level of behavior composition. They represent complete autonomous objectives that coordinate multiple robot behaviors, subsystems, and RoadRunner trajectories.

Architecture: Strategic vs Tactical Split

┌─────────────────────────────────────────────────────────┐
│ Autonomous.java (STRATEGIC)                             │
│ - Which missions to run                                 │
│ - What order (based on starting position, partners)     │
│ - Match-specific configuration                          │
└─────────────────────┬───────────────────────────────────┘
                      │ triggers missions
                      ▼
┌─────────────────────────────────────────────────────────┐
│ Missions.java (TACTICAL)                                │
│ - How to execute each mission                           │
│ - State machines for each mission type                  │
│ - RoadRunner trajectory building                        │
└─────────────────────┬───────────────────────────────────┘
                      │ uses
                      ▼
┌─────────────────────────────────────────────────────────┐
│ Robot.Behavior + Subsystem.Behavior                     │
│ - TARGETING, LAUNCH_ALL (Robot level)                   │
│ - Intake.LOAD_ALL, Launcher.SPINNING (Subsystem level)  │
└─────────────────────────────────────────────────────────┘

Layer Hierarchy:

Layer 4: MISSIONS           (Missions.java - BALL_GROUP, LAUNCH_PRELOADS, OPEN_SESAME)
Layer 3: ROBOT BEHAVIORS    (Robot.Behavior - TARGETING, LAUNCH_ALL)
Layer 2: SUBSYSTEM BEHAVIORS (Intake.LOAD_ALL, Launcher.SPINNING)
Layer 1: SUBSYSTEMS         (Loader, Launcher, Vision)
Layer 0: HARDWARE           (Motors, Servos, Sensors)

Available Missions (lebot2):

Mission	Description	What It Does
`LAUNCH_PRELOADS`	Fire preloaded balls	Delegates to Robot.Behavior.LAUNCH_ALL
`BALL_GROUP`	Collect from a ball group	RoadRunner trajectory → intake → monitor ball count
`OPEN_SESAME`	Release classifier	RoadRunner trajectory → back into release lever
`GO_BALL_CLUSTER`	(Future) Vision-based pickup	Navigate to ball clusters after classifier release

Integration with Robot.java:

Missions is a field on Robot, and missions.calc() is called from Robot.update() only when a mission is active (conserves compute when idle):

// In Robot.java
public class Robot {
    public final Missions missions;

    public Robot(HardwareMap hardwareMap, boolean simulated) {
        // ... subsystem init ...
        missions = new Missions(this);
    }

    public void update(Canvas fieldOverlay) {
        // Phase 1: Read sensors
        // Phase 2: Calculations
        if (missions.isActive()) {
            missions.calc(fieldOverlay);  // Only when mission running
        }
        // ... subsystem calcs ...
        // Phase 3: Write outputs
    }
}

Mission Implementation Pattern:

Each mission is a state machine with its own enum:

public class Missions implements TelemetryProvider {
    private final Robot robot;

    public enum Mission {
        NONE, LAUNCH_PRELOADS, BALL_GROUP, OPEN_SESAME, GO_BALL_CLUSTER
    }

    public enum MissionState {
        IDLE, RUNNING, COMPLETE, FAILED
    }

    // Per-mission state machines
    private enum BallGroupState {
        IDLE, NAVIGATING_TO_GROUP, INTAKING, COMPLETE
    }

    // Start a mission
    public void startBallGroup(int groupIndex) {
        targetGroupIndex = groupIndex;
        currentMission = Mission.BALL_GROUP;
        missionState = MissionState.RUNNING;
        ballGroupState = BallGroupState.IDLE;
    }

    // Main update - dispatches to current mission handler
    public void calc(Canvas fieldOverlay) {
        if (missionState != MissionState.RUNNING) return;

        switch (currentMission) {
            case BALL_GROUP:
                updateBallGroupMission();
                break;
            // ... other missions
        }
    }

    private void updateBallGroupMission() {
        TankDrivePinpoint driveTrain = (TankDrivePinpoint) robot.driveTrain;

        switch (ballGroupState) {
            case IDLE:
                // Build RoadRunner trajectory to ball group
                Pose2d targetPose = getBallGroupPose(targetGroupIndex);
                Action trajectory = driveTrain.actionBuilder(robot.driveTrain.getPose())
                    .splineToLinearHeading(targetPose, targetPose.heading.toDouble())
                    .build();
                driveTrain.runAction(trajectory);
                ballGroupState = BallGroupState.NAVIGATING_TO_GROUP;
                break;

            case NAVIGATING_TO_GROUP:
                if (!driveTrain.updateAction(actionPacket)) {
                    // Arrived, start intake
                    robot.intake.loadAll();
                    robot.loader.requestBeltForIntake();
                    ballGroupState = BallGroupState.INTAKING;
                }
                break;

            case INTAKING:
                robot.driveTrain.drive(0.15, 0, 0);  // Creep forward
                if (robot.loader.isFull()) {
                    robot.intake.off();
                    ballGroupState = BallGroupState.COMPLETE;
                }
                break;

            case COMPLETE:
                missionState = MissionState.COMPLETE;
                break;
        }
    }
}

Ball Group Configuration:

Field positions are configurable, and the order of approach depends on starting position and partner agreements:

// Predefined group positions (placeholder coordinates)
public static Pose2d BALL_GROUP_0_POSE = new Pose2d(24, 24, Math.toRadians(0));
public static Pose2d BALL_GROUP_1_POSE = new Pose2d(48, 24, Math.toRadians(0));
public static Pose2d BALL_GROUP_2_POSE = new Pose2d(72, 24, Math.toRadians(0));

// Configuration methods
public void setBallGroupOrder(int[] order);  // {0, 1, 2} or {2, 1, 0}
public int getNextBallGroup();               // Returns next group, -1 if done
public void advanceToNextGroup();            // Move to next in sequence

Usage from Autonomous.java:

public void execute() {
    switch (phase) {
        case BALL_GROUP:
            int nextGroup = robot.missions.getNextBallGroup();
            if (nextGroup >= 0) {
                robot.missions.startBallGroup(nextGroup);
                setPhase(AutonPhase.WAITING_BALL_GROUP);
            }
            break;

        case WAITING_BALL_GROUP:
            if (robot.missions.isComplete()) {
                robot.missions.advanceToNextGroup();
                setPhase(AutonPhase.TARGET_AND_LAUNCH);
            }
            break;
    }
}

Usage from TeleOp (DriverControls):

Missions aren’t just for autonomous - drivers can trigger them too:

// Driver triggers ball group collection
if (gamepad1.y) {
    robot.missions.startBallGroup(0);
}

// Driver triggers classifier release
if (gamepad1.x) {
    robot.missions.startOpenSesame();
}

Key Principles:

Missions don’t bypass layers: They coordinate through Robot.Behavior and Subsystem.Behavior, not directly controlling hardware
Interruptible: Call missions.abort() for emergency stop
State-based: Each mission is a non-blocking state machine
Parameterized: startBallGroup(0) vs startBallGroup(2) - same mission, different target
Configurable: Ball group order set during match setup based on strategy
Compute-efficient: missions.calc() only runs when a mission is active
Dual-use: Works from both Autonomous and TeleOp

Telemetry and Debugging

Understanding Telemetry Systems

FTC provides two telemetry systems that serve different purposes:

System	Display Location	Best For
Basic Telemetry	Driver Station phone	Competition matches
FTC Dashboard	Laptop browser	Development & tuning

flowchart TB
    subgraph Robot Controller
        CODE[Your Code]
        BASIC[telemetry object]
        DASH[FtcDashboard]
        MULTI[MultipleTelemetry]
    end

    subgraph Displays
        DS[Driver Station Phone]
        WEB[Laptop Browser<br/>192.168.43.1:8080/dash]
    end

    CODE --> BASIC --> DS
    CODE --> DASH --> WEB
    CODE --> MULTI
    MULTI --> BASIC
    MULTI --> DASH

    style MULTI fill:#ffeb3b

FTC Dashboard

FTC Dashboard is a web-based tool that shows real-time data from your robot. Access it at 192.168.43.1:8080/dash when connected to your robot.

Features:

Real-time telemetry graphs
Field visualization with robot position
Live configuration editing (@Config values)
Camera stream viewing
Faster update rates than Driver Station

MultipleTelemetry: The Best of Both Worlds

MultipleTelemetry is a wrapper that sends telemetry to BOTH the Driver Station AND FTC Dashboard simultaneously:

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;

public class MyOpMode extends OpMode {
    @Override
    public void init() {
        // Wrap the standard telemetry to send to both destinations
        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());

        // Now all telemetry.addData() calls go to BOTH displays
        telemetry.addData("Status", "Initialized");
    }
}

Telemetry Initialization Checklist

Follow this checklist to set up telemetry properly in your OpMode:

Step 1: Import Required Classes

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;

Step 2: Declare Dashboard Instance

public class MyOpMode extends OpMode {
    private FtcDashboard dashboard;
    public static boolean debugTelemetryEnabled = false;  // Toggle for verbose output

Step 3: Initialize in init()

@Override
public void init() {
    // Get dashboard instance
    dashboard = FtcDashboard.getInstance();

    // Set transmission intervals (milliseconds)
    // Higher = less network traffic, lower = more responsive
    dashboard.setTelemetryTransmissionInterval(250);  // Dashboard: 4 updates/sec
    telemetry.setMsTransmissionInterval(250);         // Driver Station: 4 updates/sec

    // Optional: Use MultipleTelemetry for dual output
    // telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
}

Step 4: Send Telemetry in loop()

@Override
public void loop() {
    // Create a packet for this frame
    TelemetryPacket packet = new TelemetryPacket();

    // Get the canvas for field drawing
    Canvas fieldOverlay = packet.fieldOverlay();

    // Update robot (passes canvas for visualization)
    robot.update(fieldOverlay);

    // Collect telemetry from all subsystems
    for (TelemetryProvider provider : robot.subsystems) {
        Map<String, Object> data = provider.getTelemetry(debugTelemetryEnabled);
        for (Map.Entry<String, Object> entry : data.entrySet()) {
            String line = entry.getKey() + ": " + entry.getValue();
            packet.addLine(line);           // To Dashboard
            telemetry.addLine(line);        // To Driver Station
        }
    }

    // Send everything
    dashboard.sendTelemetryPacket(packet);
    telemetry.update();
}

When to Use Each Telemetry System

Scenario	Recommended System	Why
Competition match	Basic telemetry only	Faster, no laptop needed
Practice/testing	MultipleTelemetry	See data on laptop AND phone
Tuning PID/positions	FTC Dashboard	Graphs, live config editing
Debugging autonomous	FTC Dashboard	Field visualization
Driver practice	Basic telemetry	Simulates competition

Competition Mode: Disabling Dashboard for Performance

Problem: FTC Dashboard adds network overhead that can slow your loop rate.

Solution: Add a “competition mode” toggle that disables dashboard during matches:

@Config(value = "0_COMPETITION")
public class MyOpMode extends OpMode {

    // Toggle this in Dashboard before competition matches
    public static boolean COMPETITION_MODE = false;

    private FtcDashboard dashboard;

    @Override
    public void init() {
        dashboard = FtcDashboard.getInstance();

        if (COMPETITION_MODE) {
            // Disable dashboard telemetry entirely
            dashboard.setTelemetryTransmissionInterval(1000);  // Slow updates
            telemetry.setMsTransmissionInterval(100);          // Fast DS updates
        } else {
            // Normal development mode
            dashboard.setTelemetryTransmissionInterval(250);
            telemetry.setMsTransmissionInterval(250);
        }
    }

    @Override
    public void loop() {
        TelemetryPacket packet = new TelemetryPacket();

        robot.update(packet.fieldOverlay());

        // Only send to dashboard if not in competition mode
        if (!COMPETITION_MODE) {
            collectDebugTelemetry(packet);
            dashboard.sendTelemetryPacket(packet);
        }

        // Always send essential data to Driver Station
        telemetry.addData("State", robot.getState());
        telemetry.addData("Loop Hz", getLoopFrequency());
        telemetry.update();
    }
}

Competition Mode Strategy

flowchart TB
    START[Match Starting] --> CHECK{COMPETITION_MODE?}

    CHECK -->|true| COMP[Competition Mode]
    CHECK -->|false| DEV[Development Mode]

    COMP --> C1[Skip dashboard packets]
    COMP --> C2[Minimal DS telemetry]
    COMP --> C3[Faster loop rate]

    DEV --> D1[Full dashboard telemetry]
    DEV --> D2[Field visualization]
    DEV --> D3[Debug data]

    C1 --> FAST[~60+ Hz loop]
    C2 --> FAST
    C3 --> FAST

    D1 --> NORMAL[~40-50 Hz loop]
    D2 --> NORMAL
    D3 --> NORMAL

Competition Mode Checklist:

Set COMPETITION_MODE = true before matches
Verify essential data still shows on Driver Station
Check loop frequency is acceptable (should be 50+ Hz)
Test that autonomous still works without dashboard
Remember to set back to false for practice!

Implementing getTelemetry() in Subsystems

Return a LinkedHashMap to preserve the order of your telemetry items:

@Override
public Map<String, Object> getTelemetry(boolean debug) {
    Map<String, Object> telemetry = new LinkedHashMap<>();

    // Always show important values (even in competition)
    telemetry.put("State", articulation.name());
    telemetry.put("Position", shoulder.getCurrentPosition());
    telemetry.put("Target", shoulder.getTargetPosition());

    // Only show detailed values in debug mode
    if (debug) {
        telemetry.put("Motor Power", shoulder.getPower());
        telemetry.put("Velocity", shoulder.getVelocity());
        telemetry.put("Stage", stage);
        telemetry.put("Error", targetPosition - currentPosition);
    }

    return telemetry;
}

@Override
public String getTelemetryName() {
    return "ARM";  // This appears as a header in the dashboard
}

Live Configuration with @Config

The @Config annotation lets you change values WITHOUT rebuilding the app:

@Config(value = "ARM")  // Creates a section called "ARM" in dashboard
public class Arm implements Subsystem {

    // These can be changed live in the dashboard!
    public static double SHOULDER_SPEED = 0.8;
    public static int INTAKE_POSITION = 500;
    public static int OUTTAKE_POSITION = 2000;
    public static double kP = 0.01;
    public static double kI = 0.0;
    public static double kD = 0.001;

    // ... rest of class
}

@Config Workflow:

Run your OpMode
Open Dashboard in browser (192.168.43.1:8080/dash)
Go to “Configuration” tab
Find your class (e.g., “ARM”)
Adjust values and see immediate effect
When happy, copy values back to code permanently

Important Notes:

Only public static fields appear in Configuration
Primitives work best (double, int, boolean)
Changes are lost when OpMode restarts
Use meaningful names like SHOULDER_kP not just kP

Telemetry Performance Tips

Tip	Why
Use `LinkedHashMap`	Preserves order of telemetry items
Batch your `addData()` calls	Fewer packet sends
Call `telemetry.update()` once per loop	Not multiple times
Use `debug` parameter	Skip expensive calculations in competition
Avoid string concatenation in loops	Use `String.format()` or `Misc.formatInvariant()`

CPU Management and Loop Performance

The Harsh Reality: You Don’t Get 50Hz

Throughout this document, we’ve mentioned that the loop runs “approximately 50 times per second.” This is optimistic. In practice:

Scenario	Typical Loop Rate	Notes
Minimal code, bulk reads	60-80 Hz	Best case, rarely achieved
Normal robot code	30-50 Hz	Common during development
Heavy sensor usage	15-25 Hz	I2C sensors are expensive
Unoptimized code	10-15 Hz	Performance is impaired
Worst case observed	~13 Hz	Iron Reign has experienced this

Why does this matter? At 13 Hz, your robot only updates every 77ms. A PID controller running this slowly cannot respond quickly enough to disturbances, causing overshoots and oscillations.

Understanding the Control Hub Architecture

flowchart TB
    subgraph Control Hub
        CPU[Quad-Core ARM CPU]
        MAIN[Main Thread<br/>YOUR CODE]
        OTHER[Other Threads<br/>Vision, Network, etc.]
        LYNX[Lynx Module<br/>Embedded Microcontroller]
    end

    subgraph Hardware
        M[Motors]
        S[Servos]
        E[Encoders]
        I2C[I2C Sensors]
        DIG[Digital/Analog]
    end

    CPU --> MAIN
    CPU --> OTHER
    MAIN <-->|LynxCommands<br/>~2-3ms each| LYNX
    LYNX --> M
    LYNX --> S
    LYNX --> E
    LYNX --> I2C
    LYNX --> DIG

    style MAIN fill:#ff9800
    style LYNX fill:#2196f3

Key Constraints:

Android is NOT a real-time OS - You don’t get guaranteed cycle times
Only the main thread accesses hardware - All motor/sensor calls must go through one thread
Each hardware call blocks - LynxCommands take 2-3ms each and CANNOT be parallelized
I2C is especially slow - Each I2C read requires multiple LynxCommands (7+ ms)

Why Multithreading Hardware Calls DOESN’T Help

You might think: “I have 4 CPU cores, I’ll just read sensors in parallel!”

This does NOT work. Here’s why:

sequenceDiagram
    participant T1 as Thread 1
    participant T2 as Thread 2
    participant LOCK as Bus Lock
    participant HUB as Lynx Hub

    Note over LOCK: Only ONE thread can<br/>talk to hardware at a time

    T1->>LOCK: Acquire lock
    T1->>HUB: Read encoder 1
    T2->>LOCK: Wait for lock...
    HUB-->>T1: Result (3ms)
    T1->>LOCK: Release lock

    T2->>LOCK: Acquire lock
    T2->>HUB: Read encoder 2
    T1->>LOCK: Wait for lock...
    HUB-->>T2: Result (3ms)
    T2->>LOCK: Release lock

    Note over T1,T2: Total time: 6ms<br/>Same as single-threaded!

The SDK uses a master lock on each USB bus. All LynxCommands are serialized regardless of threading. Worse, multithreading adds:

Thread contention overhead
Unpredictable timing between measurements
Harder-to-debug concurrency bugs

Exception: If you have two Expansion Hubs on separate USB buses (not RS485-connected), they CAN be accessed in parallel. But this is rare.

The Real Solution: Bulk Reads

Bulk reads retrieve ALL sensor data from a hub in a single LynxCommand. There is no equivalent “bulk write” - each motor command is still sent individually.

Approach	Hardware Calls	Time @ 2-3ms each	Typical Loop Rate
No bulk reads (4 encoders + 4 motors)	8 calls	~20-24ms	~40 Hz
With bulk reads (1 bulk + 4 motors)	5 calls	~12-15ms	~65 Hz
Optimized (bulk + lazy writes)	2-3 calls	~6-9ms	~100+ Hz

The key insight: reads are optimizable, writes are not - so minimize both, but especially focus on consolidating reads.

Implementing Bulk Reads

Step 1: Get All Lynx Modules

import com.qualcomm.hardware.lynx.LynxModule;
import java.util.List;

public class Robot {
    private List<LynxModule> allHubs;

    public Robot(HardwareMap hardwareMap) {
        // Get all connected hubs
        allHubs = hardwareMap.getAll(LynxModule.class);
    }
}

Step 2: Set Bulk Caching Mode

public void init() {
    // Set bulk caching mode for each hub
    for (LynxModule hub : allHubs) {
        hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
    }
}

Step 3: Clear Cache at Start of Each Loop

@Override
public void loop() {
    // FIRST THING: Clear the bulk cache
    for (LynxModule hub : allHubs) {
        hub.clearBulkCache();
    }

    // Now all encoder reads in this cycle use cached data
    robot.update(canvas);

    // Motor power sets still send individual commands
    telemetry.update();
}

Bulk Caching Modes Explained

Mode	Behavior	Best For
OFF	Every read = new command	Simple code, slow
AUTO	Cache auto-refreshes on repeated reads	Easy upgrade, moderate speed
MANUAL	You control when cache refreshes	Maximum performance

Recommendation: Use MANUAL mode and clear cache once per loop cycle.

// AUTO mode - simpler but less efficient
hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);

// MANUAL mode - maximum performance
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);

Warning with MANUAL: If you forget to clear the cache, you’ll get stale data!

Optimizing Writes: The Read→Process→Write Pattern

Since there’s no “bulk write,” every setPower() and setPosition() is a separate ~2-3ms command. The best strategies are:

Only write when values change (lazy writes)
Structure your loop as Read→Process→Write phases
Avoid interleaving reads and writes within subsystems

The Ideal Loop Structure

flowchart TB
    subgraph "Phase 1: READ"
        R1[Clear bulk cache] --> R2[All sensor reads<br/>use cached data]
    end

    subgraph "Phase 2: PROCESS"
        P1[State machines evaluate]
        P2[PID controllers calculate]
        P3[Decisions made]
        P1 --> P2 --> P3
    end

    subgraph "Phase 3: WRITE"
        W1[Motor powers set]
        W2[Servo positions set]
        W1 --> W2
    end

    R2 --> P1
    P3 --> W1

    style R1 fill:#4caf50
    style R2 fill:#4caf50
    style W1 fill:#ff9800
    style W2 fill:#ff9800

Enforcing Good Patterns in Subsystems

One approach: Split each subsystem’s update into explicit phases:

public interface Subsystem extends TelemetryProvider {
    void readSensors();          // Phase 1: Read from hardware
    void update(Canvas overlay); // Phase 2: Process state machines
    void writeOutputs();         // Phase 3: Write to hardware
    void stop();
}

Then in your main loop:

@Override
public void loop() {
    // PHASE 1: All reads happen first
    for (LynxModule hub : allHubs) {
        hub.clearBulkCache();
    }
    for (Subsystem subsystem : subsystems) {
        subsystem.readSensors();
    }

    // PHASE 2: All processing happens with consistent sensor data
    for (Subsystem subsystem : subsystems) {
        subsystem.update(canvas);
    }

    // PHASE 3: All writes happen at the end
    for (Subsystem subsystem : subsystems) {
        subsystem.writeOutputs();
    }

    telemetry.update();
}

Benefits:

All sensor reads use the same bulk cache snapshot
No interleaved reads/writes causing extra LynxCommands
State machines see consistent data (no mid-update sensor changes)

Lazy Writes: Only Send When Changed

Avoid sending redundant motor commands:

public class LazyMotor {
    private DcMotorEx motor;
    private double lastPower = Double.NaN;  // NaN ensures first write always sends
    private static final double TOLERANCE = 0.001;

    public LazyMotor(DcMotorEx motor) {
        this.motor = motor;
    }

    public void setPower(double power) {
        // Only send command if power actually changed
        if (Double.isNaN(lastPower) || Math.abs(power - lastPower) > TOLERANCE) {
            motor.setPower(power);
            lastPower = power;
        }
    }

    public void forcePower(double power) {
        // Bypass lazy check when you need guaranteed write
        motor.setPower(power);
        lastPower = power;
    }
}

When lazy writes help most:

Motors that hold steady power for extended periods (drivetrain at cruise, arm holding position)
Servos that don’t move every cycle

When to use forcePower():

In stop() methods - always force motors to 0
After mode changes
When recovering from errors

Practical Implementation in a Subsystem

public class DriveTrain implements Subsystem {
    private LazyMotor leftFront, rightFront, leftBack, rightBack;

    // Cached sensor values (read in Phase 1)
    private int leftEncoderPos, rightEncoderPos;
    private double imuHeading;

    // Computed outputs (calculated in Phase 2)
    private double leftPower, rightPower;

    @Override
    public void readSensors() {
        // All reads - uses bulk cache
        leftEncoderPos = leftFront.motor.getCurrentPosition();
        rightEncoderPos = rightFront.motor.getCurrentPosition();
        imuHeading = imu.getYaw();
    }

    @Override
    public void update(Canvas overlay) {
        // Pure computation - no hardware calls!
        updateOdometry(leftEncoderPos, rightEncoderPos, imuHeading);
        calculateMotorPowers();  // Sets leftPower, rightPower
    }

    @Override
    public void writeOutputs() {
        // All writes - lazy to avoid redundant commands
        leftFront.setPower(leftPower);
        rightFront.setPower(rightPower);
        leftBack.setPower(leftPower);
        rightBack.setPower(rightPower);
    }

    @Override
    public void stop() {
        // Force write on stop - safety critical
        leftFront.forcePower(0);
        rightFront.forcePower(0);
        leftBack.forcePower(0);
        rightBack.forcePower(0);
    }
}

When NOT to Over-Engineer

The phased approach adds complexity. Use it when:

You’re consistently below 40 Hz and need every ms
You have many subsystems with interleaved hardware calls
Odometry accuracy is suffering from timing inconsistencies

For simpler robots or early development, the standard single update() method is fine - just be mindful of not reading the same sensor multiple times per loop.

Three-Phase Update Pattern (lebot2 Implementation)

The lebot2 robot implements a strict three-phase update cycle that eliminates sensor read ordering bugs and enforces clean separation between sensor reads, calculations, and hardware writes. We use a caching wrapper to minimize expensive I2C sensor reads, and we rely on manual Bulk Reads to optimize non-I2C sensor communications.

Problem solved: Our coders can’t accidentally read stale sensor data or trigger I2C reads in the wrong order. All sensors are refreshed before any calculations run.

The Three Phases

┌─────────────────────────────────────────────────────────────┐
│ Phase 1: readSensors()                                      │
│ ├── clearBulkCache()           ← Motor encoders refreshed   │
│ ├── distanceSensors.refresh()  ← I2C reads cached           │
│ ├── imu heading cached         ← I2C read cached            │
│ └── subsystem.readSensors()    ← Each subsystem refreshes   │
├─────────────────────────────────────────────────────────────┤
│ Phase 2: calc()                                             │
│ ├── articulation state machine ← Robot-level coordination   │
│ ├── subsystem.calc()           ← State machines, PID        │
│ └── All logic uses CACHED sensor values only                │
├─────────────────────────────────────────────────────────────┤
│ Phase 3: act()                                              │
│ └── subsystem.act()            ← LazyMotor/Servo.flush()    │
└─────────────────────────────────────────────────────────────┘

Subsystem Interface

public interface Subsystem extends TelemetryProvider {
    /** Phase 1: Refresh all I2C sensors owned by this subsystem */
    void readSensors();

    /** Phase 2: Run state machines, calculations (use cached values only) */
    void calc(Canvas fieldOverlay);

    /** Phase 3: Flush all pending motor/servo commands */
    void act();

    /** Emergency stop */
    void stop();

    /** Reset state machines to initial state */
    void resetStates();
}

Hardware Wrapper Classes

LazyMotor - Defers motor writes until Phase 3:

public class LazyMotor {
    private DcMotorEx motor;
    private double pendingPower = 0;
    private boolean powerChanged = false;

    public void setPower(double power) {  // Called in Phase 2
        if (Math.abs(power - pendingPower) > 0.001) {
            pendingPower = power;
            powerChanged = true;
        }
    }

    public void flush() {  // Called in Phase 3
        if (powerChanged) {
            motor.setPower(pendingPower);
            powerChanged = false;
        }
    }
}

LazyServo - Defers servo writes until Phase 3:

public class LazyServo {
    private Servo servo;
    private double pendingPosition = 0;
    private boolean positionChanged = false;

    public void setPosition(double position) {  // Called in Phase 2
        pendingPosition = position;
        positionChanged = true;
    }

    public void flush() {  // Called in Phase 3
        if (positionChanged) {
            servo.setPosition(pendingPosition);
            positionChanged = false;
        }
    }
}

CachedDistanceSensor - Caches I2C reads from Phase 1:

public class CachedDistanceSensor {
    private DistanceSensor sensor;
    private double cachedDistance = 0;

    public void refresh() {  // Called in Phase 1
        cachedDistance = sensor.getDistance(DistanceUnit.CM);
    }

    public double getDistance() {  // Called in Phase 2
        return cachedDistance;
    }
}

Robot.java Update Loop

public void update(Canvas fieldOverlay) {
    // ===== PHASE 1: READ ALL SENSORS =====
    for (LynxModule hub : hubs) {
        hub.clearBulkCache();
    }
    for (Subsystem subsystem : subsystems) {
        subsystem.readSensors();
    }
    voltage = voltageSensor.getVoltage();

    // ===== PHASE 2: CALCULATIONS =====
    switch (articulation) {
        case INTAKE:
            handleIntakeArticulation();
            break;
        // ... other articulations
    }
    for (Subsystem subsystem : subsystems) {
        subsystem.calc(fieldOverlay);
    }

    // ===== PHASE 3: WRITE OUTPUTS =====
    for (Subsystem subsystem : subsystems) {
        subsystem.act();
    }
}

Example Subsystem: Loader

public class Loader implements Subsystem {
    private final LazyMotor beltMotor;
    private final CachedDistanceSensor frontSensor;
    private final CachedDistanceSensor backSensor;

    // Cached values from Phase 1
    private double frontDistance = 100;
    private double backDistance = 100;

    @Override
    public void readSensors() {
        // PHASE 1: Refresh I2C sensors
        frontSensor.refresh();
        backSensor.refresh();
    }

    @Override
    public void calc(Canvas fieldOverlay) {
        // PHASE 2: Read cached values and process logic
        frontDistance = frontSensor.getDistance();
        backDistance = backSensor.getDistance();

        // Ball detection logic using cached values
        ballAtFront = frontDistance < BALL_DETECT_THRESHOLD_CM;
        ballAtBack = backDistance < BALL_DETECT_THRESHOLD_CM;

        // Queue belt power (will be flushed in act())
        beltMotor.setPower(beltPower);
    }

    @Override
    public void act() {
        // PHASE 3: Flush motor command
        beltMotor.flush();
    }
}

Rules for Team Coders

DO	DON’T
Put all your code in `calc()`	Don’t read sensors directly in `calc()`
Read sensors via `cachedSensor.getValue()`	Don’t call `sensor.getValue()`
Write motors via `lazyMotor.setPower()`	Don’t call `motor.setPower()` directly
Let Robot handle `refresh()` and `flush()`	Don’t call these yourself

Why this matters: I2C sensors take ~7ms per read. If you accidentally read a sensor twice, or read before it’s refreshed, you get stale data or wasted time. This pattern makes it impossible to get wrong.

Understanding calc() - Where All Decisions Happen

The name calc() can be misleading - it’s where you make the robot act, not just calculate. Think of it as having three informal stages:

Stage	What Happens	Example
Sense	Transform raw sensor data into useful information	Convert encoder ticks to distance, filter noisy readings
Think	Make decisions based on behavior and strategy	State machine transitions, PID error calculation
Command	Issue decisions to lazy motors/servos	`lazyMotor.setPower(power)` queues the command

The act() method is purely mechanical - it just flushes pending commands to hardware. No decisions are made in act().

Execution Order

Robot.update() executes in this order:

All readSensors() calls
Robot’s behavior handling (can change subsystem behaviors)
All subsystem calc() calls
All act() calls

This ordering ensures the coordinator (Robot) can set behaviors before subsystems execute them.

When to Use Three-Phase Pattern

Robot Complexity	Recommendation
Simple (2-3 subsystems, no I2C)	Single `update()` is fine
Medium (4+ subsystems, some I2C)	Consider three-phase
Complex (many I2C sensors, odometry)	Use three-phase

The three-phase pattern is especially important when:

You have distance sensors or other I2C devices
Odometry timing consistency matters
Multiple subsystems need coordinated sensor data
Newer coders are writing subsystem code

Virtual Sensors

Virtual sensors are computed values derived from physical sensors. They abstract raw sensor data into higher-level concepts that behaviors can use without knowing the underlying implementation.

Why Virtual Sensors?

Encapsulation: Behaviors don’t need to know how “isFull” is determined
Consistency: One definition used everywhere, not duplicated logic
Testability: Can mock virtual sensor values for unit testing
Evolution: Can change implementation (e.g., add time confirmation) without touching behavior code

Update Order Matters

Virtual sensors must be updated AFTER physical sensors but BEFORE behaviors access them:

readSensors()     → Physical sensors refresh (I2C, encoders, etc.)
updateVirtual()   → Virtual sensors compute from physical (Pose2d, isFull, etc.)
calc()            → Behaviors use virtual sensor values
act()             → Commands flush to hardware

Current Virtual Sensors (lebot2)

Virtual Sensor	Physical Sources	Computed In	Used By
`Pose2d`	Pinpoint odometry pod	Localizer.update()	All path following, targeting
`Loader.isFull()`	Front/back distance sensors + count	Loader.calc()	Intake behavior decisions
`Loader.isBallAtBack()`	Back distance sensor	Loader.calc()	Launcher timing
`Vision.getDistanceToGoal()`	Limelight + AprilTags	Vision.calc()	Targeting, launch power

Pattern: Well-Designed Virtual Sensor

public class Loader implements Subsystem {
    // Physical sensors
    private final CachedDistanceSensor frontSensor;
    private final CachedDistanceSensor backSensor;

    // Virtual sensor state
    private boolean isFull = false;
    private long fullSinceMs = 0;
    private static final long FULL_CONFIRM_MS = 100; // Debounce time

    @Override
    public void calc(Canvas fieldOverlay) {
        // Read cached physical values
        double frontDist = frontSensor.getDistance();
        double backDist = backSensor.getDistance();

        // Compute raw full condition
        boolean rawFull = (ballCount >= MAX_BALLS) &&
                          (frontDist < THRESHOLD) &&
                          (backDist < THRESHOLD);

        // Time-confirmed virtual sensor (debouncing)
        if (rawFull) {
            if (fullSinceMs == 0) {
                fullSinceMs = System.currentTimeMillis();
            }
            isFull = (System.currentTimeMillis() - fullSinceMs) >= FULL_CONFIRM_MS;
        } else {
            fullSinceMs = 0;
            isFull = false;
        }
    }

    // Virtual sensor accessor - behaviors call this
    public boolean isFull() {
        return isFull;
    }
}

Benefits of This Pattern

Debouncing built in: Prevents flickering when ball is at sensor threshold
Single source of truth: All behaviors use the same definition of “full”
Behavior code stays simple: Just calls loader.isFull(), doesn’t know about thresholds or timing
Easy to tune: Change FULL_CONFIRM_MS or THRESHOLD in one place

I2C Sensors: The Hidden Performance Killer

I2C sensors (color sensors, distance sensors, IMUs) are MUCH slower than other hardware:

Sensor Type	Time per Read	Bulk-Readable?
Motor encoder	~2-3ms	YES
Servo position	~2-3ms	YES
Digital input	~2-3ms	YES
Analog input	~2-3ms	YES
I2C sensor	7+ ms	NO

Strategies for I2C:

Minimize I2C sensors - Can you use analog/digital alternatives?
Read less frequently - Not every sensor needs updating every cycle
Use Control Hub - SDK 5.5+ has faster I2C on CH vs Expansion Hub

// Read I2C sensor only every N cycles
private int loopCount = 0;
private double cachedDistance = 0;

@Override
public void update(Canvas c) {
    loopCount++;

    // Only read distance sensor every 5 cycles
    if (loopCount % 5 == 0) {
        cachedDistance = distanceSensor.getDistance(DistanceUnit.CM);
    }

    // Use cached value for logic
    if (cachedDistance < 10) {
        // React to obstacle
    }
}

Time-Based PID: Handling Variable Loop Rates

Since loop times are unpredictable, never assume a fixed dt in your PID controllers:

public class TimedPIDController {
    private double kP, kI, kD;
    private double integral = 0;
    private double lastError = 0;
    private long lastTime = 0;

    public double calculate(double target, double current) {
        long now = System.nanoTime();

        // Calculate actual elapsed time in seconds
        double dt = (lastTime == 0) ? 0.02 : (now - lastTime) / 1_000_000_000.0;
        lastTime = now;

        double error = target - current;

        // Proportional
        double p = kP * error;

        // Integral - multiply by dt to handle variable loop rates
        integral += error * dt;
        double i = kI * integral;

        // Derivative - divide by dt for rate of change
        double derivative = (dt > 0) ? (error - lastError) / dt : 0;
        double d = kD * derivative;

        lastError = error;

        return p + i + d;
    }
}

Why this matters: If your PID assumes 50Hz (dt=0.02s) but actually runs at 20Hz (dt=0.05s), your integral term accumulates 2.5x too fast!

Don’t use this code - this code snippet, like all of the snippets in this document, is meant to be illustrative, not used directly. Teams should have a more complete time-based PID Controller class. Ours is here

Monitoring Loop Performance

Always track your loop rate during development:

public class LoopMonitor {
    private long lastLoopTime = 0;
    private double avgLoopTime = 0;
    private static final double SMOOTHING = 0.1;

    public void update() {
        long now = System.nanoTime();
        if (lastLoopTime != 0) {
            double loopTime = (now - lastLoopTime) / 1_000_000.0;  // Convert to ms
            avgLoopTime = avgLoopTime * (1 - SMOOTHING) + loopTime * SMOOTHING;
        }
        lastLoopTime = now;
    }

    public double getLoopTimeMs() {
        return avgLoopTime;
    }

    public double getLoopHz() {
        return avgLoopTime > 0 ? 1000.0 / avgLoopTime : 0;
    }
}

Add to your telemetry:

telemetry.addData("Loop", "%.1f ms (%.1f Hz)",
    loopMonitor.getLoopTimeMs(),
    loopMonitor.getLoopHz());

CPU Optimization Checklist

High Priority (Do These First)

Enable bulk reads in MANUAL mode
Clear cache once at start of each loop
Minimize I2C sensors - use alternatives where possible
Cache sensor values - don’t read the same sensor twice per loop
Use time-based dt in all PID controllers

Medium Priority

Skip I2C reads on some cycles (every 3-5 loops)
Profile your loop - know which subsystems are slow
Avoid string operations in tight loops
Pre-calculate constants - don’t recompute trig in every loop

Low Priority (Diminishing Returns)

Use primitive arrays instead of ArrayLists
Avoid object allocation in loop()
Cache expensive calculations

Hardware Call Budget

Think of each loop cycle as a “budget” of time:

Target: 20ms cycle (50 Hz)

Budget breakdown:
- Bulk cache clear:     2ms
- 4 motor power sets:   8ms (4 × 2ms)
- Telemetry update:     2-5ms
- Your logic:           3-5ms
─────────────────────────────
Total:                  15-20ms ✓

If you add 3 I2C sensors:
- I2C reads:            21ms (3 × 7ms)
─────────────────────────────
Total:                  36-41ms ✗ (Only 24-28 Hz!)

Real-World Performance Patterns

flowchart TB
    subgraph "Optimized Loop (~50 Hz)"
        O1[Clear bulk cache] --> O2[Read cached encoders]
        O2 --> O3[Calculate PID with dt]
        O3 --> O4[Set motor powers]
        O4 --> O5[Minimal telemetry]
    end

    subgraph "Unoptimized Loop (~15 Hz)"
        U1[Read encoder 1] --> U2[Read encoder 2]
        U2 --> U3[Read encoder 3]
        U3 --> U4[Read encoder 4]
        U4 --> U5[Read I2C sensor 1]
        U5 --> U6[Read I2C sensor 2]
        U6 --> U7[Calculate PID]
        U7 --> U8[Set motors]
        U8 --> U9[Heavy telemetry]
    end

Understanding the Built-in PID Controller

The REV Expansion Hub (Lynx module) has an embedded microcontroller with its own PID implementation. This is NOT running on your Android CPU - it’s dedicated hardware.

How Motor Modes Work

// Always use DcMotorEx for full functionality
DcMotorEx motor = hardwareMap.get(DcMotorEx.class, "motor");

// Mode 1: No PID - you control raw power
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
motor.setPower(0.5);  // 50% of battery voltage

// Mode 2: Velocity PID - hub maintains target speed
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motor.setPower(0.5);  // Now acts like setSpeed - hub adjusts current to maintain velocity

// Mode 3: Position PID - hub drives to target position
motor.setTargetPosition(1000);
motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
motor.setPower(0.5);  // Max power allowed while seeking position

Benefits of the Built-in PID

Benefit	Explanation
Offloaded processing	PID calculations run on the Lynx module, not your main CPU
No encoder delay	Encoder reads happen locally on the hub, bypassing Android entirely
20 Hz is adequate	Motor electrical/mechanical time constants under typical FTC loads mean faster loops have diminishing returns
Consistent timing	The embedded controller has real-time guarantees your Android code doesn’t

Limitations of the Built-in PID

Limitation	Explanation
Encoder-only input	Can only use encoder feedback - no IMU, vision, or other sensors
No integral windup mitigation	The built-in controller lacks sophisticated anti-windup
Clumsy configuration	PID coefficients use internal units (not -1 to 1), awkward to tune
No feedforward	Cannot add kV/kA terms for better trajectory tracking

When to Use Built-in vs Custom PID

flowchart TB
    Q1{What feedback<br/>source?}
    Q1 -->|Encoder only| Q2{Need sophisticated<br/>control?}
    Q1 -->|IMU, Vision,<br/>Distance sensor| CUSTOM

    Q2 -->|Basic velocity/position| BUILTIN[Use Built-in PID<br/>RUN_USING_ENCODER or<br/>RUN_TO_POSITION]
    Q2 -->|Feedforward, anti-windup,<br/>motion profiles| CUSTOM[Use Custom PID<br/>e.g., util/PIDController]

    style BUILTIN fill:#4caf50
    style CUSTOM fill:#2196f3

Scenario	Recommendation
Simple drivetrain velocity	Built-in PID (RUN_USING_ENCODER)
Arm position hold	Built-in PID works, custom may be better for gravity compensation
Heading control from IMU	Custom PID required - can’t use encoder
Vision-based alignment	Custom PID required - can’t use encoder
Slide with motion profiling	Custom PID + feedforward
Turret tracking with Limelight	Custom PID required - visual feedback

Our Custom PID: util/PIDController

When you need custom PID, use our PIDController class which includes:

Time-based dt calculation (handles variable loop rates)
Multiple integral windup mitigation options
Configurable tolerance for “at target” detection
Continuous mode for angular wraparound

// Example: Heading control using IMU
PIDController headingPID = new PIDController(kP, kI, kD);
headingPID.setIntegrationBounds(-maxIntegral, maxIntegral);  // Anti-windup
headingPID.setContinuous(-180, 180);  // Handle angle wraparound

public void update() {
    double currentHeading = imu.getYaw();
    double correction = headingPID.calculate(targetHeading, currentHeading);
    driveWithRotation(correction);
}

Always Use DcMotorEx

Recommendation: Always use DcMotorEx instead of DcMotor:

// YES - Use this
DcMotorEx motor = hardwareMap.get(DcMotorEx.class, "motor");

// NO - Avoid this
DcMotor motor = hardwareMap.get(DcMotor.class, "motor");

DcMotorEx provides:

setVelocity() - set target velocity in ticks/sec or angular units
getVelocity() - read actual velocity
getCurrent() - monitor motor current draw
setVelocityPIDFCoefficients() - tune the built-in controller
setPositionPIDFCoefficients() - tune position control

Benchmarking: Measure, Don’t Guess

The numbers in this document are approximations. Your actual performance depends on your specific hardware, SDK version, and code. The only way to know your real performance is to measure it.

The Scientific Approach

Build a series of test OpModes that progressively add complexity. Run each for 30+ seconds and record the loop rate. This gives you hard data about what each feature costs.

Test 1: Bare Minimum Baseline

Start with the absolute minimum - just measure the loop overhead itself:

@TeleOp(name = "Benchmark 1: Bare Loop")
public class BenchmarkBareLoop extends OpMode {
    private long lastTime = 0;
    private double avgLoopMs = 0;
    private int loopCount = 0;

    @Override
    public void init() {
        telemetry.setMsTransmissionInterval(100);
    }

    @Override
    public void loop() {
        long now = System.nanoTime();
        if (lastTime != 0) {
            double loopMs = (now - lastTime) / 1_000_000.0;
            avgLoopMs = avgLoopMs * 0.95 + loopMs * 0.05;
        }
        lastTime = now;
        loopCount++;

        telemetry.addData("Loop", "%.2f ms (%.0f Hz)", avgLoopMs, 1000.0 / avgLoopMs);
        telemetry.addData("Count", loopCount);
        telemetry.update();
    }
}

Expected result: Very fast (100+ Hz) - this is your ceiling.

Test 2: Single Encoder Read (No Bulk)

Add one encoder read without bulk caching:

@TeleOp(name = "Benchmark 2: One Encoder")
public class BenchmarkOneEncoder extends OpMode {
    private DcMotorEx motor;
    private long lastTime = 0;
    private double avgLoopMs = 0;

    @Override
    public void init() {
        motor = hardwareMap.get(DcMotorEx.class, "motor");
        telemetry.setMsTransmissionInterval(100);
    }

    @Override
    public void loop() {
        long now = System.nanoTime();

        int position = motor.getCurrentPosition();  // This is the hardware call

        if (lastTime != 0) {
            double loopMs = (now - lastTime) / 1_000_000.0;
            avgLoopMs = avgLoopMs * 0.95 + loopMs * 0.05;
        }
        lastTime = now;

        telemetry.addData("Loop", "%.2f ms (%.0f Hz)", avgLoopMs, 1000.0 / avgLoopMs);
        telemetry.addData("Encoder", position);
        telemetry.update();
    }
}

Compare to Test 1: The difference is the cost of one LynxCommand.

Test 3: Four Encoders (No Bulk)

@TeleOp(name = "Benchmark 3: Four Encoders No Bulk")
public class BenchmarkFourEncodersNoBulk extends OpMode {
    private DcMotorEx m1, m2, m3, m4;
    // ... same timing code ...

    @Override
    public void loop() {
        long now = System.nanoTime();

        int p1 = m1.getCurrentPosition();
        int p2 = m2.getCurrentPosition();
        int p3 = m3.getCurrentPosition();
        int p4 = m4.getCurrentPosition();

        // ... timing and telemetry ...
    }
}

Expected: ~4x slower than Test 2 (each read is a separate command).

Test 4: Four Encoders WITH Bulk Read

@TeleOp(name = "Benchmark 4: Four Encoders With Bulk")
public class BenchmarkFourEncodersBulk extends OpMode {
    private List<LynxModule> allHubs;
    private DcMotorEx m1, m2, m3, m4;

    @Override
    public void init() {
        allHubs = hardwareMap.getAll(LynxModule.class);
        for (LynxModule hub : allHubs) {
            hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
        }
        // ... motor init ...
    }

    @Override
    public void loop() {
        long now = System.nanoTime();

        for (LynxModule hub : allHubs) {
            hub.clearBulkCache();
        }

        int p1 = m1.getCurrentPosition();
        int p2 = m2.getCurrentPosition();
        int p3 = m3.getCurrentPosition();
        int p4 = m4.getCurrentPosition();

        // ... timing and telemetry ...
    }
}

Compare to Test 3: Should be dramatically faster (approaching Test 2 speed).

Test 5: Add Motor Writes

@TeleOp(name = "Benchmark 5: Read + Write")
public class BenchmarkReadWrite extends OpMode {
    // ... bulk read setup ...

    @Override
    public void loop() {
        // Bulk read
        for (LynxModule hub : allHubs) hub.clearBulkCache();

        int p1 = m1.getCurrentPosition();
        int p2 = m2.getCurrentPosition();
        int p3 = m3.getCurrentPosition();
        int p4 = m4.getCurrentPosition();

        // Writes - each is a separate command
        m1.setPower(0.1);
        m2.setPower(0.1);
        m3.setPower(0.1);
        m4.setPower(0.1);

        // ... timing ...
    }
}

Observe: How much do the four setPower() calls add?

Test 6: Add I2C Sensor

@TeleOp(name = "Benchmark 6: Add I2C")
public class BenchmarkWithI2C extends OpMode {
    private DistanceSensor distSensor;  // I2C device
    // ... other setup ...

    @Override
    public void loop() {
        // ... bulk read, encoder reads ...

        double distance = distSensor.getDistance(DistanceUnit.CM);  // I2C call

        // ... motor writes, timing ...
    }
}

Expected: Significant slowdown - I2C is expensive.

Test 7: Add FTC Dashboard

@TeleOp(name = "Benchmark 7: With Dashboard")
public class BenchmarkWithDashboard extends OpMode {
    private FtcDashboard dashboard;

    @Override
    public void init() {
        dashboard = FtcDashboard.getInstance();
        dashboard.setTelemetryTransmissionInterval(250);
        // ... other setup ...
    }

    @Override
    public void loop() {
        TelemetryPacket packet = new TelemetryPacket();

        // ... all the hardware calls ...

        packet.put("Loop Hz", 1000.0 / avgLoopMs);
        dashboard.sendTelemetryPacket(packet);

        telemetry.addData("Loop", "%.2f ms (%.0f Hz)", avgLoopMs, 1000.0 / avgLoopMs);
        telemetry.update();
    }
}

Compare to Test 6: How much does dashboard add?

Recording Your Results

Create a table of your actual measurements:

Test	Description	Your Loop Time	Your Hz
1	Bare loop	___ ms	___ Hz
2	One encoder	___ ms	___ Hz
3	Four encoders (no bulk)	___ ms	___ Hz
4	Four encoders (bulk)	___ ms	___ Hz
5	Bulk + 4 motor writes	___ ms	___ Hz
6	Add I2C sensor	___ ms	___ Hz
7	Add FTC Dashboard	___ ms	___ Hz

Then calculate the cost of each feature:

Cost of one encoder read (no bulk): Test 2 - Test 1
Cost of bulk read: Test 4 - Test 1
Cost of four motor writes: Test 5 - Test 4
Cost of I2C: Test 6 - Test 5
Cost of Dashboard: Test 7 - Test 6

Why This Matters

Different Control Hubs, SDK versions, and firmware can have different performance characteristics. The numbers in tutorials (including this one) are guidelines, not guarantees.

When you have real data:

You know your actual performance ceiling
You can make informed decisions about what features to include
You can identify when a code change hurts performance
You can prove to yourself (and your team) that optimizations actually help

Extending the Benchmarks

Once you understand the basics, create benchmarks for:

Your actual subsystem code
Different numbers of I2C sensors
Vision processing overhead
Different telemetry verbosity levels
Competition mode vs development mode

The best teams know their numbers.

Summary: Golden Rules for Performance

Bulk reads are non-negotiable - Enable them in MANUAL mode
I2C is expensive - Minimize or rate-limit I2C sensor reads
Multithreading doesn’t help - All hardware is single-threaded
Measure your loop rate - You can’t improve what you don’t measure
Use time-based dt - Never assume consistent loop timing
Budget your calls - Think in terms of “how many ms does this cost?”
Benchmark, don’t guess - Run experiments to know your real performance

Class Relationships

Full Architecture Diagram

graph TB
    subgraph OpModes
        ITD[IntoTheDeep_6832<br/>Main OpMode]
        AUTO[Autonomous]
        DC[DriverControls]
    end

    subgraph Robot Layer
        ROBOT[Robot<br/>implements Subsystem]
    end

    subgraph Subsystems
        DT[DriveTrain]
        TRI[Trident]
        SEN[Sensors]
    end

    subgraph Trident Children
        SAM[Sampler<br/>extends Arm]
        SPEC[SpeciMiner<br/>extends Arm]
    end

    subgraph Utilities
        JOINT[Joint<br/>Servo wrapper]
        MOTOR[DcMotorExResetable<br/>Motor wrapper]
        POS[PositionCache]
        STICK[StickyGamepad]
    end

    subgraph Localization
        MEC[MecanumDriveReign]
        LOC[Localizer]
        PP[PinpointLocalizer]
        OTOS[OTOSLocalizer]
    end

    subgraph Vision
        VP[VisionProvider]
        APR[AprilTagProvider]
        LL[Limelight]
    end

    ITD --> ROBOT
    ITD --> AUTO
    ITD --> DC

    ROBOT --> DT
    ROBOT --> TRI
    ROBOT --> SEN

    TRI --> SAM
    TRI --> SPEC

    SAM --> JOINT
    SPEC --> MOTOR

    DT --> MEC
    MEC --> LOC
    LOC --> PP
    LOC --> OTOS

    ROBOT --> VP
    VP --> APR
    ROBOT --> LL

    DC --> STICK
    ROBOT --> POS

    style ROBOT fill:#ffeb3b
    style DT fill:#4caf50
    style TRI fill:#2196f3
    style SEN fill:#9c27b0

Data Flow During a Match

sequenceDiagram
    participant G as Gamepad
    participant DC as DriverControls
    participant R as Robot
    participant T as Trident
    participant A as Arm
    participant M as Motors/Servos

    G->>DC: Button A pressed
    DC->>R: setArticulation(INTAKE)

    loop Update Cycle
        R->>T: update()
        T->>A: update()
        A->>A: Check state, calculate targets
        A->>M: Set motor power / servo position
        M-->>A: Current position feedback
        A-->>T: State complete?
        T-->>R: Articulation complete
    end

    R->>DC: Ready for next command

Practical Examples

Example 1: Creating a New Subsystem

Let’s create an Intake subsystem from scratch:

package org.firstinspires.ftc.teamcode.robots.myrobot.subsystem;

import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;

import java.util.LinkedHashMap;
import java.util.Map;

@Config(value = "INTAKE")
public class Intake implements Subsystem {

    // Configurable constants (can be changed in Dashboard)
    public static double INTAKE_POWER = 0.8;
    public static double EJECT_POWER = -0.6;

    // Hardware
    private DcMotor intakeMotor;

    // State
    public enum State { STOPPED, INTAKING, EJECTING }
    private State state = State.STOPPED;

    // Constructor - called once when robot initializes
    public Intake(HardwareMap hardwareMap) {
        intakeMotor = hardwareMap.get(DcMotor.class, "intake");
        intakeMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
    }

    // Called ~50 times per second
    @Override
    public void update(Canvas fieldOverlay) {
        switch (state) {
            case INTAKING:
                intakeMotor.setPower(INTAKE_POWER);
                break;
            case EJECTING:
                intakeMotor.setPower(EJECT_POWER);
                break;
            case STOPPED:
            default:
                intakeMotor.setPower(0);
                break;
        }
    }

    // Called when OpMode ends
    @Override
    public void stop() {
        intakeMotor.setPower(0);
    }

    // Public methods for DriverControls to call
    public void intake() { state = State.INTAKING; }
    public void eject() { state = State.EJECTING; }
    public void stopIntake() { state = State.STOPPED; }

    // Telemetry for Dashboard
    @Override
    public Map<String, Object> getTelemetry(boolean debug) {
        Map<String, Object> telemetry = new LinkedHashMap<>();
        telemetry.put("State", state.name());
        if (debug) {
            telemetry.put("Motor Power", intakeMotor.getPower());
        }
        return telemetry;
    }

    @Override
    public String getTelemetryName() {
        return "INTAKE";
    }
}

Example 2: Using StickyGamepad for Button Presses

Normal gamepad buttons return true continuously while held. StickyGamepad gives you a single true on press:

public class DriverControls {

    private StickyGamepad stickyGamepad1;
    private Robot robot;

    public DriverControls(Gamepad gamepad1, Robot robot) {
        this.stickyGamepad1 = new StickyGamepad(gamepad1);
        this.robot = robot;
    }

    public void update() {
        // MUST call this every loop to update button states
        stickyGamepad1.update();

        // a returns true ONLY on the frame it's pressed
        if (stickyGamepad1.a) {
            robot.setArticulation(Robot.Articulation.INTAKE);
        }

        // For continuous controls, use the raw gamepad
        double throttle = -stickyGamepad1.gamepad.left_stick_y;
        double strafe = stickyGamepad1.gamepad.left_stick_x;
        double rotation = stickyGamepad1.gamepad.right_stick_x;

        robot.driveTrain.drive(throttle, strafe, rotation);
    }
}

Example 3: Position Caching Between OpModes

Save robot position when TeleOp ends, restore it in the next Autonomous:

// In Robot.java
private PositionCache positionCache;

public Robot(HardwareMap hardwareMap, boolean simulated) {
    // ... other initialization
    positionCache = new PositionCache(hardwareMap.appContext);
}

// Call periodically during operation
public void savePosition() {
    DTPosition position = new DTPosition(
        driveTrain.getPose(),
        shoulder.getCurrentPosition(),
        slide.getCurrentPosition(),
        System.currentTimeMillis()
    );
    positionCache.write(position);
}

// Call during initialization
public void restorePosition() {
    DTPosition cached = positionCache.read();
    if (cached != null && cached.isRecent(5000)) {  // Within 5 seconds
        driveTrain.setPose(cached.getPose());
        shoulder.setPosition(cached.shoulderAngle);
        // ... restore other state
    }
}

Getting Started Checklist

Before You Start Coding

Read through deepthought/Robot.java to see how subsystems are organized
Understand the Subsystem interface (4 methods to implement)
Run an existing OpMode and watch telemetry on FTC Dashboard
Try changing a @Config value while the robot is running

Creating Your First Subsystem

Create a new Java class implementing Subsystem
Add hardware initialization in constructor
Implement update() with your control logic
Implement stop() to safely shut down
Add getTelemetry() for debugging
Register your subsystem in Robot’s subsystems array

Common Mistakes to Avoid

Mistake	Why It’s Bad	What To Do Instead
Calling `Thread.sleep()`	Freezes entire robot	Use state machines with timers
Not calling `update()`	Subsystem won’t work	Add to Robot’s subsystem array
Forgetting `stop()`	Motors may keep running	Always set power to 0
Hardcoding values	Can’t tune without rebuild	Use `@Config` static fields
One giant class	Impossible to debug	Split into focused subsystems

Debugging Tips

Use FTC Dashboard - It shows telemetry in real-time
Add telemetry for EVERYTHING during development
Test subsystems individually before combining
Use @Config to tune values without rebuilding
Check update times - if a subsystem takes too long, the robot becomes unresponsive

Quick Reference

Subsystem Interface Methods

void update(Canvas fieldOverlay)     // Called every loop (~50Hz)
void stop()                          // Called when OpMode ends
Map<String, Object> getTelemetry()   // Return data for dashboard
String getTelemetryName()            // Return section name

Common Hardware Initialization

// Motor
DcMotor motor = hardwareMap.get(DcMotor.class, "motor_name");
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

// Servo
Servo servo = hardwareMap.get(Servo.class, "servo_name");
servo.setPosition(0.5);  // 0.0 to 1.0

// Sensor
DistanceSensor distance = hardwareMap.get(DistanceSensor.class, "sensor_name");
double cm = distance.getDistance(DistanceUnit.CM);

State Machine Template

public enum State { IDLE, STEP_1, STEP_2, DONE }
private State state = State.IDLE;
private ElapsedTime timer = new ElapsedTime();

public void update() {
    switch (state) {
        case IDLE:
            // Wait for trigger
            break;
        case STEP_1:
            doFirstThing();
            if (firstThingDone()) {
                timer.reset();
                state = State.STEP_2;
            }
            break;
        case STEP_2:
            doSecondThing();
            if (timer.seconds() > 0.5) {
                state = State.DONE;
            }
            break;
        case DONE:
            cleanup();
            state = State.IDLE;
            break;
    }
}

Please help support our team! $40 buys a motor, $55 buys a new battery, $150 adds controllers and sensors, $500 pays tournament fees, $750 upgrades our chassis