Refactoring Lebot

This document chronicles the refactoring of lebot into lebot2, applying the principles from our Coding Guidelines. It serves both as a learning resource and a reference for future refactoring projects.

Table of Contents

1. The Robot: Lebot
2. Problems with the Original Code
3. The Refactoring Plan
4. Implementation Journal
5. Before and After Comparisons
6. Lessons Learned

The Robot: Lebot

Lebot is a ball-launching robot designed for a game where robots collect balls and launch them at targets. Understanding the physical design is essential before discussing code architecture.

Physical Components

┌─────────────────────────────────────────────────────────────┐
│                         ROBOT TOP VIEW                       │
│                                                              │
│   [Left Motor]                           [Right Motor]       │
│       ○                                       ○              │
│    [Flywheel]  ←── Launches balls at calculated speed        │
│        ↑                                                     │
│    [Paddle]    ←── Lifts ball into flywheel (also gate)     │
│        ↑                                                     │
│    [Back Sensor] ←── Detects ball ready to launch           │
│        ↑                                                     │
│   ┌─────────┐                                                │
│   │ LOADER  │  ←── Side belt moves balls toward rear        │
│   │ CHAMBER │      (holds up to 3 balls)                    │
│   └─────────┘                                                │
│        ↑                                                     │
│    [Front Sensor] ←── Detects incoming balls                │
│        ↑                                                     │
│    [Intake]    ←── Overhead belts pull balls in             │
│        ↑                                                     │
│   ══════════════  (funnel opening)                          │
│                                                              │
│                    [Omni Casters]                            │
│                      ○      ○                                │
└─────────────────────────────────────────────────────────────┘

Ball Flow Path

1. Intake: Overhead belts at the front pull balls into the robot
2. Funnel: Guides balls from wide intake into single-file chamber
3. Loader Chamber: Side belt moves balls toward the rear; holds max 3 balls
4. Paddle Gate: When down, stops balls at rear; when up, allows launch
5. Flywheel: Spinning wheel that launches balls when paddle lifts them into contact

Hardware Inventory

Operating Modes

The robot operates in distinct modes during a match:

1. INTAKE - Actively gathering balls
   • Overhead belts ON, side belt ON
   • Paddle DOWN (gate closed)
2. TRANSIT - Driving to launch position
   • All ball handling OFF
   • Driver controls active
3. TARGETING - Aligning with target
   • IMU-based rough turn
   • Limelight fine adjustment
4. LAUNCHING - Firing balls
   • Flywheel spins to calculated speed
   • Paddle lifts ball into flywheel
   • Side belt feeds next ball
   • Repeat until empty

Key Constraints

• Max 3 balls: Game rule AND physical limit
• Flywheel must be at speed: Ball contact before speed = jam
• Speed depends on distance: Calculated from Limelight measurements
• Dual-role components:
  • Side belt: intake helper AND launch feeder
  • Paddle: launch trigger AND loader gate

Problems with the Original Code

The original lebot/Robot.java (746 lines) exhibits several anti-patterns identified in our Coding Guidelines.

1. God Class Anti-Pattern

The Robot class handles everything:

• Drivetrain control
• Intake motors
• Flywheel and paddle
• Limelight vision
• Distance sensors
• Ball counting
• Multiple state machines

Guideline Violated: “Break the robot into subsystems - independent units that each handle one responsibility.”

2. Integer-Based State Machines

// Original code
public int index = 0;
public void shootSequence() {
    switch (index) {
        case 0: // what does 0 mean?
            index++;
            break;
        case 1: // and 1?
            // ...
    }
}

Problems:

• Magic numbers (what is case 4?)
• Hard to read and debug
• Easy to make off-by-one errors

Guideline Violated: “Use enums for state machine states.”

3. Duplicate Hardware Initialization

IMU initialized in both Robot.java AND tankDrive.java:

// In Robot.java
imu = hardwareMap.get(BNO055IMU.class, "imu");

