Skip to main content

Lesson 3: IO Layers + Simple Velocity Control

This lesson introduces a critical architectural pattern used in production FRC robots: separating subsystem logic from hardware implementation using an IO layer. You'll also learn how to implement simple proportional (P-only) control to regulate mechanism velocity instead of just setting voltage directly.

Learning Objectives​

By the end of this lesson, you will understand:

  • ✅ Why we separate IO from subsystem logic (testability, swapping sim/real hardware)
  • ✅ How to structure an IO layer with inputs, interfaces, and implementations
  • ✅ The read inputs → compute outputs → write outputs control loop pattern
  • ✅ What proportional control is and why we use it
  • ✅ How to implement simple P-only velocity control
  • ✅ How to publish telemetry for debugging and tuning

Prerequisites​

Before starting this lesson:

Why IO Layers?​

The Problem with Direct Hardware Access​

In Lesson 2, we created a subsystem that directly controlled simulated hardware:

// ❌ Direct hardware access in subsystem
class SimIntake : public frc2::SubsystemBase {
 private:
  double m_rollerSpeed = 0.0;  // Simulated motor
  
  void handleIntakingState() {
    m_rollerSpeed = 0.8;  // Directly set speed
  }
};

This works for simple examples, but has problems:

  1. Can't swap between sim and real hardware: Logic is tied to simulation
  2. Hard to test: You can't test subsystem logic without the hardware
  3. Duplicate code: Same logic written twice (sim version and real version)
  4. Messy state: Sensor readings and control outputs mixed with state logic

The IO Layer Solution​

Instead, we create a boundary between subsystem logic and hardware:

// ✅ Subsystem logic separated from hardware
class SimShooterWheel : public frc2::SubsystemBase {
 private:
  std::unique_ptr<ShooterWheelIO> m_io;  // Hardware abstraction
  ShooterWheelIOInputs m_inputs;          // Sensor readings snapshot
  
  void processState() {
    // 1. Read inputs from hardware
    m_io->updateInputs(m_inputs);
    
    // 2. Compute outputs based on state and inputs
    double command = computeMotorCommand();
    
    // 3. Write outputs to hardware
    m_io->setVoltage(command);
  }
};

Now:

  • ✅ Subsystem logic is hardware-agnostic (works with any IO implementation)
  • ✅ Easy to swap implementations (sim, real hardware, mock for testing)
  • ✅ Clear separation of concerns (state logic vs. hardware interface)
  • ✅ Sensor readings are atomic snapshots (no race conditions)

Anatomy of an IO Layer​

Let's break down the three key components using a shooter wheel as our example mechanism.

1. IOInputs - Snapshot Struct​

A struct that holds all sensor readings at a single point in time:

struct ShooterWheelIOInputs {
  double velocityRadPerSec = 0.0;     // Current wheel velocity
  double appliedVolts = 0.0;          // Voltage being applied
  double currentAmps = 0.0;           // Current draw
  double tempCelsius = 0.0;           // Motor temperature
};

Key Points:

  • Plain struct (no methods, just data)
  • Initialized with safe defaults
  • Represents a snapshot in time (atomic)
  • Used for telemetry and control decisions

2. IO Interface - Abstract Base Class​

An interface defining what operations the hardware supports:

class ShooterWheelIO {
 public:
  virtual ~ShooterWheelIO() = default;
  
  /**
   * Update inputs with current sensor readings.
   * Called by subsystem to get fresh data.
   */
  virtual void updateInputs(ShooterWheelIOInputs& inputs) = 0;
  
  /**
   * Set motor voltage command.
   * @param volts Voltage to apply (-12 to +12)
   */
  virtual void setVoltage(double volts) = 0;
};

Key Points:

  • Pure virtual methods (= 0) make it an abstract interface
  • No hardware-specific code here
  • Defines the contract all implementations must follow
  • Virtual destructor for proper cleanup

3. IO Implementation - Sim or Real​

Concrete implementations for different hardware contexts:

class ShooterWheelIOSim : public ShooterWheelIO {
 public:
  void updateInputs(ShooterWheelIOInputs& inputs) override {
    // Update simulation physics
    m_velocity += (m_appliedVolts / 12.0) * kMaxAccel * 0.02;  // Simple first-order
    m_velocity *= 0.98;  // Friction damping
    
    // Fill snapshot
    inputs.velocityRadPerSec = m_velocity;
    inputs.appliedVolts = m_appliedVolts;
    inputs.currentAmps = std::abs(m_appliedVolts) * 2.0;
    inputs.tempCelsius = 20.0;
  }
  
