Lesson 2: Subsystem Basics

This lesson teaches the core architectural pattern that TEAM1771 uses for most subsystems: state machines driven by commands. You'll learn how commands request state changes (rather than directly controlling hardware), how triggers coordinate these requests based on controller input, and how to handle conflicting inputs gracefully.

If that sounds like a lot of mumbo jumbo, just know that you'll be learning how to make a simple subsystem.

Learning Objectives

By the end of this lesson, you will understand:

• ✅ The mental model: desired state vs. direct control
• ✅ How to structure a TEAM1771 subsystem with state machine pattern
• ✅ How to create command factories (setDesiredStateCmd, runSubsystemCmd)
• ✅ How to bind PS5 controller triggers with WhileTrue and OnFalse
• ✅ How to compose and resolve conflicting trigger inputs
• ✅ Why emergency stop uses state machine (not .stop() calls)

Prerequisites

Before starting this lesson:

• Complete Lesson 0: Setup - WPILib installation
• Complete Lesson 1: C++ Basics - C++ fundamentals
• Review Style Guide - Architecture patterns
• Review Controller Map - Button conventions

Mental Model: Desired State vs. Direct Control

The Problem with Direct Control

In simple robot code, you might be tempted to write:

// ❌ BAD - Direct control from button press
m_controller.R2().OnTrue(frc2::cmd::RunOnce([this] {
  m_intakeMotor.Set(0.8);  // Turn on motor
}));

m_controller.R2().OnFalse(frc2::cmd::RunOnce([this] {
  m_intakeMotor.Set(0.0);  // Turn off motor
}));

It should be fairly easy to understand what's going on here. When the right trigger (R2) on the controller is pressed, it turns the intake motor on at 80% speed. When the trigger is let go, the intake goes back to 0% speed.

This approach has serious problems:

1. No centralized logic: Motor control is scattered across multiple button handlers
2. Hard to debug: Where is the motor getting its speed from? Multiple places!
3. Race conditions: What if two buttons both try to control the same motor?
4. No telemetry: How do you display "what state is the intake in" on the dashboard?
5. Emergency stop is fragile: You'd have to call .Set(0) on every motor individually

The 1771 Solution: State Machines

Instead, we use a state machine where:

1. Commands request state changes (e.g., "I want to be INTAKING")
2. The subsystem stores the desired state (e.g., m_desiredState = State::kIntaking)
3. A default command runs continuously and calls processState()
4. processState() switches on the state and executes the appropriate logic

// ✅ GOOD - State-based control
m_controller.R2().OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kIntaking))
                 .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

Now:

• ✅ All motor logic is in one place (processState())
• ✅ Easy to debug: check m_currentState and m_desiredState
• ✅ Telemetry shows the current state
• ✅ Emergency stop just sets m_desiredState = State::kOff
• ✅ Multiple buttons can safely request state changes

*In case you're curious, m_ denotes that m_controller is a member variable of the class. Also, k denotes that State::kOff is a constant. These are optional markers that are frequently used in FRC code to make it easier to read code.

Anatomy of a 1771 Subsystem

Let's break down the key components of our subsystem pattern using a simulated intake as an example.

1. State Enumeration

Define clear states for your subsystem:

enum class State {
  kOff,        // Motors stopped, roller speed = 0
  kIntaking,   // Running intake roller inward
  kOuttaking   // Running intake roller outward
};

Best Practices:

• Use descriptive names (not just ON/OFF)
• Use enum class (not plain enum) for type safety
• Keep states simple and discrete
• Start with k prefix (WPILib convention for constant/enum)

2. State Snapshot

Provide a thread-safe way to read subsystem state:

struct StateSnapshot {
  State currentState;
  double rollerSpeed;
};

StateSnapshot getState() const {
  return {
    .currentState = m_currentState,
    .rollerSpeed = m_rollerSpeed
  };
}

Why snapshots?

• Telemetry can safely read state without race conditions
• Returns all relevant values at once (atomic snapshot)
• Const method = doesn't modify subsystem

3. Telemetry Registration

Allow RobotContainer to register telemetry callbacks

• Telemetry is data reporting, so it could be publishing a sensor reading to be read in Elastic, or maybe passing along the subsystem state to another subsystem (i.e. the shooter spins up when the intake has finished intaking a gamepiece)

void registerTelemetry(std::function<void()> callback) {
  m_telemetryCallbacks.push_back(callback);
}

