Lesson 3: Subsystems

What are Subsystems?

Think of each subsystem as a self-contained box that manages a specific robot mechanism.

• Keeps code organized by robot functions (e.g. drivetrain, intake)
• Improve readability
• Makes testing easier

PID Control and Tuning

Many mechanisms (like an arm or wrist) need to move precisely to a target position or speed. A PID controller helps do that by adjusting the motor output based on how far off the target it is (error).

What is PID?

PID stands for these 3 aspects:

• P (Proportional): Bigger error = bigger correction
• I (Integral): Helps fix small long-term errors (like friction)
• D (Derivative): Reacts to how fast the error is changing (helps smooth things)

The tunable constants kP, kI, and kD represent how much the controller will move based on each aspect.

Why Tune PID?

• If the values are too low: system may be too slow or never reach the goal
• If too high: system may overshoot or oscillate wildly

CTRE Talon FX Motor

• Smart motor controller with integrated encoder (can translate mechanical motion into code representing an angle, position, speed, etc)

• Supports advanced features:

  • Built-in PID controls
  • Motion profiling
  • Current limiting

Key Talon FX Features

• Tells the motor to spin forward/backward at x percent power (Motor output power is in between -1.0 to 1.0): .Set(ControlMode::PercentOutput, value)
• Returns motor's encoder position: .GetSelectedSensorPosition()
• Tunes motor's built-in PID controllers: .Config_kP(...), .Config_kI(...), .Config_kD(...), .Config_kF(...)
• .SetNeutralMode(Brake/Coast)

Motion Magic

A control mode in CTRE TalonFX motor controllers that:

• Moves a mechanism to a target position
• Controls velocity and acceleration
• Has built-in control so it is not needed to manually write PID loops

Code Overview

For each subsystem (e.g. Wrist), you may see:

Each plays a different role to make code clean, testable, and adaptable.

Why This Structure?

• Makes it easier to switch between real robot and simulation
• Supports hardware abstraction
• Enables unit testing without needing the robot
• Encourages clean separation of logic

File 1 - Wrist.java

aka the main logic of the subsystem

• Receives sensor data from WristIO
• Tells WristIO what motor outputs to apply
• Holds PID loops, state machines, etc.

Think of it as the "brain of the wrist"

File 2 - WristIO.java

aka the Java interface file

• Defines the contract between the logic and the hardware
• Includes methods like updateInputs(), setVoltage(), etc. that must be provided by the real/sim classes.

public interface WristIO {
    public void updateInputs(WristIOInputs inputs);
    public void setVoltage(double volts);
}

• It ensures that implementations like WristIOTalonFX or WristIOSim will work when using Wrist.java

No matter the type of implementation, follow this interface

WristIO Inputs

• A data-holding class in WristIO.java that represents the current state of the hardware
• Used to pass data like sensor readings and status info from the IO implementation (e.g. WristIOTalonFX) to the main subsystem logic (Wrist.java)

Where It Is Set

• Inside WristIOTalonFX or WristIOSim, for example:

@Override
public void updateInputs(WristIOInputs inputs) {
    inputs.position = Radians.of(sim.getAngleRads());
    inputs.velocity = RadiansPerSecond.of(sim.getVelocityRadPerSec());
    inputs.appliedVoltage = appliedVoltage;
    inputs.supplyCurrent = Amps.of(Math.abs(sim.getCurrentDrawAmps()));
    // Add more
}

Raw sensor data is being used to populate the fields in the inputs object

How It Is Passed Around

• In Wrist.java, declared and updated like this:

public class Wrist extends SubsystemBase {
  private final WristIO io;
  private final WristIOInputs inputs = new WristIOInputs();
}

@Override
public void periodic() {
    io.updateInputs(inputs);
    // Use the input data for logic after
}

Syntax Example

Aka a set of rules that defines how the code must be written so that the computer understands it

public static class WristIOInputs {
  public Angle position = Radians.zero();
  public AngularVelocity velocity = RadiansPerSecond.zero();
  public Voltage appliedVoltage = Volts.zero();
  public Current supplyCurrent = Amps.zero();
  public Current statorCurrent = Amps.zero();
  public Temperature temperature = Celsius.zero();
  public boolean connected = false;
}

File 3 - WristIOTalonFX.java

aka real hardware implementation

• Implements WristIO using a TalonFX motor
• Talks to real motor controllers
• Updates sensor readings and sends voltage outputs

Plug in this file for real-world operation

File 4 - WristIOSim.java

aka simulation implementation

• Mocks sensor values and simulates movement
• Lets you run and test Wrist.java without a robot
• Implements WristIO using WPILib's sim features

Swap this in when testing in the simulation

How They Work Together

Wrist.java

  ↕ (uses)

WristIO (interface)

  ↕ (implemented by)

WristIOTalonFX.java   ← for real robot
WristIOSim.java       ← for sim/testing

Choose which implementation to use based on which real/sim build mode you want to run

How to "Plug In" The Implementation

• Inside RobotContainer, create your subsystem like this:

// For real robot
Wrist wrist = new Wrist(new WristIOTalonFX());

// For sim
Wrist wrist = new Wrist(new WristIOSim());

When to Use This Pattern

• If the subsystem uses motors/sensors
• Systems that want to be simulated
• Subsystems that's hardware might change later

Task for This Lesson

Follow the instructions here. A template project is provided for you in the same folder.

Important:

• Please test your solution to the task in simulation before submitting. See the WPILib Simulator page for help.
• When you are ready to submit, create a branch and pull request on this repository with your solution.