Diagnostics and Testing

Table of Contents

1. Overview
2. The Diagnostic Problem
3. Hardware Inventory Pattern
   • Lebot2 Hardware Inventory
4. Dependency Hierarchy
   • Layer 0: Hardware Components
   • Layer 1: Subsystem Dependencies
   • Layer 2: Behavior Dependencies
   • Layer 3: Robot-Level Coordination
   • Layer 4: Missions (Composite Autonomous Behaviors)
5. Failure Impact Analysis
6. POST Test Structure
   • Level 0: Hardware Existence
   • Level 1: Hardware Function
   • Level 2: Subsystem Function
   • Level 3: Integration Tests
   • Level 4: Mission Tests
7. Diagnostic OpMode Design
   • Current Implementation
   • Safety Features
8. Competition Checklist

Overview

This document describes Iron Reign’s approach to systematic robot diagnostics. The goal is to quickly identify hardware failures before they manifest as confusing behavior-level bugs.

The core insight: When a robot misbehaves, the failure could be at any level:

• A motor burned out (hardware)
• A sensor cable came loose (hardware)
• A state machine has a logic error (behavior)
• Two subsystems are conflicting over a shared resource (coordination)

Without systematic diagnostics, debugging becomes guesswork. A structured POST (Power-On Self-Test) approach lets us verify each layer before assuming the problem is in the code.

The Diagnostic Problem

The Scenario

Your robot was working yesterday. Today, during autonomous, it drives forward but then spins in circles instead of making a clean turn toward the goal.

Possible causes:

1. Right drive motor failed
2. Left drive motor running at wrong power
3. Pinpoint odometry cable loose (heading data invalid)
4. PID turn gains are wrong (software)
5. Vision isn’t detecting the AprilTag (wrong pipeline?)
6. Battery voltage dropped, affecting motor performance

Without diagnostics, you might spend 30 minutes debugging your PID code when the actual problem is a loose I2C cable.

The Solution: Layered Verification

Test from the bottom up:

1. Hardware exists - Can we talk to the device at all?
2. Hardware functions - Does it respond to direct commands?
3. Subsystem functions - Does the subsystem behavior work in isolation?
4. Integration functions - Do coordinated behaviors work together?
5. Mission functions - Do complete autonomous sequences work?

If Layer 1 fails, don’t bother debugging Layer 3.

Two Initialization Approaches

The diagnostic framework uses two different initialization strategies depending on what’s being tested:

Direct hardware (L0-L1):

// Tests FTC SDK level - proves hardware works regardless of subsystem code
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
leftRear.setPower(0.3);  // Direct SDK call

Robot initialization (L2+):

// Tests real code path - same as competition OpMode
robot = new Robot(hardwareMap, telemetry);
robot.intake.setBehavior(Intake.Behavior.INTAKE);
robot.update();  // Three-phase: readSensors → calc → act

Why this split matters:

• L0-L1 isolation: If someone breaks Loader.java, you can still verify the conveyor motor works
• L2+ integration: Tests use the same hardware names, directions, and configurations as real code
• No maintenance drift: If someone renames “conveyor” to “belt” in Loader.java, L2+ tests automatically follow (via Robot init)

Hardware Inventory Pattern

Before you can test hardware, you need to know what hardware exists. Every robot should have a documented hardware inventory.

What to Document

For each hardware component:

Lebot2 Hardware Inventory

Reference Robot: lebot2 (2025-2026 DECODE season)

Drive System

Notes:

• Drive motors bypass LazyMotor because RoadRunner needs direct control
• leftRear direction is REVERSE so positive power = forward
• Pinpoint provides heading (no separate IMU)

Ball Handling

Notes:

• conveyor direction is REVERSE so negative power = balls toward rear
• conveyor is a shared resource (Intake requests, Launcher claims)
• Distance sensors are Rev 2m sensors, threshold ~10cm for ball detection

Launch System

Notes:

• shooter uses RUN_USING_ENCODER for velocity PID (setVelocity in deg/sec)
• shooter direction is REVERSE
• shooter2 is a slave motor (follower) to double flywheel power - planned addition
• paddle positions: DOWN=0.167, UP=0.447

