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
}

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

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

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

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

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

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

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

@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;
}

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

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

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;

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

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

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

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

@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
}

@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
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@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 ...
    }
}

@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 ...
    }
}

@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 ...
    }
}

@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 ...
    }
}

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

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";
    }
}

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

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

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

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

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