2. Create Your First Subsystem

In this tutorial, we will learn how to create a complete robot subsystem. We will reference the structure of the Chassis subsystem to create a new Intake subsystem. The Intake subsystem is typically used to collect game pieces, such as balls or cubes. Code download link

WPILib's subsystem design follows several key principles that collectively ensure code robustness, maintainability, and testability:

• Dependency Injection Pattern: Creates corresponding instances through factory methods in different environments (such as real robots, simulators, or test platforms), enabling seamless environment switching.
• Interface Segregation Principle: Uses hardware abstraction layers to decouple business logic from specific hardware. Subsystems only depend on abstract hardware interfaces rather than concrete motor or sensor classes, improving code flexibility.
• Centralized Configuration Management: All hardware parameters, PID constants, and mechanical characteristics are centrally managed in configuration classes, facilitating adjustments and ensuring system configuration consistency.
• State Monitoring Mechanism: Deep integration of real-time hardware monitoring, tracking connection status and operational data, with alert systems providing timely feedback on anomalies to ensure stable robot operation.

These principles collectively build a powerful and easily maintainable robot software architecture.

Example

Let's analyze the key structure that comprises a subsystem using the chassis subsystem as an example:

Chassis.java

public class Chassis extends SubsystemBase {
  // a. Hardware Interfaces
  // b. Core Methods
  // c. Static Initialization Block
  // d. Periodic Methods
  // e. Constructor and Factory Methods
}

ChassisConfig.java

public class ChassisConfig {
  // f. Configuration Parameters
}

a. Hardware Interfaces

public class Chassis extends SubsystemBase {
  private final GenericWheelIO leftIO;
  private final GenericWheelIO rightIO;
  private final GenericWheelIOInputsAutoLogged leftInputs = new GenericWheelIOInputsAutoLogged();
  private final GenericWheelIOInputsAutoLogged rightInputs = new GenericWheelIOInputsAutoLogged();
  private final Alert leftOfflineAlert = new Alert("Chassis Left Offline", Alert.AlertType.WARNING);
  private final Alert rightOfflineAlert = new Alert("Chassis Right Offline", Alert.AlertType.WARNING);
}

Here we use the GenericWheelIO interface instead of concrete motor classes, allowing seamless switching between real robots, simulators, and test environments. *InputsAutoLogged is used to automatically log data to AdvantageKit.

b. Core Methods

public class Chassis extends SubsystemBase {
  public void setWheelsVelocities(double leftVelocity, double rightVelocity) {
    leftIO.setVelocity(leftVelocity / ChassisConfig.WHEEL_RADIUS_METER, 0.0);
    rightIO.setVelocity(rightVelocity / ChassisConfig.WHEEL_RADIUS_METER, 0.0);
  }
}

Public methods should accept parameters in meaningful units (such as meters/second) and internally convert them to the units required by motors (such as radians/second).

c. Static Initialization Block

public class Chassis extends SubsystemBase {
  static {
    final var driveGains = ChassisConfig.getDriveGains();
    ChassisConfig.driveKp.initDefault(driveGains.kp());
    ChassisConfig.driveKd.initDefault(driveGains.kd());
    ChassisConfig.driveKs.initDefault(driveGains.ks());
  }
}

The static block executes when the class is loaded and is used to set different default PID parameters based on the runtime mode.

d. Periodic Methods

public class Chassis extends SubsystemBase {
  @Override
  public void periodic() {
    // 1. Update hardware inputs
    leftIO.updateInputs(leftInputs);
    rightIO.updateInputs(rightInputs);

    // 2. Log data
    Logger.processInputs("Chassis Left", leftInputs);
    Logger.processInputs("Chassis Right", rightInputs);

    // 3. State monitoring and alerts
    leftOfflineAlert.set(!leftInputs.connected);
    rightOfflineAlert.set(!rightInputs.connected);

    // 4. Dynamic parameter updates
    LoggedTunableNumber.ifChanged(
        hashCode(),
        () -> {
          leftIO.setPdf(
              ChassisConfig.driveKp.get(),
              ChassisConfig.driveKd.get(),
              ChassisConfig.driveKs.get());
          rightIO.setPdf(
              ChassisConfig.driveKp.get(),
              ChassisConfig.driveKd.get(),
              ChassisConfig.driveKs.get());
        },
        ChassisConfig.driveKp,
        ChassisConfig.driveKd,
        ChassisConfig.driveKs);

    // 5. Odometry updates
    // Simplify for tank drive odometry
    RobotContainer.getOdometry()
        .addWheeledObservation(
            new WheeledObservation(
                Timer.getFPGATimestamp(),
                new DifferentialDriveWheelPositions(
                    leftInputs.positionRad * ChassisConfig.WHEEL_RADIUS_METER,
                    rightInputs.positionRad * ChassisConfig.WHEEL_RADIUS_METER),
                null));
  }
}