Vision

Notes:

• Pipeline 0 = Blue alliance, Pipeline 1 = Red alliance
• Uses botpose for field localization and distance calculation
• USB device, not subject to I2C timing constraints

System

Hardware Summary

Dependency Hierarchy

Layer 0: Hardware Components

This is the physical inventory above. Each component can be tested in isolation.

Virtual Sensors

Virtual sensors are computed values derived from one or more physical sensors. They abstract away implementation details from behavior code, allowing the underlying sensor strategy to change without modifying behaviors.

Update Order Matters:

Physical Sensors (Phase 1)
    ↓
Virtual Sensors (early Phase 2)
    ↓
Behaviors use virtual sensor values (Phase 2)
    ↓
Hardware writes (Phase 3)

Virtual sensors must be updated AFTER all physical sensors they depend on have been refreshed.

Current Virtual Sensors

Virtual Sensor Design Pattern

public class Loader implements Subsystem {
    // Physical sensor wrappers
    private CachedDistanceSensor frontSensor;
    private CachedDistanceSensor backSensor;

    // Virtual sensor state (computed in calc())
    private boolean isFull = false;

    @Override
    public void readSensors() {
        // Phase 1: Physical sensor reads only
        frontSensor.refresh();
        backSensor.refresh();
    }

    @Override
    public void calc(Canvas fieldOverlay) {
        // Phase 2a: Update virtual sensors first
        updateVirtualSensors();

        // Phase 2b: Behavior logic uses virtual sensor values
        if (isFull()) {
            // Stop intake
        }
    }

    private void updateVirtualSensors() {
        // Current implementation: count-based
        isFull = (ballCount >= MAX_BALLS);

        // Future: could use time-based confirmation
        // isFull = bothSensorsSeeingBallFor(MIN_CONFIRM_TIME);

        // Future: could use 3rd sensor
        // isFull = frontBall && middleBall && backBall;
    }

    public boolean isFull() {
        return isFull;
    }
}

Benefits of Virtual Sensors

1. Implementation hiding: Behaviors don’t know if isFull uses counting, timing, or extra sensors
2. Easy upgrades: Add a 3rd ball sensor? Only change updateVirtualSensors()
3. Testability: Can inject test values into virtual sensor state
4. Clear update ordering: Virtual sensors always computed before behavior logic

Planned Virtual Sensor: Loader.isFullConfirmed()

Current isFull() is count-based (fast but can drift if counting misses events).

A more robust version could require time-based confirmation:

• Both frontDist and backDist see balls
• Belt has been running
• Condition held for N milliseconds

This guards against transient sensor noise or balls bouncing during loading.

Layer 1: Subsystem Dependencies

Each subsystem depends on specific hardware:

┌─────────────────────────────────────────────────────────────────────────┐
│ SUBSYSTEM          │ HARDWARE DEPENDENCIES                              │
├────────────────────┼────────────────────────────────────────────────────┤
│ TankDrivePinpoint  │ leftRear, rightRear, pinpoint                      │
│                    │ (pinpoint provides: x, y, heading, velocities)     │
├────────────────────┼────────────────────────────────────────────────────┤
│ Intake             │ intake                                             │
│                    │ (also: conveyor via Loader.requestBeltForIntake)   │
├────────────────────┼────────────────────────────────────────────────────┤
│ Loader             │ conveyor, frontDist, backDist                      │
│                    │ (arbitrates belt ownership)                        │
├────────────────────┼────────────────────────────────────────────────────┤
│ Launcher           │ shooter, shooter2, paddle                          │
│                    │ (also: conveyor via Loader.claimBeltForLauncher)   │
│                    │ (also: Vision for distance calculation)            │
├────────────────────┼────────────────────────────────────────────────────┤
│ Vision             │ limelight                                          │
│                    │ (provides: tx, ty, distance, hasBotPose)           │
└─────────────────────────────────────────────────────────────────────────┘

Layer 2: Behavior Dependencies

Each behavior requires specific hardware to be functional:

┌─────────────────────────────────────────────────────────────────────────┐
│ BEHAVIOR                    │ REQUIRES                                  │
├─────────────────────────────┼───────────────────────────────────────────┤
│ TankDrivePinpoint.MANUAL    │ leftRear, rightRear                       │
│ TankDrivePinpoint.PID_TURN  │ leftRear, rightRear, pinpoint (heading)   │
│ TankDrivePinpoint.TRAJECTORY│ leftRear, rightRear, pinpoint (full pose) │
├─────────────────────────────┼───────────────────────────────────────────┤
│ Intake.OFF                  │ (none)                                    │
│ Intake.INTAKE               │ intake motor                              │
│ Intake.LOAD_ALL             │ intake motor, conveyor (via Loader)       │
│                             │ frontDist, backDist (full detection)      │
│ Intake.EJECT                │ intake motor                              │
├─────────────────────────────┼───────────────────────────────────────────┤
│ Launcher.IDLE               │ (none)                                    │
│ Launcher.SPINNING           │ shooter, shooter2, Vision (distance)      │
│ Launcher.fire()             │ shooter, shooter2, paddle, conveyor       │
├─────────────────────────────┼───────────────────────────────────────────┤
│ Vision (always active)      │ limelight                                 │
└─────────────────────────────────────────────────────────────────────────┘

Layer 3: Robot-Level Coordination

Robot behaviors coordinate multiple subsystems:

┌─────────────────────────────────────────────────────────────────────────┐
│ ROBOT BEHAVIOR    │ SUBSYSTEM BEHAVIORS COORDINATED                     │
├───────────────────┼─────────────────────────────────────────────────────┤
│ Robot.MANUAL      │ All subsystems independent, driver controls each    │
├───────────────────┼─────────────────────────────────────────────────────┤
│ Robot.ROLLTAKE    │ Rolling Inake, intake system runs while robot drives│
│                   │ to projected end of current ball group              │
├───────────────────┼─────────────────────────────────────────────────────┤
│ Robot.SHIMYTAKE   │ Inake balls while executing an forward / backward   │
│                   │ angling motion kind of like a W pattern, to gather  │
│                   │ balls in a loose group                              │
├───────────────────┼─────────────────────────────────────────────────────┤
│ Robot.TARGETING   │ TankDrivePinpoint.PID_TURN (to vision tx)           │
│                   │ Vision (provides tx for turn target)                │
├───────────────────┼─────────────────────────────────────────────────────┤
│ Robot.LAUNCH_ALL  │ Launcher.SPINNING → fire() repeatedly               │
│                   │ Loader (belt claimed, ball feeding)                 │
│                   │ Vision (continuous distance updates)                │
│                   │ Intake (suppressed during firing)                   │
└─────────────────────────────────────────────────────────────────────────┘

Layer 4: Missions (Composite Autonomous Behaviors)

Missions are high-level autonomous behaviors that compose lower-level behaviors into complete game objectives. Unlike Robot behaviors which coordinate subsystems for a single action, Missions sequence multiple actions with navigation and decision-making.

Mission Architecture

┌─────────────────────────────────────────────────────────────────────────┐
│ MISSION              │ COMPOSES THESE BEHAVIORS                         │
├──────────────────────┼──────────────────────────────────────────────────┤
│ LaunchPreloads       │ 1. Navigate from start to launch position        │
│                      │ 2. TARGETING (turn to goal AprilTag)             │
│                      │ 3. LAUNCH_ALL (fire all 3 preloaded balls)       │
├──────────────────────┼──────────────────────────────────────────────────┤
│ BALLGroup            │ 1. Navigate to ball group N location             │
│                      │ 2. INTAKE while driving through balls            │
│                      │ 3. Navigate back to launch position              │
│                      │ 4. TARGETING + LAUNCH_ALL                        │
│                      │ 5. Increment N                                   │
├──────────────────────┼──────────────────────────────────────────────────┤
│ OpenSesame           │ 1. Navigate from current position to gate        │
│                      │ 2. Turn around if not facing gate                │
│                      │ 3. Back into gate to release the Classifier      │
├──────────────────────┼──────────────────────────────────────────────────┤
│ Autonomous           │ Primary mission that chains:                     │
│ (primary)            │ LaunchPreloads → Do BallGroups (time permitting) │
└─────────────────────────────────────────────────────────────────────────┘