void Periodic() override {
  // ONLY call telemetry callbacks - no control logic!
  for (auto& callback : m_telemetryCallbacks) {
    callback();
  }
}

Key Points:

• Periodic() is for telemetry ONLY
• No motor control, no state transitions in Periodic()
• Callbacks registered in RobotContainer

4. setDesiredStateCmd - Request State Change

This command factory returns a one-shot command (runs only once) that sets the desired state:

frc2::CommandPtr setDesiredStateCmd(State state) {
  return frc2::cmd::RunOnce([this, state] {
    m_desiredState = state;
  }, {this});
}

Characteristics:

• Uses frc2::cmd::RunOnce() (runs once, then ends immediately)
• Captures this and state in lambda
• Requires {this} as subsystem requirement (for scheduling)
• Does not control hardware directly

Lambdas are a little bit confusin but are essentially just another way to define a function. They are very frequently used in FRC code to define what a command actually does. If you want to learn more about how lambdas work, please watch this.

5. runSubsystemCmd - Default Command

This command factory returns a continuously-running command that drives the state machine:

frc2::CommandPtr runSubsystemCmd() {
  return frc2::cmd::Run([this] {
    processState();
  }, {this});
}

Characteristics:

• Uses frc2::cmd::Run() (runs forever until interrupted)
• Calls processState() every robot loop (~50Hz or about every 20ms)
• Set as default command in RobotContainer
• This is what actually controls the hardware

6. processState - State Machine Logic

The core state machine implementation:

void processState() {
  // 1. Transition to desired state
  if (m_currentState != m_desiredState) {
    m_currentState = m_desiredState;
  }

  // 2. Execute state-specific logic
  switch (m_currentState) {
    case State::kOff:
      handleOffState();
      break;
    case State::kIntaking:
      handleIntakingState();
      break;
    case State::kOuttaking:
      handleOuttakingState();
      break;
  }
}

void handleIntakingState() {
  m_rollerSpeed = 0.8;  // Simulate motor running at 80%
}

void handleOuttakingState() {
  m_rollerSpeed = -0.6;  // Simulate motor running reverse at 60%
}

void handleOffState() {
  m_rollerSpeed = 0.0;  // Simulate motor stopped
}

Key Points:

• All hardware control logic is centralized here
• State transitions happen in one place
• Easy to add logging, delays, or complex logic
• Each state has its own handler method

Trigger Binding Patterns

Now let's look at how to bind controller inputs to state changes using WPILib's trigger system.

Pattern 1: WhileTrue + OnFalse (Hold-to-Run)

The most common pattern for TEAM1771 is hold-to-run:

// R2: Hold to intake, release returns to off
m_driverController.R2()
  .OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kIntaking))
  .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

How it works:

• OnTrue: Schedules the command when trigger becomes true (R2 pressed)
• OnFalse: Schedules the command when trigger becomes false (R2 released)
• Result: State is kIntaking while held, returns to kOff when released

Alternative: WhileTrue

You can also use WhileTrue to schedule a command that runs continuously while held:

// Equivalent behavior using WhileTrue
m_driverController.R2()
  .WhileTrue(m_intake.setDesiredStateCmd(SimIntake::State::kIntaking)
                     .AndThen(m_intake.runSubsystemCmd()))
  .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

For simple state changes, OnTrue + OnFalse is cleaner.

Pattern 2: Composing Triggers with And/Or

You can combine multiple conditions:

// Only allow intaking if robot is enabled AND intake is not already full
m_driverController.R2()
  .And(frc2::Trigger([this] {
    return !m_intake.isFull();  // Custom condition
  }))
  .OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kIntaking))
  .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

// Run outtake if EITHER L2 is held OR auto-outtake flag is set
m_driverController.L2()
  .Or(frc2::Trigger([this] {
    return m_autoOuttakeEnabled;
  }))
  .OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kOuttaking))
  .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

Pattern 3: Resolving Conflicting Inputs

What if the driver presses both R2 AND L2 simultaneously?

Option A: One trigger wins (precedence)

// L2 (outtake) has precedence over R2 (intake)
(m_driverController.R2()
  && !m_driverController.L2())  // Only if L2 is NOT pressed
  .OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kIntaking))
  .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

m_driverController.L2()
  .OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kOuttaking))
  .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

Now:

• If both pressed: Outtaking (L2 wins)
• If only R2 pressed: Intaking
• If neither pressed: Off

Option B: Both pressed = safe state