  void setVoltage(double volts) override {
    m_appliedVolts = std::clamp(volts, -12.0, 12.0);
  }
  
 private:
  double m_velocity = 0.0;
  double m_appliedVolts = 0.0;
  static constexpr double kMaxAccel = 50.0;  // rad/s² per full voltage
};

Key Points:

  • Overrides all virtual methods from interface
  • Contains simulation-specific physics
  • Could swap with ShooterWheelIOReal using real motor controllers
  • Simulation updates in updateInputs(), not in a separate periodic

For real hardware, you'd create ShooterWheelIOReal using motor controller objects (e.g., TalonFX, SparkMax). The subsystem doesn't know or care which implementation it's using!

Simple Proportional (P) Control​

Now that we have clean sensor readings from the IO layer, let's use them to control velocity accurately.

Why Not Just Set Voltage?​

In previous lessons, we set motor voltages directly:

// ❌ Open-loop voltage control
void handleIntakingState() {
  m_io->setVoltage(6.0);  // Hope this gives us the right speed?
}

Problems:

  • Actual velocity depends on battery voltage (12V vs. 10V gives different speeds)
  • Load changes affect speed (more resistance = slower)
  • No way to hit a target velocity precisely

Enter Proportional Control​

Instead, we use feedback control to automatically adjust voltage based on error:

// ✅ Closed-loop proportional control
void processState() {
  m_io->updateInputs(m_inputs);  // Read current velocity
  
  // Calculate error
  double error = m_targetVelocity - m_inputs.velocityRadPerSec;
  
  // Proportional response: voltage proportional to error
  double voltage = kP * error;
  
  m_io->setVoltage(voltage);  // Apply correction
}

How it works:

  1. Measure current velocity from sensors
  2. Calculate error: how far off from target
  3. Respond proportionally: bigger error = bigger correction
  4. Repeat every loop until error is small

Visual: P Control in Action​

Imagine you're driving a car and trying to maintain 60 mph:

Target: 60 mph
Current: 40 mph  →  Error: +20  →  Press gas pedal MORE (big correction)

Target: 60 mph
Current: 55 mph  →  Error: +5   →  Press gas pedal slightly (small correction)

Target: 60 mph
Current: 60 mph  →  Error: 0    →  Hold steady (no correction needed)

Target: 60 mph
Current: 65 mph  →  Error: -5   →  Release gas slightly (negative correction)

This is exactly how P control works! The controller automatically adjusts its output (voltage) based on how far you are from the target.

Watch it in action:

Tuning kP​

The proportional gain kP controls how aggressively the controller responds:

  • Too small (kP = 0.02):

    Target: ─────────100─────────
    Actual: ___/‾‾‾‾‾‾‾‾‾‾‾‾‾  (slow, takes forever)

  • Too large (kP = 0.5):

    Target: ─────────100─────────
    Actual: __/\__/\__/\__/\__  (oscillates wildly)

  • Just right (kP = 0.1):

    Target: ─────────100─────────
    Actual: __/‾‾‾‾‾‾‾‾‾‾‾‾‾‾  (fast, smooth approach)

For a shooter wheel, a good starting point might be kP = 0.1 (0.1 volts per rad/s of error).

Example:

  • Target: 100 rad/s
  • Current: 50 rad/s
  • Error: 100 - 50 = 50 rad/s
  • Command: 0.1 * 50 = 5.0 volts

As the wheel speeds up, error decreases, so the commanded voltage decreases too. Eventually it settles near the target!

Understanding PID Terms​

While we're only using P (Proportional) in this lesson, here's what the full PID controller includes:

P (Proportional) - What we're using:

  • Responds to current error
  • Bigger error = bigger response
  • Simple and effective for many mechanisms

I (Integral) - Advanced (not in this lesson):

  • Responds to accumulated past error
  • Eliminates steady-state error (tiny remaining offset)
  • Can cause overshoot if not tuned carefully

D (Derivative) - Advanced (not in this lesson):

  • Responds to rate of change of error
  • Dampens oscillations
  • Provides "braking" as you approach target