Mission Dependencies

Missions depend on helper functions and match configuration:

┌─────────────────────────────────────────────────────────────────────────┐
│ HELPER               │ PURPOSE                                          │
├──────────────────────┼──────────────────────────────────────────────────┤
│ MatchConfig          │ Alliance (RED/BLUE), start position              │
│                      │ (AUDIENCE_WALL / GOAL_WALL)                      │
├──────────────────────┼──────────────────────────────────────────────────┤
│ FieldPositions       │ Poses for all 4 starting positions               │
│                      │ Launch position(s), ball group locations         │
│                      │ Goal AprilTag IDs per alliance                   │
├──────────────────────┼──────────────────────────────────────────────────┤
│ NavigationHelper     │ Compute intermediate waypoints                   │
│                      │ Obstacle avoidance paths                         │
│                      │ Alliance-specific mirroring                      │
└─────────────────────────────────────────────────────────────────────────┘

Starting Positions

Mission Chaining and Success Handling

Missions can chain conditionally based on success/failure and available time:

public class Autonomous {
    private int ballGroupIndex = 1;  // Start with group 1

    public void run() {
        // Always attempt preloads first
        boolean preloadsSuccess = launchPreloads.execute();

        // Iterate through ball groups while time permits
        while (preloadsSuccess && timer.seconds() < 25 && ballGroupIndex <= 3) {
            boolean groupSuccess = ballGroup.execute(ballGroupIndex);
            if (groupSuccess) {
                ballGroupIndex++;
            } else {
                break;  // Stop if a group fails
            }
        }

        // End game: release classifier if time permits
        if (timer.seconds() < 28) {
            openSesame.execute();
        }
    }
}

Implementation Location

Missions can be implemented in:

• Autonomous.java - If tightly coupled to autonomous flow
• Missions.java (new class) - If missions should be reusable/testable independently
• Robot.java - If missions are simple enough to be Robot behaviors

Recommendation: Create a separate Missions.java class that:

• Takes Robot reference for subsystem access
• Provides individual mission methods
• Handles mission-level state machines
• Can be tested independently via diagnostics

Failure Impact Analysis

This matrix shows what breaks when each hardware component fails:

Cascading Failures

Some failures cascade through the dependency hierarchy:

pinpoint fails
  → TankDrivePinpoint.PID_TURN fails
    → Robot.TARGETING fails
      → Can't aim at goal
        → Launch accuracy drops (even if Launcher is fine)

backDist fails
  → Loader can't detect ball ready
    → Launcher.fire() timing wrong
      → Robot.LAUNCH_ALL fires at wrong times
        → Balls miss or jam

This is why bottom-up testing matters. Fix Layer 0 problems before debugging Layer 3.

POST Test Structure

Level 0: Hardware Existence

Goal: Verify all devices are connected and responding.

Level 1: Hardware Function (Direct Control)

Goal: Verify each device responds to commands.

Motors

Servos

Sensors

Level 2: Subsystem Function (Behavior Tests)

Goal: Verify each subsystem’s behaviors work correctly.

TankDrivePinpoint

Intake

Loader

Launcher

Vision

Level 3: Integration Tests

Goal: Verify coordinated multi-subsystem behaviors.

Level 4: Mission Tests

Goal: Verify complete autonomous mission sequences work end-to-end.

These tests require significant field space (typically half-field minimum) and are designed for practice sessions rather than pit diagnostics.

Test Environment Requirements:

• Half-field or full-field setup
• AprilTags mounted at correct positions
• Balls available for intake tests
• Timer to verify mission completes within autonomous period (30 seconds)

Diagnostic vs Practice: Mission tests blur the line between diagnostics and practice runs. The key difference:

• Diagnostic mode: Verify the mission can work (single successful run)
• Practice mode: Verify the mission works reliably (multiple runs, gather statistics)

Current Implementation Status

The tables above describe the ideal test procedures. The Lebot2Diagnostics.java implementation uses a dual-initialization approach:

Architecture:

public class Lebot2Diagnostics extends OpMode {
    // L0-L1: Direct hardware references (isolated from subsystem code)
    private DcMotorEx leftRear, rightRear, intake, conveyor, shooter;
    private Servo paddle;
    private DistanceSensor frontDist, backDist;

    // L2+: Full Robot (same init as competition OpMode)
    private Robot robot;
    private boolean robotInitialized = false;

    @Override
    public void init() {
        // Direct hardware always available for L0-L1
        initDirectHardware();
    }

    private void initRobotIfNeeded() {
        if (!robotInitialized && currentTest.level.ordinal() >= TestLevel.LEVEL_2_SUBSYSTEM.ordinal()) {
            robot = new Robot(hardwareMap, telemetry);
            robotInitialized = true;
        }
    }
}

L2 test example (real subsystem):

private void runIntakeBehaviorTest() {
    initRobotIfNeeded();
    robot.intake.setBehavior(Intake.Behavior.INTAKE);
    runRobotUpdateLoop(1.0);  // Run for 1 second

    // Verify via subsystem state
    if (robot.intake.getBehavior() == Intake.Behavior.INTAKE) {
        currentTest.pass("INTAKE behavior active");
    } else {
        currentTest.fail("Behavior not set correctly");
    }
}

What’s NOT yet implemented:

• Mission class - Missions are documented concepts, not working code
• Some Robot behaviors (ROLLTAKE, SHIMYTAKE) - Referenced in docs, may not exist in code

See Roadmap for remaining work.

Diagnostic OpMode Design

Current Implementation

The diagnostic OpMode (Lebot2Diagnostics.java) implements a layered test framework with safety features for competition pit use.

Test Structure

public class DiagnosticTest {
    public final String name;
    public final TestLevel level;
    public final MotionRequirement motion;  // NONE, ELEVATED, GROUND_SHORT, GROUND_MEDIUM
    public final String instruction;        // Shown to operator before test
    public final String[] crossValidation;  // Other hardware that validates this test

    public TestState state;  // NOT_RUN, WAITING_PERMISSION, RUNNING, PASS, FAIL, ABORTED, SKIPPED
    public String resultMessage;
    public double durationMs;
}

Motion Requirements

Tests are tagged with their motion requirements to ensure safety:

Cross-Validation

Tests can validate multiple things simultaneously:

• Drive forward test validates: both encoders change, Pinpoint position changes
• Turn test validates: encoder differential, Pinpoint heading change
• This helps isolate failures (e.g., “encoders work but Pinpoint doesn’t” = I2C issue)

Safety Features

1. Operator Permission Required
   • Tests with motion wait for operator confirmation (A button)
   • Instruction text explains what will happen
   • Operator can skip test (B button) if conditions aren’t safe
2. Universal Abort
   • B button immediately stops all motors during any motion test
   • Test marked as ABORTED, not FAIL (distinguishes operator choice from failure)
3. Operator Instructions
   • Each test shows what to do before running
   • Examples: “ELEVATE ROBOT - wheels must be off ground”
   • Examples: “Robot will drive FORWARD ~30cm. Clear path.”
4. Short Motion Bounds
   • All motion tests limited to <1.5m
   • Designed for pit/practice field constraints
   • Can be run in limited space at competitions

User Interface

• D-pad Up/Down: Select test level
• D-pad Left/Right: Select specific test within level
• A (Green): Run selected test / Confirm proceed
• B (Red): ABORT current test / Skip test
• X (Blue): Run all tests in current level
• Y (Yellow): Run full POST (all levels 0-3)

Output Format

=== LEBOT2 DIAGNOSTICS ===
Level: 1 - HARDWARE FUNCTION
Test: leftRear forward

>>> leftRear spin <<<
State: WAITING_PERMISSION

INSTRUCTION:
ELEVATE ROBOT - wheels must be off ground. Press A when ready.

[A] Proceed   [B] Skip

After tests run:

=== LEBOT2 DIAGNOSTICS ===
Level: 1 - HARDWARE FUNCTION