// In tankDrive.java
imu = hardwareMap.get(BNO055IMU.class, "imu");

Guideline Violated: “Each piece of hardware should be owned by exactly one subsystem.”

4. Mixed Sensor Reads and Logic

// Original - reads scattered throughout
public void update(Canvas fieldOverlay) {
    LLResult llResult = limelight.getLatestResult();  // read
    if (llResult != null && llResult.isValid()) {      // logic
        botpose = llResult.getBotpose_MT2();           // more logic
    }
    updateDistance();                                   // another read
    if (shootingAll) {
        shootALLSequence();                            // logic using reads
    }
    // ...
}

Guideline Violated: “Follow the Read→Process→Write pattern.”

5. Direct Dashboard Calls in Telemetry

// Original - side effect in getTelemetry
public Map<String, Object> getTelemetry(boolean debug) {
    // ... build map ...
    dashboard.sendTelemetryPacket(p);  // WRONG! Side effect!
    return telemetry2;
}

Guideline Violated: “Telemetry should be collected, not transmitted, by subsystems.”

The Refactoring Plan

New Architecture

lebot2/
├── Robot.java                    # Coordinator, complex behaviors
├── DriverControls.java           # Gamepad handling
├── Lebot2_6832.java             # Unified OpMode (Auton+TeleOp)
├── subsystem/
│   ├── Subsystem.java           # Interface
│   ├── drivetrain/
│   │   ├── DriveTrainBase.java  # Interface for swappable drivetrains
│   │   └── TankDrive.java       # Differential drive implementation
│   ├── Intake.java              # Overhead belts only
│   ├── Loader.java              # Side belt + sensors + ball counting
│   ├── Launcher.java            # Flywheel + paddle + launch sequence
│   └── Vision.java              # Limelight
└── util/
    └── (shared utilities)

Subsystem Ownership

Design Decisions

1. Swappable Drivetrain: Interface pattern allows TankDrive, MecanumDrive, SwerveDrive
2. Loader owns belt: Side belt’s primary purpose is ball management
3. Launcher owns paddle: Paddle’s critical function is launch triggering
4. Cross-subsystem coordination: Robot class orchestrates multi-subsystem behaviors

Implementation Journal

Step 1: Directory Structure and Interfaces

Completed: January 2026

Created the lebot2 directory structure and copied the Subsystem interface pattern from deepthought.

Files created:

• lebot2/subsystem/Subsystem.java - Base interface extending TelemetryProvider
• lebot2/util/TelemetryProvider.java - Telemetry collection interface

The Subsystem interface:

public interface Subsystem extends TelemetryProvider {
    void update(Canvas fieldOverlay);
    void stop();
    void resetStates();
}

Step 2: DriveTrainBase Interface and TankDrive

Completed: January 2026

Created swappable drivetrain architecture.

Key decisions:

• Interface allows future chassis types (MecanumDrive, SwerveDrive)
• TankDrive ignores strafe parameter (differential drive limitation)
• Heading control via PID for autonomous turns

Files created:

• lebot2/subsystem/drivetrain/DriveTrainBase.java
• lebot2/subsystem/drivetrain/TankDrive.java

DriveTrainBase interface:

public interface DriveTrainBase extends Subsystem {
    void drive(double throttle, double strafe, double turn);
    void setHeadingTarget(double degrees);
    void setLimelightTarget(double tx);
    boolean isTurning();
    double getHeading();
    Pose2d getPose();
    void setPose(Pose2d pose);
}

Step 3: Simple Subsystems (Intake)

Completed: January 2026

The Intake subsystem is the simplest - just overhead belts with on/off control.

Files created:

• lebot2/subsystem/Intake.java

Key pattern: Simple subsystems don’t need state machines. Just provide control methods and let the coordinator decide when to use them.

Step 4: Loader Subsystem (Belt + Sensors + Ball Counting)

Completed: January 2026

The Loader manages ball flow through the robot with two distance sensors.

Files created:

• lebot2/subsystem/Loader.java

Ball counting logic:

public enum LoaderState {
    EMPTY,
    HAS_BALLS,
    FULL
}

// Front sensor: ball entering
// Back sensor: ball ready to launch
// Rising edge detection for accurate counting

Virtual sensors: The Loader exposes virtual sensors - computed values derived from physical sensors that abstract implementation details from behaviors:

The isFull() method uses time-confirmed debouncing to prevent flickering when balls are at sensor thresholds:

// Compute time-confirmed isFull virtual sensor
boolean rawFull = (state == LoaderState.FULL) && ballAtBack;

if (rawFull) {
    if (fullConditionStartMs == 0) {
        fullConditionStartMs = System.currentTimeMillis();
    }
    isFullConfirmed = (System.currentTimeMillis() - fullConditionStartMs) >= FULL_CONFIRM_MS;
} else {
    fullConditionStartMs = 0;
    isFullConfirmed = false;
}

This pattern allows behaviors to simply call loader.isFull() without knowing about sensor thresholds, debounce timing, or the combination of count + sensor confirmation. See Virtual Sensors in Coding Guidelines for more details.

Sensor ownership decision: Loader owns both distance sensors because their primary purpose is ball management, even though the back sensor is read during launch.

Step 5: Launcher Subsystem (Flywheel + Paddle)

Completed: January 2026

The most complex subsystem with a proper enum-based state machine.

Files created:

• lebot2/subsystem/Launcher.java

Launch state machine:

public enum LaunchState {
    IDLE,        // Waiting for command
    SPINNING_UP, // Flywheel ramping to target speed
    READY,       // At speed, waiting for fire command
    FIRING,      // Paddle up, ball launching
    COOLDOWN     // Reset before next ball
}

Distance-based speed calculation: The flywheel speed is calculated from target distance (via Limelight) to ensure consistent arc trajectories.

Step 6: Vision Subsystem

Completed: January 2026

Wraps Limelight3A for target detection.

Files created:

• lebot2/subsystem/Vision.java

Features:

• Pipeline switching for alliance colors
• Target detection with tx/ty values
• Distance calculation from ty angle

Step 7: Robot Coordinator

Completed: January 2026

The Robot class orchestrates multi-subsystem behaviors through articulations.

Files created:

• lebot2/Robot.java

Articulation enum:

public enum Articulation {
    MANUAL,      // Direct driver control
    INTAKE,      // Gathering balls
    TRANSIT,     // Driving to position
    TARGETING,   // Aligning with target
    LAUNCHING,   // Single ball launch
    LAUNCH_ALL,  // Multi-ball sequence
    RELEASE      // Emergency release
}

Cross-subsystem coordination example:

case LAUNCHING:
    if (launcher.getState() == Launcher.LaunchState.READY) {
        launcher.fire();
    }
    if (launcher.getState() == Launcher.LaunchState.COOLDOWN) {
        loader.feedForward();
        articulation = Articulation.MANUAL;
    }
    break;

Step 8: DriverControls and Unified OpMode

Completed: January 2026

Files created:

• lebot2/DriverControls.java - Gamepad input handling
• lebot2/Lebot2_6832.java - Unified OpMode (Auton+TeleOp)

Unified OpMode pattern (from deepthought):

public enum GameState {
    AUTONOMOUS,
    TELE_OP,
    TEST,
    DEMO
}

// Same initialization for all modes
// GameState selected during init_loop
// No controller input during competition autonomous

Step 9: RoadRunner Integration

Completed: January 2026

Integrated RoadRunner v1.0 trajectory following with dual localization options.

Files created:

• lebot2/rr_localize/TankDrive.java - Drive wheel encoder localization
• lebot2/rr_localize/PinpointLocalizer.java - Pinpoint odometry wrapper
• lebot2/rr_localize/TankDrivePinpoint.java - Pinpoint-based localization
• lebot2/rr_localize/Localizer.java (interface from rrQuickStart)
• lebot2/rr_localize/tuning/TuningOpModes.java - Tuning utilities