The periodic method executes at a fixed frequency and is responsible for reading hardware status, logging key data, monitoring faults, dynamically updating tunable parameters, and handling special logic for other subsystems.

e. Constructor and Factory Methods

public class Chassis extends SubsystemBase {
  private Chassis(GenericWheelIO leftIO, GenericWheelIO rightIO) {
    this.leftIO = leftIO;
    this.rightIO = rightIO;
  }

  // Real instance - uses Kraken motors
  public static Chassis createReal() {
    return new Chassis(
        new GenericWheelIOKraken(
                "Left Drive",
                Constants.Ports.Can.LEFT_DRIVE_MASTER,
                ChassisConfig.getDriveConfig())
            .withFollower(Constants.Ports.Can.LEFT_DRIVE_SLAVE, true),
        new GenericWheelIOKraken(
                "Right Drive",
                Constants.Ports.Can.RIGHT_DRIVE_MASTER,
                ChassisConfig.getDriveConfig())
            .withFollower(Constants.Ports.Can.RIGHT_DRIVE_SLAVE, true));
  }

  // Simulated instance - uses simulated motors
  public static Chassis createSim() {
    return new Chassis(
        new GenericWheelIOSim(
            2,
            0.025,
            ChassisConfig.DRIVE_REDUCTION,
            ChassisConfig.driveKp.get(),
            ChassisConfig.driveKd.get()),
        new GenericWheelIOSim(
            2,
            0.025,
            ChassisConfig.DRIVE_REDUCTION,
            ChassisConfig.driveKp.get(),
            ChassisConfig.driveKd.get()));
  }

  // IO test instance - empty implementation
  public static Chassis createIO() {
    return new Chassis(new GenericWheelIO() {}, new GenericWheelIO() {});
  }
}

The factory pattern separates creation logic from usage logic, making switching between different environments simple.

f. Configuration Parameters

public class ChassisConfig {
  // Tunable parameters
  static final LoggedTunableNumber driveKp = new LoggedTunableNumber(DebugGroup.CHASSIS, "DriveKp");
  static final LoggedTunableNumber driveKd = new LoggedTunableNumber(DebugGroup.CHASSIS, "DriveKd");
  static final LoggedTunableNumber driveKs = new LoggedTunableNumber(DebugGroup.CHASSIS, "DriveKs");

  // Returns different gain parameters based on runtime mode
  static Gains getDriveGains() {
    return switch (Constants.MODE) {
      case REAL -> new Gains(10.0, 0.1, 0.2);
      case SIM, REPLAY -> new Gains(0.25, 0.0, 0.0);
    };
  }

  // Mechanical constants
  static final double DRIVE_REDUCTION = 10.71; // 10.71:1
  public static final double TRACK_WIDTH = 0.69; // meters
  static final double WHEEL_RADIUS_METER = 0.0479;

  // Motor configuration
  static TalonFXConfiguration getDriveConfig() {
    var config = new TalonFXConfiguration();
    // ...
    return config;
  }

  record Gains(double kp, double kd, double ks) {}
}

The configuration class centrally manages all parameters, including tunable parameters that can be adjusted in real-time on the Dashboard, mechanical constants, and environment-specific configurations.

Your Turn

Now you need to reference the structure of Chassis to complete the design of the Intake subsystem. The core methods and configuration parameters that must be implemented have been provided, and you can complete your code design based on them.

Intake.java

public class Intake extends SubsystemBase {
  public double getPivotDegree() {
    // TODO: implement
    return 0.0;
  }

  public void setPivotDegree(double degree) {
    // TODO: implement
  }

  public double getRollerVelocityRPM() {
    // TODO: implement
    return 0.0;
  }

  public void setRollerVoltage(double voltage) {
    // TODO: implement
  }

  static {
    // TODO: implement
  }

  private final GenericArmIO pivotIO;
  private final GenericRollerIO rollerIO;
  // TODO: implement

  @Override
  public void periodic() {
    // TODO: implement
  }

  private Intake(GenericArmIO pivotIO, GenericRollerIO rollerIO) {
    this.pivotIO = pivotIO;
    this.rollerIO = rollerIO;
  }

  public static Intake createReal() {
    return new Intake(/**/);
  }

  public static Intake createSim() {
    return new Intake(/**/);
  }

  public static Intake createIO() {
    return new Intake(new GenericArmIO() {}, new GenericRollerIO() {});
  }
}

Demonstration

The robot is controlled by two joysticks and two triggers. The joysticks move the corresponding wheels forward. The left trigger operates the intake, ejecting coral when pressed and injecting when released. The right trigger adjusts the angle of the intake.