>> [OK] leftRear spin - Fwd/Rev OK (±50 ticks)
   [OK] rightRear spin - Fwd/Rev OK (±50 ticks)
   [OK] intake spin - Motor ran (visual confirm)
   [!!] shooter spinup - Timeout at 312 deg/s (target 500)
   [--] paddle move - skipped

Summary: 3 PASS, 1 FAIL, 1 SKIP, 5 pending

Test Implementation Patterns

Multi-Phase Tests

Complex tests use a phase state machine:

private void runMotorSpinTest(DcMotorEx motor) {
    switch (testPhase) {
        case 0:  // Start forward
            capturedEncoderStart = motor.getCurrentPosition();
            motor.setPower(0.3);
            testPhase = 1;
            break;
        case 1:  // Wait forward, then reverse
            if (testTimer.seconds() > 0.5) {
                // Check forward result, start reverse
                testPhase = 2;
            }
            break;
        case 2:  // Check reverse result
            // Pass or fail based on encoder changes
            break;
    }
}

Subsystem Tests

Level 2+ tests that need full subsystem initialization are marked as “requires full Robot init” and should be run via TeleOp rather than the standalone diagnostics OpMode.

Competition Checklist

Pre-Match Quick POST (30 seconds)

Run before every match:

• Voltage > 12.5V
• All 5 motors respond to brief pulse (6 when shooter2 added)
• Both distance sensors reading valid range
• Pinpoint heading changes when robot rotated
• Limelight sees AprilTag (if in view)

Pit Diagnostic (5 minutes)

Run after any concerning behavior:

• Full Level 0 (existence)
• Full Level 1 (hardware function)
• Specific Level 2 tests for failing subsystem

Major Reset (15 minutes)

Run after crashes, cable changes, or code updates:

• Full Level 0-3 POST
• Record results for comparison
• Verify against known-good baseline

Future Enhancements

Automated Baseline Recording

Save “known good” test results and compare against them:

// After successful competition
diagnostics.saveBaseline("competition_2026_01_15");

// Before next event
diagnostics.compareToBaseline("competition_2026_01_15");
// Output: "frontDist: was 4.2cm, now 847.3cm - CABLE DISCONNECTED?"

Continuous Background Monitoring

Run lightweight checks during teleop:

// In Robot.update()
if (loopCount % 100 == 0) {  // Every ~2 seconds
    healthCheck.verifyMotorCurrents();
    healthCheck.verifyDistanceSensorsInRange();
    healthCheck.verifyPinpointUpdating();
}

Telemetry Integration

Show health status on dashboard during matches:

HEALTH: [OK] [OK] [OK] [!!] [OK]
        DRV  INT  LDR  LCH  VIS

LCH: shooter velocity low (812 vs 935 target)

Roadmap: Real Subsystem Testing

This roadmap outlines the remaining steps to complete the diagnostic framework.

Architecture Decision: Full Robot Init for L2+

We chose full Robot initialization for L2+ tests rather than partial subsystem init:

Rationale:

• Tests the same code path as competition (no hidden bugs)
• No maintenance drift - hardware names/configs come from subsystems
• L3+ tests require Robot anyway for coordinated behaviors
• L0-L1 still provide isolated hardware testing if subsystem code is broken

Trade-off accepted:

• Slower init for L2+ tests (~2-3 seconds)
• If Robot init fails, L2+ tests can’t run (but L0-L1 still work)

Phase 1: Robot Integration (✅ In Progress)

Goal: L2+ tests use robot.subsystem.setBehavior() instead of direct motor control.

Implementation pattern:

private Robot robot;
private boolean robotInitialized = false;

private void initRobotIfNeeded() {
    if (!robotInitialized) {
        robot = new Robot(hardwareMap, telemetry);
        robotInitialized = true;
    }
}

private void runRobotUpdateLoop(double seconds) {
    ElapsedTime timer = new ElapsedTime();
    while (timer.seconds() < seconds && !isAborted()) {
        robot.update();
    }
}

Status:

• Architecture defined (L0-L1 direct, L2+ via Robot)
• Add Robot field and lazy initialization to diagnostics
• Convert L2 tests to use subsystem behaviors
• Convert L3 tests to use Robot articulations
• Add runRobotUpdateLoop() helper

Phase 2: Create Mission Class