Dual localization approach: | Class | Localization | Use Case | |——-|————–|———-| | TankDrive | Drive wheel encoders | Testing, comparison baseline | | TankDrivePinpoint | Pinpoint odometry pods | Competition (more accurate) |

Why two options? Drive wheel encoders can slip, especially on mecanum or when pushing. Dedicated odometry pods provide more reliable position tracking.

Motor configuration (for lebot2 chassis):

// Differential drive with rear-mounted motors
leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "leftRear"));
rightMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "rightRear"));

// Left motor reversed so positive power = forward
leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE);

Step 10: SDK Compatibility Fixes

Completed: January 2026

Updated imports for GoBildaPinpointDriver after SDK update.

The issue: The 2024-2025 SDK moved GoBildaPinpointDriver from org.firstinspires.ftc.teamcode to com.qualcomm.hardware.gobilda.

Files fixed (6 total across robots):

• lebot2/rr_localize/tuning/TuningOpModes.java
• deepthought/rr_localize/tuning/TuningOpModes.java
• swervolicious/rr_localize/tuning/TuningOpModes.java
• lebot/PinpointLocalizer.java
• swervolicious/rr_localize/PinpointLocalizer.java
• deepthought/rr_localize/PinpointLocalizer.java

Old import:

import org.firstinspires.ftc.teamcode.GoBildaPinpointDriver;

New import:

import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;

Lesson: When SDK versions change, check for moved/renamed classes in the release notes.

Step 11: Three-Phase Update Pattern

Completed: January 2026

Added a strict three-phase update cycle to eliminate sensor read ordering bugs and enforce clean separation between I2C reads, calculations, and hardware writes.

Problem identified: Student coders could accidentally read stale sensor data or trigger I2C reads in the wrong order. The original update() method mixed sensor reads, logic, and motor writes unpredictably.

Solution: Split the subsystem interface into three explicit phases:

public interface Subsystem extends TelemetryProvider {
    void readSensors();           // Phase 1: I2C reads only
    void calc(Canvas fieldOverlay); // Phase 2: Logic using cached values
    void act();                   // Phase 3: Flush motor/servo commands
    void stop();
    void resetStates();
}

New wrapper classes created (lebot2/util/):

Robot.java update loop:

public void update(Canvas fieldOverlay) {
    // Phase 1: All sensor reads
    for (LynxModule hub : hubs) {
        hub.clearBulkCache();
    }
    for (Subsystem s : subsystems) {
        s.readSensors();
    }

    // Phase 2: All calculations
    // Articulation state machine runs here
    for (Subsystem s : subsystems) {
        s.calc(fieldOverlay);
    }

    // Phase 3: All writes
    for (Subsystem s : subsystems) {
        s.act();
    }
}

MANUAL bulk caching: Changed to LynxModule.BulkCachingMode.MANUAL with explicit clearBulkCache() at the start of each cycle for maximum performance.

Files modified:

• lebot2/subsystem/Subsystem.java - Three-phase interface
• lebot2/Robot.java - MANUAL bulk caching, three-phase loop
• lebot2/subsystem/Intake.java - Uses LazyMotor
• lebot2/subsystem/Loader.java - Uses LazyMotor + CachedDistanceSensor
• lebot2/subsystem/Launcher.java - Uses LazyServo
• lebot2/subsystem/drivetrain/TankDrive.java - Caches IMU heading
• lebot2/subsystem/Vision.java - Minimal changes (Limelight is USB, not I2C)

Key insight: Drive motors are NOT wrapped in LazyMotor because RoadRunner needs direct access for trajectory following. The three-phase pattern applies to subsystem-owned hardware only.

Documentation updated:

• Coding_Guidelines.md - Added “Three-Phase Update Pattern” section with rules for student coders

Step 12: TankDrivePinpoint Subsystem Integration

Completed: January 2026

Refactored TankDrivePinpoint to implement DriveTrainBase, unifying RoadRunner trajectory following with the Subsystem three-phase pattern.