Visual comparison:

P only:     Target ────────
            Actual ___/‾‾  (slight steady-state error)

P + I:      Target ────────
            Actual ___/‾‾‾ (reaches exactly, may overshoot)

P + I + D:  Target ────────
            Actual ___/‾‾‾ (smooth, no overshoot, exact)

Learn more:

Why P-Only?​

In real FRC, you'd often use PID control (Proportional + Integral + Derivative) for better performance. But for this lesson:

  • P-only is simpler to understand and implement
  • Good enough for many mechanisms (especially flywheels)
  • Easy to tune with just one constant
  • Introduces the core concept of feedback control

You'll learn more advanced control (I, D terms, feedforward) in later lessons or in real competition code.

Subsystem with IO + P Control​

Now let's see how to structure a subsystem that uses an IO layer and proportional control.

Constructor - Dependency Injection​

Pass in the IO implementation when creating the subsystem:

class SimShooterWheel : public frc2::SubsystemBase {
 public:
  explicit SimShooterWheel(std::unique_ptr<ShooterWheelIO> io)
    : m_io(std::move(io)) {
    SetName("SimShooterWheel");
  }
  
 private:
  std::unique_ptr<ShooterWheelIO> m_io;
  ShooterWheelIOInputs m_inputs;
};

In RobotContainer:

// For simulation
m_shooterWheel{std::make_unique<ShooterWheelIOSim>()}

// For real robot, just swap the implementation
m_shooterWheel{std::make_unique<ShooterWheelIOReal>()}

State Machine with Velocity Control​

The subsystem state machine now includes velocity setpoints:

enum class State {
  kOff,      // Motor off, no target
  kSpinUp    // Motor running to target velocity
};

void processState() {
  // 1. Read inputs from hardware
  m_io->updateInputs(m_inputs);
  
  // 2. State transition
  if (m_currentState != m_desiredState) {
    m_currentState = m_desiredState;
  }
  
  // 3. Execute state logic
  switch (m_currentState) {
    case State::kOff:
      handleOffState();
      break;
    case State::kSpinUp:
      handleSpinUpState();
      break;
  }
}

void handleOffState() {
  m_io->setVoltage(0.0);
  m_targetVelocity = 0.0;
}

void handleSpinUpState() {
  // Simple P controller
  double error = m_targetVelocity - m_inputs.velocityRadPerSec;
  double voltage = kP * error;
  voltage = std::clamp(voltage, -12.0, 12.0);  // Safety limits
  
  m_io->setVoltage(voltage);
}

Setting Target Velocity​

Add a method to update the velocity setpoint:

void setTargetVelocity(double velocityRadPerSec) {
  m_targetVelocity = velocityRadPerSec;
}

Or combine with state change:

frc2::CommandPtr setSpinUpCmd(double velocityRadPerSec) {
  return frc2::cmd::RunOnce([this, velocityRadPerSec] {
    m_desiredState = State::kSpinUp;
    m_targetVelocity = velocityRadPerSec;
  }, {this});
}

Telemetry for Debugging​

Publishing telemetry is crucial for tuning and debugging control loops:

void ConfigureTelemetry() {
  m_shooterWheel.registerTelemetry([this] {
    auto state = m_shooterWheel.getState();
    
    // State and targets
    frc::SmartDashboard::PutString("ShooterWheel/State", stateToString(state.currentState));
    frc::SmartDashboard::PutNumber("ShooterWheel/TargetVelocity", state.targetVelocity);
    
    // Actual measurements (from IO layer)
    frc::SmartDashboard::PutNumber("ShooterWheel/ActualVelocity", state.inputs.velocityRadPerSec);
    frc::SmartDashboard::PutNumber("ShooterWheel/Error", state.error);
    
    // Motor outputs
    frc::SmartDashboard::PutNumber("ShooterWheel/AppliedVolts", state.inputs.appliedVolts);
    frc::SmartDashboard::PutNumber("ShooterWheel/CurrentAmps", state.inputs.currentAmps);
  });
}

