3.1.1 - Subsystems

Subsystems are an important part of code; they let us interface directly with hardware on the robot, and they help split up large tasks by grouping them into distinct groups of tasks per subsystem.

This page provides a general “best practices” list for subsystems.

Describe state categorically

Subsystems - like a lot of things in robot programming - have mutable state. It becomes important to be able to share this state outside of the subsystem itself to make decisions in the future. For example, a “manipulator” subsystem would need to expose state, such as whether a gamepiece is present, in order for the robot to act on this information later.

However, some methods of exposing state are better than others. One thing we want to avoid is the XY Problem, where some client (code external to the subsystem) asks for one value (say, elevator height) when what they really want to know is something else (whether the elevator is at setpoint).

Exposing state through some value X just so that the client can process it into some value Y is a bad idea, because it puts too much trust in the client to do the right thing here. It also violates rules of encapsulation if we allow public access to physical information about the subsystem, such as an elevator’s height or a shooter’s voltage draw.

All of this information is eventually going to be used to make a decision, so it’s better to represent that with a categorical value (more on that word in 3.1.4 - State, Theory.

Generally tend towards descriptive, discrete types rather than continuous, raw data.

Here’s some examples:

  • getSpeed() should be isReady() if it’s being compared against some speed setpoint value.

  • getMotorTemperature() should be isOkay() if it’s being used to verify that the subsystem is physically okay.

  • getArmPosition() should be atSetpoint() if it’s being used to see if a subsystem has reached it’s setpoint.

Notice how all of these examples require context - what are we using the value for? Help clean up the client’s code by moving those checks that they shouldn’t do into the subsystem itself.

Expose Triggers, not booleans

Consider this method in a Manipulator subsystem:

public boolean hasPiece() {
  /* snip */
}

This works fine, and is usable outside of our class. It doesn’t expose any information about the subsystem besides what the user actually wants to know, so it seems good. However, there’s no guarantee in code at compile time that this value continues to be valid. Obviously, this is because such a statement is false. Not having a piece can easily change to having a piece in the future, and vice versa.

Instead, it’s recommended to use a Trigger object, which is itself a form of a BooleanSupplier. This way, we know that the value from this method is always correct, because it updates with the state of the subsystem.

We should change the method signature to this:

public Trigger hasPiece() {
  return new Trigger(() -> {
    /* snip
  });
}

Now, our code returns a Trigger object which will always be valid, and is significantly more easily usable. Fun fact: Trigger objects also act as BooleanSuppliers, so they are very versatile.

Additionally, it’s more likely that the end user will want to use this object as a Trigger, and exposing these as the return type for methods makes it easier to access these.

Return Commands

Sometimes, programmers will expose public void methods, such as setStow() or startAction(). However, these can be confusing and difficult to work with. Here’s why:

  1. These commands don’t finish instantly. Instead, they may only set a desired state - they won’t wait to actually achieve said state. The user has to then wait for the subsystem to finish the desired action, which can be forgotten by programmers.

  2. It’s up to the client to make even remotely complex actions. For example, to intake a gamepiece with this method, the client is expected to call the startIntake() method, wait for a piece, and then call stopIntake(). If they forget a step, their command will not run.

  3. This induces repetitive code. If two large commands require different behavior for the most part, but have one subsystem do the same thing, then the implementation for that action will have to be copied across multiple files.

Instead, it’s preferred to avoid exposing incomplete actions and instead returning Command instances that do a whole action.

This helps separate basic subsystem level behavior from full robot behavior and makes it easier to make new commands. This also ensures that if that behavior is ran, the rules of command requirements are followed.

Use enums extensively

It’s rarely a good idea to allow a nearly-infinite set of inputs to a system. If your elevator has a method go(double position) and returns a command that sends the elevator to that height, you’re allowing all of the following problems:

  • Inputs outside a reasonable range could break the robot. Checking for those is necessary here, but is verbose and can be incorrectly done.

  • Inputs that don’t actually make sense for the mechanism are also allowed unchecked. There’s rarely an infinite number of positions for a system to be in, which isn’t great.

  • It’s up to the client to make sure that the setpoints are valid, which violates subsystem’s rules of isolated internals. The user shouldn’t have to know what the correct mechanism position values are for a subsystem.

All of these problems can be replaced with a simple enum in code. Enums that represent possible subsystem states have the following advantages:

  • Ensures a finite set of valid inputs. The user can’t just create a new enum variant and apply that. The variants are fixed, and can be declared inside the subsystem.

  • The user has a better idea of what these values mean. Stow is a better term in code than 0.241 for an elevator’s position.

  • It’s no longer up to the end user to supply the physical values; instead, that can be handled inside the subsystem.

Enums are incredibly non-verbose in Java as well (for this use case). We can construct enums with associated values like so:

public enum ArmState {
  Stow(0),
  Score(0.2),
  Reverse(-0.11);

  protected final double position;
  private ArmState(double position) {
    this.position = position;
  }
}

With this code, clients (outside of the subsystem’s package) can still refer to ArmState.Stow, but they cannot see the value of position. But the subsystem can. The subsystem can see the position field of the state and easily set that as the position reference internally.

Separate hardware logic from subsystem logic

If subsystems are expected to expose subsystem-specific commands, and they have to handle the hardware objects, this is too much responsibility. Instead, we can use composition to create an “IO layer” as another layer of abstraction between hardware and subsystem logic.

These “IO” classes could be simple classes that are just instantiated internally and used to write to the hardware, but there are a few other optimizations we can make to ensure that subsystems work well.

For the rest of this small section, we’ll look at an Intake subsystem.

Cache State

It’s helpful to have consistent state throughout one cycle of the robot. This can be done by caching each value from the hardware (voltage, temperature, sensor states, etc.) independently, but this leaves the opportunity for forgetting to update one value.

Instead, we can create an “IO inputs” class that looks like this:

IntakeIOInputs.java
public class IntakeIOInputs {
  public boolean coralSensorTriggered;
  public double pivotPosition;
  public double intakeVoltage;
}

Now, we can also define a method updateInputs(IntakeIOInputs inputs) method on the IO class, which updates the values in the entire inputs object, which can be called once in the subsystem’s periodic() method. This ensures a consistent state across an entire loop, which is good.

Use interfaces

We could create an IntakeIOHardware class, which would handle all of our IO operations with hardware. However, let’s consider more advanced features in the long run - mainly simulation. If we want to simulate the robot, it’s best to have a strict division between the robot’s hardware code and it’s associated code in simulation.

Instead, let’s create an IntakeIO interface, which IntakeIOHardware implements. Here’s what the interface may look like in code:

IntakeIO.java
public interface IntakeIO {
  void setRollerVoltage(double voltage);
  void setPivotPosition(double position);

  void updateInputs(IntakeIOInputs inputs);
}

Now, instead of storing the io layer as IntakeIOHardware, we can simply update IntakeIOHardware to implement IntakeIO and store the IO object as an IntakeIO object, allowing for other classes that also implement IntakeIO. We can create a separate class, IntakeIOSim which has all the same methods, but can vary in implementation, making simulations easier to change.

We can also create more classes that implement IntakeIO for unit testing purposes if we so desire.

Example subsystem: Elevator

Here is an example elevator subsystem, copied from team 3414’s 2025 REEFSCAPE program code.

Firstly, the subsystem’s IO interface is declared. This is the class that the subsystem class itself will use to communicate with the hardware.

ElevatorIO.java
 1package frc.robot.subsystems.elevator;
 2
 3public interface ElevatorIO {
 4  void updateInputs(ElevatorIOInputs inputs);
 5
 6  public class ElevatorIOInputs {
 7    public boolean leftMotorConnected = true;
 8    public boolean rightMotorConnected = true;
 9    public double leftVoltage = 0.0;
10    public double rightVoltage = 0.0;
11    public double leftCurrent = 0.0;
12    public double rightCurrent = 0.0;
13    public double leftTemp = 0.0;
14    public double rightTemp = 0.0;
15    public double leftVelocityRPS = 0.0;
16    public double rightVelocityRPS = 0.0;
17    public double leftPosition = 0.0;
18    public double rightPosition = 0.0;
19    public double position = 0.0;
20    public double reference = 0.0;
21    public boolean zeroCANrangeConnected = true;
22    public boolean zeroCANrangeDetected = false;
23    public double zeroCANrangeDistance = 0.0;
24    public double zeroCANrangeStrength = 0.0;
25  }
26
27  /**
28   * Sets the elevator's position reference to the desired position in mechanical
29   * units.
30   */
31  void setPosition(double position);
32
33  /**
34   * Sets the elevator's applied voltage to the desired voltage.
35   */
36  void setVoltage(double voltage);
37
38  /**
39   * Disables the elevator's soft limits. This is useful when re-zeroing the
40   * elevator and the soft limits can't be trusted because the position may be
41   * incorrect.
42   */
43  void disableLimits();
44
45  /**
46   * Enables the soft limits of the elevator
47   */
48  void enableLimits();
49
50  /**
51   * Resets the elevator's encoders to accept the current position as zero.
52   */
53  void calibrateZero();
54}

Here, we see that we not only declare the ElevatorIO interface, but we also define the ElevatorIOInputs class. For a real robot, there are a lot of inputs, so we list each of them here. We also define several other low-level methods, such as setVoltage() and setPosition(), and some more features.

We can now take a look at the hardware implementation of the ElevatorIO interface:

ElevatorIOHardware.java
  1package frc.robot.subsystems.elevator;
  2
  3import com.ctre.phoenix6.BaseStatusSignal;
  4/* snip */
  5
  6public class ElevatorIOHardware implements ElevatorIO {
  7  private final TalonFX leftMotor;
  8  private final TalonFX rightMotor;
  9
 10  private final CANrange CANrange;
 11  private final SoftwareLimitSwitchConfigs noLimits = new SoftwareLimitSwitchConfigs()
 12      .withForwardSoftLimitEnable(false)
 13      .withReverseSoftLimitEnable(false);
 14
 15  private final DynamicMotionMagicVoltage control;
 16
 17  private double reference = Double.NaN;
 18
 19  private final StatusSignal<Voltage> leftVoltageSignal;
 20  private final StatusSignal<Voltage> rightVoltageSignal;
 21  private final StatusSignal<Current> leftCurrentSignal;
 22  private final StatusSignal<Current> rightCurrentSignal;
 23  private final StatusSignal<Temperature> leftTempSignal;
 24  private final StatusSignal<Temperature> rightTempSignal;
 25  private final StatusSignal<AngularVelocity> leftVelocitySignal;
 26  private final StatusSignal<AngularVelocity> rightVelocitySignal;
 27  private final StatusSignal<Angle> leftPositionSignal;
 28  private final StatusSignal<Angle> rightPositionSignal;
 29
 30  private final StatusSignal<Distance> CANrangeDistanceSignal;
 31  private final StatusSignal<Boolean> CANrangeDetectedSignal;
 32  private final StatusSignal<Double> CANrangeStrengthSignal;
 33
 34  public ElevatorIOHardware() {
 35    rightMotor = new TalonFX(ElevatorConstants.kRightMotorID, "*");
 36    leftMotor = new TalonFX(ElevatorConstants.kLeftMotorID, "*");
 37    rightMotor.getConfigurator().apply(ElevatorConstants.kMotorConfig);
 38    leftMotor.getConfigurator().apply(ElevatorConstants.kMotorConfig);
 39    leftMotor
 40        .setControl(new Follower(ElevatorConstants.kRightMotorID, ElevatorConstants.kInvertLeft));
 41    rightMotor.setPosition(0.0);
 42    leftMotor.setPosition(0.0);
 43
 44    CANrange = new CANrange(ElevatorConstants.kCANrangeID);
 45    CANrange.getConfigurator().apply(ElevatorConstants.kCANrangeConfig);
 46
 47    control = new DynamicMotionMagicVoltage(
 48        0, // no position setpoint yet
 49        ElevatorConstants.kMaxSpeed,
 50        ElevatorConstants.kMaxAcceleration,
 51        ElevatorConstants.kMaxJerk);
 52
 53    leftVoltageSignal = leftMotor.getMotorVoltage();
 54    rightVoltageSignal = rightMotor.getMotorVoltage();
 55    leftCurrentSignal = leftMotor.getSupplyCurrent();
 56    rightCurrentSignal = rightMotor.getSupplyCurrent();
 57    leftTempSignal = leftMotor.getDeviceTemp();
 58    rightTempSignal = rightMotor.getDeviceTemp();
 59    leftVelocitySignal = leftMotor.getVelocity();
 60    rightVelocitySignal = rightMotor.getVelocity();
 61    leftPositionSignal = leftMotor.getPosition();
 62    rightPositionSignal = rightMotor.getPosition();
 63
 64    CANrangeDetectedSignal = CANrange.getIsDetected();
 65    CANrangeDistanceSignal = CANrange.getDistance();
 66    CANrangeStrengthSignal = CANrange.getSignalStrength();
 67
 68    StatusSignalUtil.registerCANivoreSignals(
 69        leftVoltageSignal,
 70        rightVoltageSignal,
 71        leftCurrentSignal,
 72        rightCurrentSignal,
 73        leftTempSignal,
 74        rightTempSignal,
 75        leftVelocitySignal,
 76        rightVelocitySignal,
 77        leftPositionSignal,
 78        rightPositionSignal);
 79    StatusSignalUtil.registerRioSignals(
 80        CANrangeDetectedSignal,
 81        CANrangeDistanceSignal,
 82        CANrangeStrengthSignal);
 83  }
 84
 85  public void updateInputs(ElevatorIOInputs inputs) {
 86    inputs.leftMotorConnected = BaseStatusSignal.isAllGood(
 87        leftVoltageSignal,
 88        leftCurrentSignal,
 89        leftTempSignal,
 90        leftVelocitySignal,
 91        leftPositionSignal);
 92    inputs.rightMotorConnected = BaseStatusSignal.isAllGood(
 93        rightVoltageSignal,
 94        rightCurrentSignal,
 95        rightTempSignal,
 96        rightVelocitySignal,
 97        rightPositionSignal);
 98    inputs.leftVoltage = leftVoltageSignal.getValueAsDouble();
 99    inputs.rightVoltage = rightVoltageSignal.getValueAsDouble();
100    inputs.leftCurrent = leftCurrentSignal.getValueAsDouble();
101    inputs.rightCurrent = rightCurrentSignal.getValueAsDouble();
102    inputs.leftTemp = leftTempSignal.getValueAsDouble();
103    inputs.rightTemp = rightTempSignal.getValueAsDouble();
104    inputs.leftVelocityRPS = leftVelocitySignal.getValueAsDouble();
105    inputs.rightVelocityRPS = rightVelocitySignal.getValueAsDouble();
106    inputs.leftPosition = leftPositionSignal.getValueAsDouble();
107    inputs.rightPosition = rightPositionSignal.getValueAsDouble();
108    inputs.position = inputs.rightPosition;
109
110    inputs.reference = reference;
111
112    inputs.zeroCANrangeConnected = BaseStatusSignal.isAllGood(
113        CANrangeDetectedSignal,
114        CANrangeDistanceSignal,
115        CANrangeStrengthSignal);
116    inputs.zeroCANrangeDetected = CANrangeDetectedSignal.getValue();
117    inputs.zeroCANrangeDistance = CANrangeDistanceSignal.getValueAsDouble();
118    inputs.zeroCANrangeStrength = CANrangeStrengthSignal.getValueAsDouble();
119  }
120
121  public void setPosition(double reference) {
122    rightMotor.setControl(control.withPosition(reference));
123    this.reference = reference;
124  }
125
126  public void setVoltage(double voltage) {
127    rightMotor.setVoltage(voltage);
128  }
129
130  public void enableLimits() {
131    rightMotor.getConfigurator().apply(ElevatorConstants.kMotorConfig.SoftwareLimitSwitch);
132    leftMotor.getConfigurator().apply(ElevatorConstants.kMotorConfig.SoftwareLimitSwitch);
133  }
134
135  public void disableLimits() {
136    rightMotor.getConfigurator().apply(noLimits);
137    leftMotor.getConfigurator().apply(noLimits);
138  }
139
140  public void calibrateZero() {
141    rightMotor.setPosition(0.0);
142    leftMotor.setPosition(0.0);
143  }
144}

This class supplies the basic hardware-facing code needed to control a physical elevator with two TalonFX motor controllers.

Note

If you’re curious about the many, many StatusSignals throughout this code, read the section 3.2 - Tips for CTRE Status Signals.

Now, let’s look at another class that implements ElevatorIO, but instead runs a (very basic) simulation of the system:

ElevatorIOSim.java
 1package frc.robot.subsystems.elevator;
 2
 3public class ElevatorIOSim implements ElevatorIO {
 4  private double position = 0;
 5  private double reference = 0;
 6
 7  public void updateInputs(ElevatorIOInputs inputs) {
 8    position = 0.9 * position + 0.1 * reference;
 9    inputs.position = position;
10    inputs.reference = reference;
11    inputs.zeroCANrangeDetected = position < 1e-3;
12  }
13
14  public void setPosition(double position) {
15    reference = position;
16  }
17
18  public void setVoltage(double voltage) {}
19  
20  public void enableLimits() {}
21  public void disableLimits() {}
22
23  public void calibrateZero() {}
24
25}

Notice that the implementation is completely different. Some methods, such as enableLimits(), are simply no-ops that don’t do anything in simulation. This doesn’t actually accurately simulate the elevator, but that’s OK. For basic simulation tests, it’s acceptable to have a subsystem that doesn’t model all the complex real-world factors. If we wanted, we could create another class that implements the ElevatorIO interface (maybe call it ElevatorIOAdvancedSim) which would use more powerful simulation technology. But we don’t have to.

Before we finish this up by looking at the actual Elevator class, we need two more helper files.

Firstly, we may have an ElevatorConstants file which stores important constants about our elevator. Here’s what that may look like:

ElevatorConstants.java
 1package frc.robot.subsystems.elevator;
 2
 3import static edu.wpi.first.units.Units.Seconds;
 4/* snip */
 5
 6public final class ElevatorConstants {
 7  protected static final int kLeftMotorID = 51;
 8  protected static final int kRightMotorID = 52;
 9  protected static final int kCANrangeID = 53;
10
11  protected static final boolean kInvertLeft = true;
12
13  protected static final double kSupplyCurrentLimit = 100;
14
15  protected static final double kRotorToSensorRatio = 5.2;
16  protected static final double kSensorToMechanismRatio = 1;
17
18  protected static final double kGearRatio = kRotorToSensorRatio * kSensorToMechanismRatio;
19
20  private static final double kDrumRadius = Units.inchesToMeters(2.256 / 2);
21
22  /* snip */
23
24  protected static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration()
25      .withMotorOutput(new MotorOutputConfigs()
26          .withNeutralMode(NeutralModeValue.Brake)
27          .withInverted(InvertedValue.CounterClockwise_Positive))
28
29      .withFeedback(new FeedbackConfigs()
30          .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor)
31          .withSensorToMechanismRatio(kGearRatio))
32
33      .withCurrentLimits(new CurrentLimitsConfigs()
34          .withSupplyCurrentLimitEnable(true)
35          .withSupplyCurrentLimit(kSupplyCurrentLimit))
36
37      .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs()
38          .withForwardSoftLimitThreshold(kForwardSoftLimit)
39          .withForwardSoftLimitEnable(true)
40          .withReverseSoftLimitThreshold(kReverseSoftLimit)
41          .withReverseSoftLimitEnable(false))
42
43      .withSlot0(new Slot0Configs()
44          .withGravityType(GravityTypeValue.Elevator_Static)
45          .withKP(20)
46          .withKI(0)
47          .withKD(0)
48          .withKS(0.125)
49          .withKV(3.59 * (kDrumRadius * 2 * Math.PI))
50          .withKA(0.05 * (kDrumRadius * 2 * Math.PI))
51          .withKG(0.42))
52
53      .withMotionMagic(new MotionMagicConfigs()
54          .withMotionMagicCruiseVelocity(kMaxSpeed)
55          .withMotionMagicAcceleration(kMaxAcceleration)
56          .withMotionMagicJerk(kMaxJerk));
57
58  protected static final CANrangeConfiguration kCANrangeConfig = new CANrangeConfiguration()
59      .withFovParams(new FovParamsConfigs()
60          .withFOVRangeX(6.75)
61          .withFOVRangeY(6.75))
62      .withProximityParams(new ProximityParamsConfigs()
63          .withMinSignalStrengthForValidMeasurement(3500)
64          .withProximityThreshold(0.13)
65          .withProximityHysteresis(0));
66
67  protected static final Time kRangeDebounceTime = Seconds.of(0.06);
68
69  protected static final LinearSystem<N2, N1, N2> kPlant =
70      LinearSystemId.createElevatorSystem(DCMotor.getKrakenX60(1), 1, 1, 1);
71  public static final double kCalibrationTime = 3.5; // seconds
72}
73
74      
75

This class is rather straightforward; if there’s any constants about your subsystem, you can put them here.

Another simple yet important class is an enum class that represents valid elevator setpoints. We call this one ElevatorState:

ElevatorState.java
 1package frc.robot.subsystems.elevator;
 2
 3public enum ElevatorState {
 4  /** Elevator at lowest position */
 5  Zero(0),
 6  /** Elevator at ground algae intake height*/
 7  Ground(0),
 8  /** Height for ground algae intake */
 9  HighGround(0.60),
10  /** Regular "home" position - also intake position */
11  Stow(0.31),
12  /** A little higher than stow to eject a coral */
13  Eject(ElevatorState.Stow.position() + 2 * ElevatorConstants.kInch),
14  /** Height to score processor */
15  Processor(0),
16  /** L1 height */
17  L1(2.63),
18  /** Secondary L1 height for when a coral is already present */
19  SecondaryL1(ElevatorState.L1.position() + 8 * ElevatorConstants.kInch),
20  /** L2 height */
21  L2(4.016 + 4 * ElevatorConstants.kInch),
22  /** L3 height */
23  L3(7.257 - 4 * ElevatorConstants.kInch),
24  /** L4 height */
25  L4(9.757 + 0.3 * ElevatorConstants.kInch),
26  /** Height to score net */
27  Net(9.31 + 4 * ElevatorConstants.kInch),
28  /** Height to intake algae from lower reef */
29  LowerReef(2.0),
30  /** Height to intake algae from upper reef */
31  UpperReef(4.5 - 3 * ElevatorConstants.kInch);
32
33  protected final double position;
34
35  private ElevatorState(double position) {
36    this.position = position;
37  }
38}

Notice that the actual position values are hidden from any code outside the package frc.robot.subsystems.elevator, meaning anywhere outside the subsystem package, if a setpoint needs to be set, it must use the clear enum types rather than “magic numbers”.

Finally, we can take a look at the Elevator subsystem class:

Elevator.java
  1// Copyright (c) FIRST and other WPILib contributors.
  2// Open Source Software; you can modify and/or share it under the terms of
  3// the WPILib BSD license file in the root directory of this project.
  4
  5package frc.robot.subsystems.elevator;
  6
  7import static edu.wpi.first.units.Units.Seconds;
  8
  9import org.slf4j.Logger;
 10import org.slf4j.LoggerFactory;
 11
 12import edu.wpi.first.math.filter.Debouncer;
 13import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 14import edu.wpi.first.wpilibj2.command.Command;
 15import edu.wpi.first.wpilibj2.command.Commands;
 16import edu.wpi.first.wpilibj2.command.button.Trigger;
 17import frc.robot.Constants.CoralLevel;
 18import frc.robot.Robot;
 19import frc.robot.RobotObserver;
 20import frc.robot.subsystems.PassiveSubsystem;
 21import frc.robot.subsystems.elevator.ElevatorIO.ElevatorIOInputs;
 22import frc.robot.utils.LoopTimer;
 23import frc.robot.utils.OnboardLogger;
 24
 25public class Elevator extends PassiveSubsystem {
 26  private final ElevatorIO io;
 27  private final ElevatorIOInputs inputs;
 28
 29  private final OnboardLogger ologger;
 30
 31  private final Debouncer debouncer = new Debouncer(ElevatorConstants.kRangeDebounceTime.in(Seconds));
 32
 33  public ElevatorState reference = ElevatorState.Stow;
 34
 35  public Elevator() {
 36    super();
 37    if (Robot.isReal()) {
 38      io = new ElevatorIOHardware();
 39    } else {
 40      io = new ElevatorIOSim();
 41    }
 42    inputs = new ElevatorIOInputs();
 43  }
 44
 45  private void setPosition(ElevatorState state) {
 46    // calculate goal we should go to
 47    double goal = state.position;
 48    // floor values for the goal between our two extrema
 49    goal = Math.min(goal, ElevatorConstants.kForwardSoftLimit);
 50    goal = Math.max(goal, ElevatorConstants.kReverseSoftLimit);
 51    io.setPosition(goal);
 52    reference = state;
 53  }
 54
 55  public Trigger atSetpoint() {
 56    return new Trigger(
 57        () -> Math.abs(reference.position - inputs.position) < ElevatorConstants.kTolerance);
 58  }
 59
 60  public Trigger ready(ElevatorState state) {
 61    return atSetpoint().and(new Trigger(() -> reference.equals(state)));
 62  }
 63
 64  private boolean atZero() {
 65    return debouncer.calculate(inputs.zeroCANrangeDetected);
 66  }
 67
 68  @Override
 69  public void periodic() {
 70    io.updateInputs(inputs);
 71  }
 72
 73  /**
 74   * Whether or not the elevator is above the "safe" range
 75   */
 76  public Trigger unsafe() {
 77    return new Trigger(() -> inputs.position >= ElevatorConstants.kUnsafeRange
 78        || reference.position >= ElevatorConstants.kUnsafeRange);
 79  }
 80
 81  public Command go(ElevatorState state) {
 82    return Commands.sequence(
 83        runOnce(() -> setPosition(state)),
 84        Commands.waitUntil(atSetpoint()))
 85        .withName("Elevator(" + state.toString() + ")");
 86  }
 87
 88  /**
 89   * Automatically zeroes the elevator.
 90   */
 91  public Command autoZero() {
 92    return Commands.waitUntil(this::atZero).deadlineFor(
 93        Commands.sequence(
 94            go(ElevatorState.Zero),
 95            runOnce(io::disableLimits),
 96            runOnce(() -> io.setVoltage(ElevatorConstants.kZeroVoltage))))
 97
 98        .finallyDo(io::enableLimits)
 99        .finallyDo(interrupted -> {
100          if (!interrupted) {
101            io.calibrateZero();
102          }
103        })
104        .withName("Autozero");
105  }
106}

Note

This is a trimmed version of the subsystem. Not all the code is here, only that which is actually relevant to readythis section.