Goal: Implement the Mission class so L4 tests can use real mission code.

File: lebot2/Mission.java (or lebot2/Missions.java)

public class Missions {
    private final Robot robot;

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

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

    private Mission currentMission = Mission.NONE;
    private MissionState state = MissionState.IDLE;
    private int ballGroupTarget = 0;  // For parameterized BALLGroup

    public Missions(Robot robot) {
        this.robot = robot;
    }

    public void startLaunchPreloads() {
        currentMission = Mission.LAUNCH_PRELOADS;
        state = MissionState.RUNNING;
        // Initialize mission state machine
    }

    public void startBALLGroup(int targetBalls) {
        currentMission = Mission.BALL_GROUP;
        ballGroupTarget = targetBalls;
        state = MissionState.RUNNING;
    }

    public void update() {
        if (state != MissionState.RUNNING) return;

        switch (currentMission) {
            case LAUNCH_PRELOADS:
                updateLaunchPreloads();
                break;
            case BALL_GROUP:
                updateBALLGroup();
                break;
            // ...
        }
    }

    public void abort() {
        state = MissionState.ABORTED;
        robot.emergencyStop();
    }

    public MissionState getState() { return state; }
    public Mission getCurrentMission() { return currentMission; }
}

Dependencies:

• Requires Robot behaviors (TARGETING, LAUNCH_ALL) to be working
• Requires path following for navigation portions
• May need field configuration (starting position, ball locations)

Phase 3: Implement Missing Robot Behaviors

Goal: Implement ROLLTAKE, SHIMYTAKE, and other documented behaviors.

Implementation order:

1. TARGETING - Most useful, enables autonomous aiming
2. LAUNCH_ALL - Core game functionality
3. ROLLTAKE - Enhances ball collection
4. SHIMYTAKE - Edge case handling

Phase 4: L3/L4 Test Implementation

Goal: Replace placeholder tests with real behavior/mission invocations.

// L3: Integration test using real behaviors
private void runTargetingTest() {
    robot.setArticulation(Robot.Articulation.TARGETING);

    // Run update loop until targeting completes or timeout
    while (testTimer.seconds() < TARGETING_TIMEOUT) {
        robot.update();

        if (Math.abs(robot.vision.getTx()) < 2.0) {
            currentTest.pass("Targeting converged: tx=" + robot.vision.getTx());
            return;
        }
    }
    currentTest.fail("Targeting timeout");
}

// L4: Mission test using Missions.java
private void runLaunchPreloadsTest() {
    // Start the mission
    robot.missions.startLaunchPreloads();

    while (testTimer.seconds() < MISSION_TIMEOUT) {
        // Robot.update() calls missions.calc() when active
        robot.update(null);

        if (robot.missions.isComplete()) {
            currentTest.pass("LaunchPreloads mission complete");
            return;
        }
        if (robot.missions.isFailed()) {
            currentTest.fail("Mission failed");
            return;
        }
    }
    robot.missions.abort();
    currentTest.fail("Mission timeout");
}

// L4: Ball group mission test
private void runBallGroupTest() {
    robot.missions.startBallGroup(0);  // Test group 0

    while (testTimer.seconds() < MISSION_TIMEOUT) {
        robot.update(null);

        if (robot.missions.isComplete()) {
            currentTest.pass("BallGroup mission complete, balls: " + robot.loader.getBallCount());
            return;
        }
        if (robot.missions.isFailed()) {
            currentTest.fail("BallGroup mission failed");
            return;
        }
    }
    robot.missions.abort();
    currentTest.fail("BallGroup timeout");
}

Implementation Priority

Success Criteria

The diagnostics framework is “complete” when:

• L0-L1 tests use direct hardware (isolated from subsystem code)
• L2+ tests use full Robot initialization (same as competition)
• L2 tests call robot.subsystem.setBehavior() and verify state
• L3 tests use robot.setBehavior() for coordinated behaviors
• L4 tests can invoke robot.missions.* methods (Missions.java implemented)
• All tests validate outcomes via subsystem state/telemetry, not just “it ran”

Appendix: Test Inventory Summary

Last updated: January 2026