State Machines
In particular, we use Finite-state machines. State machines allow us to model a system such that there are no unknown states.
State machines are handy to use on our robots for the following reasons:
- It allows us to fully define what state a subsystem is in, and reason about all of the possible next states. Think about the complexity of error handling, for example. In a state machine, it's just another state.
- It allows us to easily add more states in case we, the game, or the robot's mechanical system requires it.
- In our code - where it's called iteratively, it's much easier to time how long you've been in each state.
In 2014, our robot had a very large plunger as its firing mechanism. We used a state machine to control it since there were a lot of intermediary steps we needed to follow.
Our robot in 2014
Background: The goal in this game was to shoot large yoga balls. Our robot was designed with effectively a giant plunger hooked up to four 25-pound constant force springs. Think of an auto-retracting tape-measure, but much stronger.
The plunger is pulled back by a strap attached to a spool. One motor is attached to the pool, and there is a pneumatically operated clutch between the motor in the spool. The clutch allows us to effectively "let go" just as you would a crossbow.
There is a limit switch that indicates to the software when the plunger is fully pulled-back.
When we were getting this subsystem working, we discovered two things:
- We had to momentarily drive the motor out quickly in order to allow the clutch to disengage. There was too much friction otherwise.
- We couldn't re-start the arming process (pulling the plunger back) once the springs were under tension - the motor didn't have enough power.
Note: the dot at the top of the figure indicates the "origin" point - what happens when the program starts up.
As a convention, I label state transitions with "Run" when there is intentionally only one path.
We could add an "Initialization unsuccessful" arrow that loops back to UN_INIT.
An example: The Arm in 2023
There are a couple different ways that we have written state machines, the one below is the pattern we used in 2023.
It allows us to have an UN_INIT state which we use to initialize the motors, which has two benefits:
First, moving motor/subsystem initialization out of the constructor reduces the chance of having dependency issues.
Second, it allows us to change motor config while the robot is on, and then we can set the Arm to UN_INIT and it will reinitialize as if we first turned the robot on.
The following is an excerpt from the Arm in 2023
public class Arm /* ... */ {
public enum ArmWantedAction {
_NOTHING,
OPEN_LOOP,
VELOCITY,
POSITION,
TRAJECTORY, MOTION_MAGIC_TRAJECTORY,
}
public enum ArmOperationalState {
UN_INIT,
OPEN_LOOP,
VELOCITY,
POSITION,
TRAJECTORY,
MOTION_MAGIC_TRAJECTORY;
}
private ArmOperationalState mArmOperationalState = ArmOperationalState.UN_INIT;
private ArmWantedAction mArmWantedAction = ArmWantedAction._NOTHING;
private ArmOperationalState init() {
mArmBaseArmLeader = new AcesTalonFX(Constants.arm.baseArmMotorConfig.kBaseArmId, "Arm BaseArmLeader");
mArmForearmLeader = new AcesTalonFX(Constants.arm.forearmMotorConfig.kForearmId, "Arm ForearmLeader");
configureSettings();
mTelemetry.baseArmLeaderStatus = mArmBaseArmLeader.getMotorDataStore();
mTelemetry.baseArmLeaderMotionProfile = mArmBaseArmLeader.getMotionProfileDataStore();
mTelemetry.forearmLeaderStatus = mArmForearmLeader.getMotorDataStore();
mTelemetry.forearmLeaderMotionProfile = mArmForearmLeader.getMotionProfileDataStore();
mTelemetry.fillContainedDataStoresMap();
commandOpenLoop(0, 0, true);
// I didn't implement it this year, but had there been an error, I could return ArmOperationalState.UN_INIT, and
// then this init procedure would be run again. If I wanted to wait before trying again, I could trivially add a
// REINIT_WAIT state, that would transition back to UN_INIT after, say, 1 second.
// We use functions + return statements for handling states because Java will throw an error if you forget to return a
// value (say if you use an if statement)
return ArmOperationalState.OPEN_LOOP;
}
@Override
public void loop(double timestamp) {
mLock.lock();
try {
// Skip reading periodic inputs from the motors if we aren't initialized yet
// Using state makes it very easy to say if certain variables can be used or not
if(mArmOperationalState != ArmOperationalState.UN_INIT) {
mArmBaseArmLeader.readPeriodicInputs(timestamp);
mArmForearmLeader.readPeriodicInputs(timestamp);
}
mTelemetry.currentState.put(mArmOperationalState);
mTelemetry.baseArmOpenLoopSetpoint.put(mBaseArmArmOpenLoopSetpoint);
mTelemetry.forearmOpenLoopSetpoint.put(mForearmArmOpenLoopSetpoint);
mTelemetry.baseArmVelocitySetpoint.put(mBaseArmVelocitySetpoint);
mTelemetry.forearmVelocitySetpoint.put(mForearmArmOpenLoopSetpoint);
mTelemetry.baseArmPositionSetpoint.put(mBaseArmPositionSetPoint);
mTelemetry.forearmPositionSetpoint.put(mForearmPositionSetPoint);
ArmOperationalState newState = handleStateTransitions(mArmOperationalState);
// We use a "switch" statement (without a default: branch) here because Java will throw an error if we don't
// account for all of the possible states of `newState`
switch (newState) {
case UN_INIT:
// The init method needs to mutate the state. In the case that all of the motors initialize successfully,
// we will move to OPEN_LOOP with a demand of 0.
// In reality, any of the below states _could_ mutate the current state if need-be, and this is how
// we would allow them to do so
newState = init();
break;
case OPEN_LOOP:
updateArmOpenLoop();
break;
case VELOCITY:
updateArmVelocity();
break;
case POSITION:
updateArmPosition();
break;
case TRAJECTORY:
updateArmTrajectory(timestamp);
break;
case MOTION_MAGIC_TRAJECTORY:
updateArmMotionMagicTrajectory(timestamp);
break;
}
if (newState != mArmOperationalState) {
// I create a common mCurrentStateStartedTimestamp variable to make it easy to get the time that's elapsed
// since entering the current state
mCurrentStateStartedTimestamp = timestamp;
mArmOperationalState = newState;
}
mArmBaseArmLeader.loop(timestamp);
mArmForearmLeader.loop(timestamp);
mArmBaseArmLeader.outputTelemetry();
mArmForearmLeader.outputTelemetry();
var rrConfiguration = ArmAngleDisplacementConverter.fromArmState(getArmState());
mTelemetry.armLocationXY.put(UnitUtil.metersToInches(new Translation2d(rrConfiguration.endEffectorX, rrConfiguration.endEffectorY)));
rrConfiguration = ArmAngleDisplacementConverter.fromArmState(getArmSetpointState());
mTelemetry.armSetpointXY.put(UnitUtil.metersToInches(new Translation2d(rrConfiguration.endEffectorX, rrConfiguration.endEffectorY)));
mTelemetry.timestamp.put(timestamp);
mTelemetryBuffer.put();
mTelemetryContainer.put();
} finally {
mLock.unlock();
}
}
private ArmOperationalState handleStateTransitions(ArmOperationalState currentState) {
// Same as above - Java will throw an error if we don't explicitly handle every state
switch(currentState) {
case UN_INIT:
// I'm in init - no one can change that
return ArmOperationalState.UN_INIT;
case OPEN_LOOP:
// Fallthrough intended
case VELOCITY:
// Fallthrough intended
case POSITION:
// Fallthrough intended
case MOTION_MAGIC_TRAJECTORY:
// Fallthrough intended
case TRAJECTORY:
if(mArmWantedAction == ArmWantedAction.OPEN_LOOP) {
return ArmOperationalState.OPEN_LOOP;
}
if(mArmWantedAction == ArmWantedAction.VELOCITY) {
return ArmOperationalState.VELOCITY;
}
if(mArmWantedAction == ArmWantedAction.POSITION) {
return ArmOperationalState.POSITION;
}
if(mArmWantedAction == ArmWantedAction.TRAJECTORY) {
return ArmOperationalState.TRAJECTORY;
}
if(mArmWantedAction == ArmWantedAction.MOTION_MAGIC_TRAJECTORY) {
return ArmOperationalState.MOTION_MAGIC_TRAJECTORY;
}
}
// Should never happen, but when in doubt return the current state
return currentState;
}