Problem identified: The codebase had two parallel drivetrain implementations:

• subsystem/drivetrain/TankDrive - Simple drivetrain implementing Subsystem, no RoadRunner
• rr_localize/TankDrivePinpoint - RoadRunner drivetrain with Pinpoint, no Subsystem integration

Robot.java was using the simple TankDrive, missing out on RoadRunner’s trajectory capabilities and Pinpoint’s superior odometry.

Solution: Refactor TankDrivePinpoint to implement DriveTrainBase (which extends Subsystem):

public final class TankDrivePinpoint implements DriveTrainBase {
    // RoadRunner infrastructure (Actions, kinematics, PARAMS)
    // + Subsystem three-phase methods
    // + Teleop driving and PID turns
}

Dual-purpose design:

Key changes:

• Added readSensors(): Calls PinpointLocalizer.refresh() for I2C bulk read
• Added calc(): Updates pose estimate, runs turn state machine, draws on field overlay
• Added act(): No-op (RoadRunner Actions write motors directly)
• Added turnToHeading(), turnToTarget(), isTurnComplete(): PID-based turns using Pinpoint heading
• Added drive(): Teleop driving (ignores strafe for tank drive)
• Removed bulk caching setup from constructor (Robot.java handles it)
• Kept all RoadRunner infrastructure intact

Files modified:

• lebot2/rr_localize/TankDrivePinpoint.java - Now implements DriveTrainBase
• lebot2/rr_localize/TankDrive.java - Also implements DriveTrainBase (encoder fallback)
• lebot2/Robot.java - Uses TankDrivePinpoint instead of subsystem TankDrive
• lebot2/rr_localize/tuning/TuningOpModes.java - Added TankDrivePinpoint case

Drivetrain hierarchy (in order of preference):

1. rr_localize/TankDrivePinpoint - RoadRunner + Pinpoint odometry (most accurate)
2. rr_localize/TankDrive - RoadRunner + drive wheel encoders (fallback if Pinpoint fails)
3. subsystem/drivetrain/TankDrive - Simple drivetrain, no RoadRunner (last resort)

Tuning compatibility preserved: TuningOpModes can still access all the public fields it needs:

TankDrivePinpoint td = new TankDrivePinpoint(hardwareMap, new Pose2d(0, 0, 0));
// Access td.leftMotors, td.rightMotors, td.localizer, TankDrivePinpoint.PARAMS

Fallback option: The original subsystem/drivetrain/TankDrive remains as a no-Pinpoint fallback if the odometry pod is damaged.

Step 13: Parallel Control Domain Architecture

Completed: January 2026

Refactored the robot from a monolithic articulation system to a parallel control domain architecture where each subsystem manages its own behaviors independently.

Problem identified: The original articulation system blocked drive during intake operations. A code review revealed:

• When Robot was in INTAKE articulation, drive was disabled
• Intake and drive use completely different hardware - no reason they can’t run in parallel
• The monolithic articulation enum tried to handle too many combinations
• A double drive() call bug overwrote throttle when turning

Resource Conflict Analysis

Before designing the solution, we systematically analyzed which hardware resources each subsystem uses:

The only true conflict: Belt Motor

The belt is the only hardware shared between subsystems:

• Intake mode: Belt pulls balls toward rear
• Launch mode: Belt feeds balls to paddle (same direction)

Interestingly, they run the same direction - the conflict is about ownership and stopping logic, not direction. When launcher fires, it needs exclusive belt control to time ball feeding.

What can run in parallel?

Key insight: Most combinations CAN run in parallel. The monolithic articulation was overly restrictive.

Solution: Parallel control domains where each subsystem manages its own state machine:

┌─────────────────────────────────────────────────────────────┐
│ PARALLEL CONTROL DOMAINS                                    │
├─────────────────────────────────────────────────────────────┤
│ DriveTrain     │ MANUAL, TRAJECTORY, PID_TURN               │
│                │ (interrupted by joystick input)            │
├────────────────┼────────────────────────────────────────────┤
│ Intake         │ OFF, INTAKE, LOAD_ALL, EJECT               │
│                │ (auto-completes when loader full)          │
├────────────────┼────────────────────────────────────────────┤
│ Loader         │ Manages belt ownership (INTAKE > LAUNCHER) │
│                │ (shared resource arbitration)              │
├────────────────┼────────────────────────────────────────────┤
│ Launcher       │ IDLE, SPINNING (auto-pulls vision dist)    │
│                │ (claims belt when firing)                  │
├────────────────┼────────────────────────────────────────────┤
│ Robot          │ MANUAL, TARGETING, LAUNCH_ALL              │
│                │ (multi-subsystem coordination only)        │
└─────────────────────────────────────────────────────────────┘

Unified Behavior Pattern: All subsystems now use consistent API:

// Every subsystem has:
public enum Behavior { ... }
public void setBehavior(Behavior b) { ... }
public Behavior getBehavior() { ... }

// Examples:
robot.setBehavior(Robot.Behavior.LAUNCH_ALL);
launcher.setBehavior(Launcher.Behavior.SPINNING);
// DriveTrain sets behavior implicitly via drive(), runAction(), turnToHeading()

Belt Ownership Model: The Loader arbitrates belt access between competing subsystems:

// Priority: LAUNCHER > INTAKE > NONE
public enum BeltOwner { NONE, INTAKE, LAUNCHER }

// Intake requests (can be preempted)
loader.requestBeltForIntake();
loader.releaseBeltFromIntake();

// Launcher claims (highest priority)
loader.claimBeltForLauncher();
loader.releaseBeltFromLauncher();

// In calc(): resolve ownership and set belt power
if (launcherClaimsBelt) {
    currentOwner = BeltOwner.LAUNCHER;
    beltMotor.setPower(launcherBeltPower);
} else if (intakeRequestsBelt) {
    currentOwner = BeltOwner.INTAKE;
    beltMotor.setPower(INTAKE_FEED_POWER);
} else {
    currentOwner = BeltOwner.NONE;
    beltMotor.setPower(0);
}

Key architectural decisions:

Files modified:

• Intake.java - Added Behavior enum, loadAll() auto-stops when full
• Loader.java - Added BeltOwner tracking, priority resolution in calc()
• Launcher.java - Added Behavior enum, auto-pulls distance from Vision
• TankDrivePinpoint.java - Renamed DriveMode to Behavior
• Robot.java - Renamed Articulation to Behavior, simplified to multi-subsystem only
• DriverControls.java - Drive always called, uses new behavior APIs
• Autonomous.java - Updated to use new behavior APIs

Bug fixed: Double drive() call in joystickDrive() that overwrote throttle when turning.

Lessons learned:

• Subsystems using different hardware should operate independently
• Robot-level behaviors should only coordinate multi-subsystem sequences
• Shared resources need explicit ownership models (like belt claiming)
• Unified API patterns (setBehavior()/getBehavior()) make code predictable

Before and After Comparisons

State Machine: Integer vs Enum

Before (lebot/Robot.java):

public int index = 0;
public void shootSequence() {
    switch (index) {
        case 0:
            index++;
            break;
        case 1:
            shooter.setVelocity(minShooterSpeed, AngleUnit.DEGREES);
            index++;
            break;
        // ... cases 2, 3, 4, 5 ...
    }
}

After (lebot2/subsystem/Launcher.java):

public enum LaunchState {
    IDLE,
    SPINNING_UP,
    READY,
    FIRING,
    COOLDOWN
}

private LaunchState state = LaunchState.IDLE;

@Override
public void update(Canvas fieldOverlay) {
    switch (state) {
        case IDLE:
            // Clear state, nothing to do
            break;
        case SPINNING_UP:
            if (isFlywheelAtSpeed()) {
                state = LaunchState.READY;
            }
            break;
        // ... etc
    }
}

Why it’s better:

• Self-documenting (SPINNING_UP vs case 1)
• IDE autocomplete support
• Compiler catches typos
• Easy to add/remove states

Lessons Learned

1. Hardware Ownership Matters

Deciding which subsystem owns dual-role components requires understanding the highest-priority use case.

Example: The paddle servo could belong to either Loader or Launcher:

• Loader perspective: It’s a gate that stops balls
• Launcher perspective: It’s the trigger that fires balls

Decision: Launcher owns it because the timing-critical function (coordinating with flywheel speed) is more important than the passive gate function.

2. State Machines Need Clear Entry/Exit Conditions

Each state transition should have documented:

• What condition triggers entry
• What actions happen on entry
• What condition triggers exit

Anti-pattern: States that transition “after a delay” without explaining why.

Good pattern:

case SPINNING_UP:
    // Entry: prepareToLaunch() called with distance
    // Exit: Flywheel reaches target velocity (±5%)
    if (isFlywheelAtSpeed()) {
        state = LaunchState.READY;
    }
    break;

3. Cross-Subsystem Coordination Belongs in Robot

A subsystem should never directly call methods on another subsystem. The Robot coordinator handles all multi-subsystem behaviors.

Why? If Launcher directly called loader.feedForward(), then:

• Launcher needs a reference to Loader
• Testing Launcher requires mocking Loader
• The dependency graph becomes tangled

Better: Robot watches both subsystems and triggers the feed when appropriate.

4. Swappable Interfaces Enable Experimentation

The DriveTrainBase interface lets us test different chassis configurations:

• TankDrive for the ball launcher
• MecanumDrive for holonomic movement
• SwerveDrive for ultimate flexibility

Same game code, different locomotion.

5. Keep Original Code As Reference

We kept lebot/ intact while building lebot2/. This provided:

• A working baseline if refactoring broke things
• Side-by-side comparison for learning
• Easy reference for hardware names and values

Don’t delete the old code until the new code is competition-tested.

6. Dual Localization Options Are Worth The Effort

Creating both TankDrive (encoder-based) and TankDrivePinpoint variants:

• Allows comparison testing on the actual robot
• Provides fallback if Pinpoint has issues
• Teaches team members how localization works

The small extra effort pays off in debugging time saved.

7. SDK Updates Break Things

The GoBildaPinpointDriver package move (org.firstinspires.ftc.teamcode → com.qualcomm.hardware.gobilda) broke 6 files across 3 robots.

Lesson: When updating SDK versions:

1. Read the release notes fully
2. Search codebase for any imported classes that moved
3. Test build immediately after SDK update
4. Commit SDK update separately from other changes

8. Unified OpMode Reduces Duplication

Having one OpMode that handles both Autonomous and TeleOp:

• Single initialization path (less bugs)
• Easy mode switching for testing
• Shared telemetry dashboard
• Natural transition from auton to teleop

The GameState enum makes this clean and explicit.

9. Virtual Sensors Abstract Physical Implementation

Virtual sensors are computed values derived from physical sensors that abstract implementation details from behavior code.

Why this matters: Initially, isFull() just checked ballCount >= MAX_BALLS. But this flickered when balls were at sensor thresholds or the count was momentarily wrong. The fix required:

• Combining ball count WITH back sensor confirmation
• Adding time-based debouncing (100ms stable before confirming)

Without virtual sensors, this fix would require updating every place that checked fullness. With virtual sensors, we changed one method and all behaviors automatically got the improved logic.

Pattern:

// Virtual sensor state
private boolean isFullConfirmed = false;
private long fullConditionStartMs = 0;

// In calc() - compute virtual sensor
boolean rawFull = (state == LoaderState.FULL) && ballAtBack;
if (rawFull) {
    if (fullConditionStartMs == 0) fullConditionStartMs = System.currentTimeMillis();
    isFullConfirmed = (System.currentTimeMillis() - fullConditionStartMs) >= FULL_CONFIRM_MS;
} else {
    fullConditionStartMs = 0;
    isFullConfirmed = false;
}