// If both pressed, go to Off state
auto bothPressed = m_driverController.R2() && m_driverController.L2();

bothPressed.OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

(m_driverController.R2()
  && !bothPressed)  // Only if both are NOT pressed
  .OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kIntaking))
  .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

(m_driverController.L2()
  && !bothPressed)
  .OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kOuttaking))
  .OnFalse(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

Which to choose?

• Use precedence (Option A) when one action is clearly more important (e.g., outtake to prevent jams)
• Use safe state (Option B) when you want to prevent operator error
• Document your choice in comments and README!

Pattern 4: Emergency Stop (Cross Button)

The emergency stop button sets all subsystems to safe states:

// Emergency stop - sets subsystem to Off via state machine
m_driverController.Cross()
  .OnTrue(m_intake.setDesiredStateCmd(SimIntake::State::kOff));

Why not call .stop() directly?

// ❌ BAD - Bypasses state machine
m_driverController.Cross().OnTrue(frc2::cmd::RunOnce([this] {
  m_intakeMotor.Set(0);  // Direct motor control
}));

This is bad because:

• Telemetry still shows the subsystem in its previous state (misleading!)
• State machine might immediately try to turn motor back on
• No clean transition or logging
• Harder to debug what happened

Using setDesiredStateCmd(State::kOff) is better because:

• ✅ State machine handles the transition properly
• ✅ Telemetry accurately reflects the state
• ✅ Consistent with how all other state changes work
• ✅ Easy to extend (e.g., log "emergency stop activated")

Hands-On: Project 02

Open projects/02-commands-state-machines/ in WPILib VS Code and explore the code:

1. Study SimIntake.h and SimIntake.cpp:

   • Find the state enum
   • Trace how setDesiredStateCmd() works
   • Finish runSubsystemCmd() and handleIntakingState() (marked with TODO:)
   • Finish the State enum in SimIntake.h (also marked with TODO:)
   • Follow runSubsystemCmd() → processState() → state handlers

2. Study RobotContainer.cpp:

   • Understand button bindings
   • Finish the L2 (Button 2) binding (marked with TODO:)
   • See how conflicting inputs are resolved
   • Find the Cross (Button 3) button emergency stop
   • Finish code for publishing state.rollerSpeed (also marked)

3. Run in simulation:

   • Build and simulate the project (ensure SimGUI opens)
   • Open Elastic dashboard and copy over the SimIntake widgets to your dashboard
   • Make sure SimGUI is in focus, robot is in teleoperated and keyboard 0 is set as controller 0
   • Press Z and watch SimIntake/State change to "INTAKING"
   • Press X and watch it change to "OUTTAKING"
   • Try pressing both - which wins?
   • Press C - verify it returns to "OFF"
   • Watch SimIntake/RollerSpeed change with different states

   Tip: You can also see the same values in SimGUI if you look in the NetworkTables widget under SmartDashboard

Checkpoints & Questions

Before moving on, make sure you can answer these questions:

State Machine Fundamentals

1. What's the difference between m_currentState and m_desiredState?

   Answer

   • m_desiredState: What state a command has requested
   • m_currentState: What state the subsystem is actually in
   • processState() transitions m_currentState to match m_desiredState

2. Why do we use setDesiredStateCmd() instead of directly setting a motor speed in the button binding?

   Answer

   • Centralizes control logic in one place (processState())
   • Makes telemetry accurate (state is always known)
   • Prevents race conditions from multiple buttons
   • Makes emergency stop simple (just set state to Off)
   • Easier to debug (state is visible on dashboard)

3. What should go in Periodic() and what should NOT?

   Answer

   Should go in Periodic():

   • Calling telemetry callbacks (publishing to SmartDashboard)
   • Reading sensor values (if needed for telemetry)

   Should NOT go in Periodic():

   • Motor control
   • State transitions
   • Decision logic
   • Anything that affects subsystem behavior

Command Factories

4. What does frc2::cmd::RunOnce() do? When do we use it?

   Answer

   • Runs the lambda once, then immediately finishes
   • Used for setDesiredStateCmd() to request state changes
   • Fires once per button press/release

5. What does frc2::cmd::Run() do? When do we use it?

   Answer

   • Runs the lambda continuously (never ends on its own)
   • Used for runSubsystemCmd() to drive the state machine
   • Executes every robot loop (~50Hz) as the default command

6. Why does runSubsystemCmd() need to be set as a default command?

   Answer

   • Default commands run continuously when no other command is using the subsystem
   • This ensures processState() is always being called
   • Without it, state transitions wouldn't happen!

Trigger Bindings

7. What's the difference between OnTrue() and WhileTrue()?

   Answer

   • OnTrue(): Schedules command once when trigger becomes true
   • WhileTrue(): Schedules command that runs continuously while trigger is true, cancels when false
   • For simple state changes, OnTrue() + OnFalse() is simpler

8. How do we make L2 take precedence over R2 when both are pressed?

   Answer

   // R2 only works if L2 is NOT pressed
   (m_controller.R2() && !m_controller.L2())
     .OnTrue(...)
   
   // L2 always works
   m_controller.L2().OnTrue(...)

9. Why does the emergency stop button (Cross) use setDesiredStateCmd(State::kOff) instead of calling a motor's .Set(0) method?

   Answer

   • Keeps state machine consistent (state matches reality)
   • Telemetry shows accurate state
   • Easier to extend (can add logging, cleanup, etc.)
   • Prevents state machine from fighting the direct control
   • Follows the same pattern as all other state changes

Stretch Goals

After completing the basic lesson, try these challenges:

Challenge 1: Add a Third State

Add a new state kHolding that maintains the current roller position without actively spinning:

1. Add kHolding to the state enum
2. Implement handleHoldingState() (set roller speed to 0 but don't reset counters)
3. Bind V (Button 4) to activate holding state
4. Test in simulation

Challenge 2: Add State Timeout

Modify the intake to automatically turn off after 3 seconds in kIntaking state:

1. Add a units::second_t m_stateTimer member variable
2. Reset timer when entering kIntaking state
3. Increment timer in handleIntakingState()
4. If timer > 3 seconds, set m_desiredState = State::kOff
5. Publish timer value to telemetry
6. Test by holding R2 and watching it auto-stop after 3 seconds

Challenge 3: Add Simulated Sensor

Add a simulated "game piece detected" sensor:

1. Add bool m_gamePieceDetected member variable
2. In handleIntakingState(), if roller has been running > 1 second, set m_gamePieceDetected = true
3. In handleOffState(), reset m_gamePieceDetected = false
4. Publish sensor value to telemetry
5. Modify R2 binding to stop intaking if game piece detected (use && with custom trigger)
6. Test and verify intake stops automatically when piece detected

Challenge 4: Multi-Subsystem Emergency Stop

Add a second subsystem (e.g., SimShooter) and make the Cross button stop both:

1. Create a new subsystem with its own states
2. Add it to RobotContainer
3. Modify Cross button binding to set both subsystems to safe states (use .AndThen())
4. Test that one button press stops everything

Challenge 5: Advanced Conflict Resolution

Implement "smart conflict resolution" for R2 vs L2:

1. If R2 pressed first, ignore L2 presses (lock out)
2. If L2 pressed first, ignore R2 presses (lock out)
3. When both released, clear the lockout
4. Hint: Use member variables to track which was pressed first
5. Add telemetry to show which trigger has priority

Key Takeaways

🎯 Mental Model:

• Commands request state changes, they don't control hardware directly
• State machine processes the desired state and controls hardware in one place
• This pattern makes code predictable, debuggable, and maintainable

🎯 Subsystem Structure:

• State enum defines possible states
• setDesiredStateCmd() requests state changes (RunOnce)
• runSubsystemCmd() drives state machine (Run, default command)
• processState() switch statement implements state logic
• Periodic() is for telemetry ONLY

🎯 Trigger Bindings:

• OnTrue() + OnFalse() for hold-to-run pattern
• && (and) / || (or) / ! (negate) for complex conditions
• Resolve conflicts explicitly (precedence or safe state)
• Emergency stop uses state machine, not direct .stop() calls

🎯 Benefits:

• ✅ All control logic in one place
• ✅ Easy to debug (state is visible)
• ✅ Telemetry always accurate
• ✅ No race conditions
• ✅ Emergency stop is simple and reliable

Next Steps

1. ✅ Complete the hands-on project 02-commands-state-machines
2. ✅ Answer all checkpoint questions (test your understanding!)
3. ✅ Try at least one stretch goal
4. 📖 Review real competition code in Reefscape-2025
5. 🚀 Move to Lesson 3 (coming soon!)

Additional Resources

• WPILib Command-Based Programming
• WPILib Triggers
• State Machines in FRC
• TEAM1771 Reefscape-2025 - Full competition code example
• TEAM1771 Style Guide - Architecture patterns reference