Subsystems
Our robots are composed of subsystems: intakes, shooters, turrets, arms, etc.
We organize the code we use to control each subsystem into a class that implements Subsystem.
Each subsystem is built upon and integrates some existing patterns.
I recommend reading through these pages, first.
Here's a bare-bones subsystem
public class DriveBase implements Loopable {
private static final Logger log = LoggerFactory.getLogger(DriveBase.class);
private static final DriveBase mInstance = new DriveBase();
public static DriveBase getInstance() {
return mInstance;
}
public enum DriveWantedAction {
_NOTHING,
OPEN_LOOP,
}
public enum DriveState {
UN_INIT,
OPEN_LOOP,
}
private final ReentrantLock mLock = new ReentrantLock();
private TalonFX leftMotor1;
private TalonFX leftMotor2;
private TalonFX rightMotor1;
private TalonFX rightMotor2;
private DriveState mDriveState = DriveState.UN_INIT;
private DriveWantedAction mDriveWantedAction = DriveWantedAction._NOTHING;
private double mCurrentStateStartedTimestamp;
private double mLeftDriveOpenLoopDemand;
private double mRightDriveOpenLoopDemand;
private DriveBase() {
}
private DriveState init() {
leftMotor1 = new TalonFX(8);
leftMotor2 = new TalonFX(9);
rightMotor1 = new TalonFX(17);
rightMotor2 = new TalonFX(19);
rightMotor2.follow(rightMotor1);
leftMotor2.follow(leftMotor1);
leftMotor1.setInverted(TalonFXInvertType.CounterClockwise);
leftMotor2.setInverted(TalonFXInvertType.CounterClockwise);
rightMotor1.setInverted(TalonFXInvertType.Clockwise);
rightMotor2.setInverted(TalonFXInvertType.Clockwise);
commandOpenLoop(0, 0);
return DriveState.OPEN_LOOP;
}
@Override
public void loop(double timestamp) {
mLock.lock();
try {
DriveState newState = handleStateTransitions(mDriveState);
switch (newState) {
case UN_INIT:
newState = init();
break;
case OPEN_LOOP:
updateDriveOpenLoop();
break;
}
if (newState != mDriveState) {
mCurrentStateStartedTimestamp = timestamp;
mDriveState = newState;
}
} finally {
mLock.unlock();
}
}
public void setOpenLoop(double leftDemand, double rightDemand) {
mLock.lock();
try {
mDriveWantedAction = DriveWantedAction.OPEN_LOOP;
mLeftDriveOpenLoopDemand = leftDemand;
mRightDriveOpenLoopDemand = rightDemand;
} finally {
mLock.unlock();
}
}
private DriveState handleStateTransitions(DriveState currentState) {
switch (currentState) {
case UN_INIT:
// I'm in init - no one can change that
return DriveState.UN_INIT;
case OPEN_LOOP:
// Fallthrough intended
if (mDriveWantedAction == DriveWantedAction.OPEN_LOOP) {
return DriveState.OPEN_LOOP;
}
}
// Should never happen, but when in doubt return the current state
return currentState;
}
private void updateDriveOpenLoop() {
commandOpenLoop(mLeftDriveOpenLoopDemand, mRightDriveOpenLoopDemand);
}
private void commandOpenLoop(double leftDemand, double rightDemand) {
leftMotor1.set(TalonFXControlMode.PercentOutput, leftDemand);
rightMotor1.set(TalonFXControlMode.PercentOutput, rightDemand);
}
}
Here's a somewhat-pruned-down subsystem from 2023 (messier, but more feature-complete):
public class Wrist implements AcesSubsystem<Wrist.Telemetry>, BufferedTelemetryProvider<Wrist.Telemetry>, UnbufferedTelemetryProvider<Wrist.Telemetry> {
// Set up logger (see logging docs)
private static final Logger log = LoggerFactory.getLogger(Wrist.class);
// Define slot IDs for particular control loop gains
private static final int kOpenLoopSlotId = 0;
private static final int kVelocitySlotId = 0;
private static final int kPositionSlotId = 1;
// This class is a Singleton (see Singleton docs)
private static final Wrist mInstance = new Wrist();
public static Wrist getInstance() {
return mInstance;
}
private Wrist() {
}
public enum WristWantedAction {
_NOTHING,
OPEN_LOOP,
VELOCITY,
RAW_DEGREE,
}
public enum WristState {
UN_INIT,
OPEN_LOOP,
VELOCITY,
RAW_DEGREE,
}
public enum GainSet {
CLOSE,
FAR
}
private GainSet mCurrentGainSet;
private final Telemetry mTelemetry = new Telemetry();
private final DataStoreCircularBuffer<GroupDataStore> mTelemetryBuffer = new DataStoreCircularBuffer<>(mTelemetry, Telemetry::new, Constants.loopRates.kWristHz, Constants.kTelemetryDefaultSecondsToBuffer);
private final ReentrantLock mLock = new ReentrantLock();
private AcesSparkMax mWristLeader;
private WristWantedAction mWristWantedAction = WristWantedAction._NOTHING;
private WristState mWristState = WristState.UN_INIT;
private double mCurrentStateStartedTimestamp;
private double mPreviousRawDegreeSetpoint;
private double mWristOpenLoopSetpoint = 0;
private double mWristVelocitySetpoint = 0;
private double mWristRawDegreeSetpoint = 0;
private WristState init() {
mWristLeader = new AcesSparkMax(Constants.wrist.motorConfig.kWristLeaderId, "Wrist wristLeader");
mWristLeader.setEngineeringUnitOffset(0);
CANSparkMax spark = (CANSparkMax) mWristLeader.getNativeHandle();
spark.restoreFactoryDefaults(true);
configureSettings();
mTelemetry.wristLeaderStatus = mWristLeader.getMotorDataStore();
mTelemetry.wristLeaderMotionProfile = mWristLeader.getMotionProfileDataStore();
mTelemetry.fillContainedDataStoresMap();
commandOpenLoop(0);
return WristState.OPEN_LOOP;
}
public void configureSettings() {
mLock.lock();
try {
configureWristAcesSparkMax(mWristLeader);
} finally {
mLock.unlock();
}
}
private void configureWristAcesSparkMax(AcesSparkMax sparkMax) {
// Put all of your configuration code here (omitted for brevity)
}
@Override
public void loop(double timestamp) {
// TODO Auto-generated method stub
mLock.lock();
try {
WristState newState = handleStateTransitions(mWristState);
mTelemetry.currentState.put(mWristState);
mTelemetry.wristOpenLoopSetpoint.put(mWristOpenLoopSetpoint);
switch (newState) {
case UN_INIT:
newState = init();
break;
case OPEN_LOOP:
updateWristOpenLoop();
break;
case RAW_DEGREE:
updateWristRawDegree();
break;
case VELOCITY:
updateWristVelocity();
break;
}
if (newState != mWristState) {
mCurrentStateStartedTimestamp = timestamp;
mWristState = newState;
}
mWristLeader.loop(timestamp);
mWristLeader.outputTelemetry();
mTelemetry.timestamp.put(timestamp);
mTelemetryBuffer.put();
mTelemetryContainer.put();
} finally {
mLock.unlock();
}
}
// Provide methods for the rest of the code to interface with this subsystem
public double getPositionDegrees() {
return mWristLeader.getPosition();
}
public void setOpenLoop(double wristOpenLoopSetpoint) {
mLock.lock();
try {
mWristWantedAction = WristWantedAction.OPEN_LOOP;
mWristOpenLoopSetpoint = wristOpenLoopSetpoint;
} finally {
mLock.unlock();
}
}
public void setVelocitySetpoint(double wristVelocitySetpoint) {
mLock.lock();
try {
mWristWantedAction = WristWantedAction.VELOCITY;
mWristVelocitySetpoint = wristVelocitySetpoint;
} finally {
mLock.unlock();
}
}
public void setDegreeSetpoint(double wristDegreeSetpoint) {
mLock.lock();
try {
mWristWantedAction = WristWantedAction.RAW_DEGREE;
mWristRawDegreeSetpoint = wristDegreeSetpoint;
} finally {
mLock.unlock();
}
}
private WristState handleStateTransitions(WristState currentState) {
switch (currentState) {
case UN_INIT:
// I'm in init - no one can change that
return WristState.UN_INIT;
case OPEN_LOOP:
// Fallthrough intended
case RAW_DEGREE:
// Fallthrough intended
case VELOCITY:
if (mWristWantedAction == WristWantedAction.OPEN_LOOP) {
return WristState.OPEN_LOOP;
}
if (mWristWantedAction == WristWantedAction.VELOCITY) {
return WristState.VELOCITY;
}
if (mWristWantedAction == WristWantedAction.RAW_DEGREE) {
return WristState.RAW_DEGREE;
}
}
// Should never happen, but when in doubt return the current state
return currentState;
}
// Methods to handle each state
private void updateWristOpenLoop() {
commandOpenLoop(mWristOpenLoopSetpoint);
}
private void updateWristVelocity() {
commandVelocitySetpoint(mWristVelocitySetpoint);
}
private void updateWristRawDegree() {
commandRawDegreeSetpoint(mWristRawDegreeSetpoint);
}
// Methods that handle setting outputs to each motor
private void commandOpenLoop(double wristSetpoint) {
mWristLeader.setSignal(new MotorSignalWithSlotId(wristSetpoint, true, MotorSignal.Type.OPEN_LOOP, kOpenLoopSlotId));
}
private void commandVelocitySetpoint(double wristSetpoint) {
mWristLeader.setSignal(new MotorSignalWithSlotId(wristSetpoint, true, MotorSignal.Type.VELOCITY, kVelocitySlotId));
}
private void commandRawDegreeSetpoint(double setpoint) { //TODO: fix
mWristLeader.setSignal(new MotorSignalWithSlotId(
Util.limit(setpoint, Constants.wrist.kMinDegrees, Constants.wrist.kMaxDegrees),
true, MotorSignal.Type.POSITION, kPositionSlotId)); //basic
// System.out.println("commanding raw degree setpoint of " + setpoint);
mTelemetry.wristDegreeSetpoint.put(setpoint);
double rawDegreeError = Math.abs(getCurrentRawRotationDegrees() - setpoint);
mPreviousRawDegreeSetpoint = setpoint;
}
private double getCurrentRawRotationDegrees() {
return mWristLeader.getPosition();
}
public boolean isAtSetpoint() {
return Util.epsilonEquals(mWristRawDegreeSetpoint, getPositionDegrees(), Constants.wrist.kWristAtPositionEpsilon);
}
@Override
public void checkForFaults() {
}
@Override
public Telemetry getTelemetry() {
return mTelemetry;
}
@Override
public void outputTelemetry(double timestamp) {
mWristLeader.outputTelemetry();
}
@Override
public void registerTelemetryBuffersTo(BufferedTelemetryConsumer<GroupDataStore> consumer) {
consumer.registerTelemetryBuffer("wrist", mTelemetryBuffer);
}
public class Telemetry extends GroupDataStore {
public DataStore_Float timestamp = new DataStore_Float();
public DataStore_AcesMotor wristLeaderStatus = new DataStore_AcesMotor();
public DataStore_MotionProfile wristLeaderMotionProfile = new DataStore_MotionProfile();
public DataStore_Enum<WristState> currentState = new DataStore_Enum<>(WristState.UN_INIT);
public DataStore_Float wristOpenLoopSetpoint = new DataStore_Float();
public DataStore_Float wristDegreeSetpoint = new DataStore_Float();
}
}
Notes:
- The reason "update" and "command" methods are separate is that:
- Turret 2022: Uses the same
commandRawDegreeSetpointfrom four different states - Arm 2023: Consolidates "safety checks" to a common function
- Turret 2022: Uses the same