// Accessor - behaviors call this
public boolean isFull() { return isFullConfirmed; }
public boolean isFullRaw() { return state == LoaderState.FULL; }  // For debugging

Benefits:

1. Single source of truth - One definition of “full” used everywhere
2. Easy to tune - Change FULL_CONFIRM_MS in one place
3. Behavior code stays simple - Just calls loader.isFull()
4. Testable - Can mock virtual sensor values independently

Other virtual sensors in lebot2:

• Pose2d from Pinpoint odometry
• Vision.getDistanceToGoal() from Limelight + AprilTags

10. Separate Strategy from Tactics with Missions

When building autonomous routines, we discovered a natural split between strategic and tactical concerns:

Strategic (Autonomous.java):

• Which missions to run
• What order to run them
• Adapting to starting position, partner agreements, opponent analysis

Tactical (Missions.java):

• How to execute each mission
• RoadRunner trajectory building
• State machine for each mission type

Why this split matters:

Initially, we put everything in Autonomous.java. The state machine grew unwieldy:

• Navigation code mixed with strategy decisions
• Hard to reuse ball collection logic
• Can’t call autonomous behaviors from TeleOp

The Missions layer solved these problems:

┌────────────────────────────────────────┐
│ Autonomous.java (Strategic)            │
│   "Collect groups 0, 1, 2 then release"│
└───────────────┬────────────────────────┘
                │ triggers
                ▼
┌────────────────────────────────────────┐
│ Missions.java (Tactical)               │
│   "How to navigate to group 0"         │
│   "How to release classifier"          │
└───────────────┬────────────────────────┘
                │ uses
                ▼
┌────────────────────────────────────────┐
│ Robot.Behavior / Subsystem.Behavior    │
│   "TARGETING", "LAUNCH_ALL", etc.      │
└────────────────────────────────────────┘

Key design decisions:

1. Missions is a Robot field, not a subsystem - Because missions coordinate subsystems, not hardware
2. calc() only when active - if (missions.isActive()) missions.calc() conserves compute
3. Dual-use from Auto and TeleOp - Drivers can trigger robot.missions.startBallGroup(0) during matches
4. Configurable order - setBallGroupOrder({2, 1, 0}) for different starting positions

Pattern for adding new missions:

// 1. Add to enum
public enum Mission { NONE, LAUNCH_PRELOADS, BALL_GROUP, OPEN_SESAME, NEW_MISSION }

// 2. Add mission-specific state enum
private enum NewMissionState { IDLE, STEP_1, STEP_2, COMPLETE }

// 3. Add start method
public void startNewMission(/* params */) {
    currentMission = Mission.NEW_MISSION;
    missionState = MissionState.RUNNING;
    newMissionState = NewMissionState.IDLE;
}

// 4. Add case in calc()
switch (currentMission) {
    case NEW_MISSION: updateNewMission(); break;
}

// 5. Implement state machine
private void updateNewMission() {
    switch (newMissionState) {
        case IDLE: /* build trajectory, start */ break;
        case STEP_1: /* wait for trajectory */ break;
        case STEP_2: /* activate subsystem behaviors */ break;
        case COMPLETE: missionState = MissionState.COMPLETE; break;
    }
}

Next Steps

Remaining work to complete the refactoring:

1. Configure Pinpoint PARAMS - Measure pod offsets for lebot2 chassis
2. Tune feedforward gains - Run RoadRunner tuning routines
3. Implement autonomous routines ✓ - Missions.java + Autonomous.java implemented
4. Field test missions - Test BallGroup, OpenSesame, LaunchPreloads on actual field
5. Measure field positions - Replace placeholder coordinates in Missions.java with real field measurements
6. Implement GO_BALL_CLUSTER - Vision-based ball cluster pickup after classifier release
7. Field test localization - Compare TankDrive vs TankDrivePinpoint accuracy
8. Remove lebot/ - Once lebot2 is proven in competition

Last updated: January 2026