What to publish:

  • Current state (for understanding behavior)
  • Target values (what we're trying to achieve)
  • Measured values (what's actually happening)
  • Error (difference between target and measured)
  • Commanded outputs (what we're commanding)

Viewing these in real-time on the Elastic dashboard helps you:

  • Verify the controller is working
  • Tune kP for better response
  • Debug unexpected behavior
  • Monitor motor health (current, temperature)

Controller Bindings​

Use the familiar hold-to-run pattern with velocity control:

void ConfigureBindings() {
  // R2: Hold to spin up to target velocity, release returns to off
  m_driverController.R2()
    .OnTrue(m_shooterWheel.setSpinUpCmd(100.0))  // 100 rad/s target
    .OnFalse(m_shooterWheel.setDesiredStateCmd(SimShooterWheel::State::kOff));
  
  // Cross: Emergency stop - set Off state (NOT direct motor stop)
  m_driverController.Cross()
    .OnTrue(m_shooterWheel.setDesiredStateCmd(SimShooterWheel::State::kOff));
}

Key Points:

  • R2 press sets SpinUp state AND target velocity in one command
  • R2 release returns to Off (stops motor via state machine)
  • Cross emergency stop uses state machine pattern (consistent with Lesson 2)
  • Target velocity is a constant for now (later could make it variable)

Read → Compute → Write Loop​

The core control loop pattern is:

1. READ INPUTS
   └─> m_io->updateInputs(m_inputs)
       Get fresh sensor data from hardware

2. COMPUTE OUTPUTS
   └─> Based on current state, inputs, and targets
       Calculate what to command (e.g., P controller)

3. WRITE OUTPUTS
   └─> m_io->setVoltage(voltage)
       Send commands to hardware

This happens every robot loop (~50Hz, or every 20ms) in processState().

Why this order matters:

  • Reading first ensures you're using fresh data
  • Computing uses consistent data from one snapshot
  • Writing last ensures all decisions are made before acting
  • The cycle repeats, creating a feedback loop

Hands-On: Project 03​

Open projects/03-io-velocity-control/ in WPILib VS Code and explore the code:

  1. Study the IO layer:

    • Find ShooterWheelIOInputs struct
    • Examine ShooterWheelIO interface (pure virtual methods)
    • Trace how ShooterWheelIOSim implements simulation physics
    • Notice how velocity updates with a simple first-order response

  2. Study SimShooterWheel subsystem:

    • Find the state enum (Off, SpinUp)
    • Follow the Read → Compute → Write pattern in processState()
    • Understand the P controller implementation in handleSpinUpState()
    • See how IO is injected via constructor

  3. Study RobotContainer:

    • See how ShooterWheelIOSim is created and passed to subsystem
    • Find R2 binding with velocity setpoint
    • Find Cross button emergency stop
    • Examine telemetry publishing

  4. Run in simulation:

    • Build and simulate the project
    • Open Elastic dashboard
    • Press R2 (or keyboard equivalent) and watch telemetry:
      • ActualVelocity should ramp up toward TargetVelocity
      • Error should decrease as velocity approaches target
      • AppliedVolts should start high, then decrease as error decreases
    • Release R2 and verify motor stops (velocity decays to zero)
    • Press Cross and verify emergency stop works

Checkpoints & Questions​

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

IO Layer Fundamentals​

  1. What are the three key components of an IO layer?

    Answer
    • IOInputs: Snapshot struct holding all sensor readings
    • IO Interface: Abstract base class defining operations
    • IO Implementation: Concrete class for sim or real hardware

  2. Why do we use std::unique_ptr<ShooterWheelIO> instead of a specific implementation?

    Answer
    • Subsystem holds an interface pointer, not a concrete type
    • This allows swapping implementations (sim, real, mock)
    • Subsystem logic doesn't depend on which implementation is used
    • This is dependency injection pattern

  3. What's the benefit of using an IOInputs snapshot struct?

    Answer
    • Atomic snapshot of all sensors at one point in time
    • No race conditions (all values from same moment)
    • Easy to log/record entire state
    • Clear separation: reading (IO) vs. using (subsystem logic)

Proportional Control​

  1. What is the formula for proportional control?

    Answer

    output = kP * error where error = target - measured

  2. What happens if kP is too large?

    Answer
    • Controller is too aggressive
    • Overshoots target significantly
    • Oscillates back and forth
    • May never settle down

  3. What happens if kP is too small?

    Answer
    • Controller is too gentle
    • Takes a long time to reach target
    • May never fully reach target (steady-state error)
    • Slow response to disturbances

  4. Why is P-only control "good enough" for a shooter flywheel?

    Answer
    • Flywheels have high inertia (resist velocity changes)
    • Small steady-state error is often acceptable
    • Simple to tune (one constant)
    • Fast response is more important than perfect accuracy
    • Can add feedforward for even better performance

Control Loop​

  1. What are the three steps in the Read → Compute → Write loop?

    Answer
    1. Read: updateInputs() to get sensor data
    2. Compute: Calculate outputs based on state/inputs/targets
    3. Write: Send commands to hardware with setVoltage() etc.

  2. Why do we call updateInputs() at the start of every processState() loop?

    Answer
    • Get fresh sensor data for this control cycle
    • Ensures decisions are based on current measurements
    • Completes the feedback loop (measure → adjust → measure...)

Architecture​

  1. How does emergency stop work with this architecture?

    Answer
    • Cross button sets m_desiredState = State::kOff
    • handleOffState() sets voltage to 0.0 and clears target
    • Uses state machine pattern (doesn't bypass subsystem logic)
    • Telemetry accurately shows Off state

Stretch Goals​

After completing the basic lesson, try these challenges:

Challenge 1: Variable Velocity Control​

Replace the fixed 100 rad/s target with analog stick control:

  1. Read right stick Y axis in RobotContainer
  2. Map stick value (-1 to +1) to velocity range (e.g., 0 to 150 rad/s)
  3. Update target velocity every loop while R2 is held
  4. Publish the commanded velocity to telemetry
  5. Test varying the stick position and watch velocity track

Challenge 2: Bang-Bang vs. P Control​

Implement an alternative "bang-bang" controller and compare:

  1. Add a bool m_useBangBang toggle in subsystem
  2. Implement bang-bang logic: full voltage if below target, zero if above
  3. Add button binding to toggle controller type
  4. Run both controllers and compare telemetry:
    • Which settles faster?
    • Which overshoots more?
    • Which is smoother?

Challenge 3: Tune kP​

Experiment with different kP values:

  1. Try kP = 0.05 (small)
  2. Try kP = 0.5 (large)
  3. Plot or observe ActualVelocity vs. Time for each
  4. Find the "sweet spot" kP that reaches target quickly without oscillation
  5. Document your findings in comments

Challenge 4: "At Target" Indicator​

Add logic to detect when velocity is close to target:

  1. Add method bool isAtTarget(double tolerance)
  2. Return true if error is within tolerance (e.g., 5 rad/s)
  3. Publish "AtTarget" boolean to telemetry
  4. Test and verify it goes true when stable, false when changing

Challenge 5: Add Feedforward​

Improve the controller with feedforward:

  1. Add a kS (static friction) constant
  2. Add a kV (velocity feedforward) constant
  3. Compute feedforward: ff = kS * sgn(target) + kV * target
  4. Combine with proportional: voltage = ff + kP * error
  5. Observe faster response and smaller steady-state error

Key Takeaways​

🎯 IO Layer:

  • Separates subsystem logic from hardware implementation
  • Enables swapping sim/real hardware without changing subsystem
  • Uses IOInputs snapshots for clean, atomic sensor readings
  • Interface + implementation = flexibility and testability

🎯 Proportional Control:

  • Feedback control: measure → compare → correct
  • Output proportional to error: output = kP * error
  • Automatically adjusts to reach target velocity
  • Simple to implement and tune (one constant)

🎯 Control Loop Pattern:

  1. Read inputs from hardware (updateInputs())
  2. Compute outputs based on state and targets (P controller)
  3. Write outputs to hardware (setVoltage())
  • Repeats every 20ms, creating a feedback loop

🎯 Benefits:

  • ✅ Hardware-agnostic subsystem logic
  • ✅ Accurate velocity control (not just voltage)
  • ✅ Easy to test and swap implementations
  • ✅ Clear telemetry for debugging and tuning
  • ✅ Follows TEAM1771 production patterns

Next Steps​

  1. ✅ Complete the hands-on project 03-io-velocity-control
  2. ✅ Answer all checkpoint questions
  3. ✅ Try at least one stretch goal (kP tuning recommended)
  4. 📖 Review real IO layers in Reefscape-2025
  5. 🚀 Move to Lesson 4 (coming soon!)

Additional Resources​