State Machines

A state is one command that runs the whole time the machine sits in it. A transition cancels that command and starts the next one.

A state has to pose the whole robot. If Stowed only said "arm vertical," the flywheel would sit wherever the last state left it. So each state commands both mechanisms at once.

Command.parallel(a, b) starts both at once and finishes only when both have finished. Command.race ends its group on the first member to finish and cancels the rest. Two holds never finish, so neither does a parallel group. In a Command.sequencethat is a bug. Here it is fine: the machine cancels a state's command the moment a transition fires.

The constructor from last lesson gets replaced. Four imports are new: StateMachine, StateMachine.State and Scheduler from org.wpilib.command3, plus org.wpilib.system.DataLogManager.

1. StateMachine sm = new StateMachine("Superstructure"); The name is required and reaches telemetry.
2. Add one state per pose with sm.addState(...). Each call returns a State for step 4.
3. sm.setInitialState(stowed); Leave it out and gradlew build fails.
4. Wire the transitions. Declare the bare switchFromAny() last: it only covers states that exist when it runs.

1// 2. Add states. Each state owns one command. parallel(...) turns two commands into one, so
2//    each state poses the whole robot. The poses are holds that never finish - that is fine,
3//    because the machine cancels the old state's command when it switches. SpinUp is the
4//    exception: .until(...) gives its command an ending, so it can use whenComplete() below.
5State stowed =
6    sm.addState(Command.parallel(arm.vertical(), flywheel.stop()).named("Stowed (hold)"));
7State pickup =
8    sm.addState(Command.parallel(arm.horizontal(), flywheel.stop()).named("Pickup (hold)"));
9State spinUp =
10    sm.addState(
11        Command.parallel(arm.vertical(), flywheel.runFast())
12            .until(flywheel::isAtTarget)
13            .named("SpinUp until at speed"));
14State ready =
15    sm.addState(
16        Command.parallel(arm.vertical(), flywheel.runFast()).named("ReadyToShoot (hold)"));

spinUp and ready pose the robot identically. The difference is that spinUp ends, which names the moment the shot becomes legal.

1// 4. Wire the transitions. Each condition is checked every loop while its state is active,
2//    and fires the moment it flips from false to true.
3stowed.switchTo(pickup).when(driver.leftTrigger()); // driver asks to intake
4pickup.switchTo(stowed).when(driver.leftTrigger().negate()); // trigger released - pack up
5 
6stowed.switchTo(spinUp).when(driver.rightTrigger()); // driver asks to shoot
7 
8// SpinUp's command ends on its own, so this uses whenComplete(): it fires once, when the
9// command finishes. Ready runs the same pose as SpinUp - it exists so drivers, LEDs, and
10// autos can tell "spinning up" apart from "ready to shoot".
11spinUp.switchTo(ready).whenComplete();
12 
13// Releasing the right trigger backs out of either shooting state.
14sm.switchFromAny(spinUp, ready).to(stowed).when(driver.rightTrigger().negate());
15 
16// B is the escape hatch: back to stowed from ANY state.
17// switchFromAny() with no args covers every state added so far, so declare it last.
18sm.switchFromAny().to(stowed).when(driver.b());

Read the arrows and the robot's behavior fits on one screen. Notice the absence: no arrow runs from pickup to spinUp. You cannot start a shot with the arm down, because nobody drew it.

A StateMachine implements Command, so a field holds it directly. Building one still runs nothing until the scheduler is handed it.

1// Field on the OpMode - this one is new:
2private final Command machine;
3// (the driver field is already there from last lesson, unchanged)
4 
5// Last line of the constructor:
6machine = sm; // a StateMachine is just a Command - schedule it like any other
7 
8// Two overrides, below the constructor:
9@Override
10public void start() {
11  Scheduler.getDefault().schedule(machine);
12}
13 
14@Override
15public void end() {
16  Scheduler.getDefault().cancel(machine);
17}

The state settles which spelling you use, not the condition. whenComplete()fires only when a state's command ends on its own. Only spinUp qualifies: .until(flywheel::isAtTarget) gives it an ending.

A rising edge is the first loop a condition goes from false to true. It has to go false again before it fires twice, so holding B does not spin the machine. The machine checks conditions in declaration order and the first edge wins, so a second one true at the same moment never fires.

.negate() hands back a Triggerthat is true exactly when the original is false. That is how the branch writes "the driver let go." Inverting a plain method means a ! in front of your lambda: one character that reverses the whole line.

Add a pair of log markers first. A working machine and a stuck one look identical from the driver seat.

1// onEnter/onExit run small extras on the way in and out of a state, without touching the
2// state's command. These two write markers into the log, so you can see exactly when the
3// machine entered and left ReadyToShoot.
4ready.onEnter(() -> DataLogManager.log("Superstructure: entered ReadyToShoot"));
5ready.onExit(() -> DataLogManager.log("Superstructure: left ReadyToShoot"));

DataLogManager.log(...) prints to the console and writes into the .wpilog. On a transition onExit runs, then the cancel, then the next state's command, all in one loop.

1. Enable in the simulator. The arm drives to vertical and the flywheel stays stopped. That is Stowed (hold), running with no button pressed.
2. Hold the left trigger and the arm swings to horizontal. Release it and the arm comes back. That is stowed to pickup and back again.
3. Hold the right trigger. The flywheel spins up and entered ReadyToShoot lands in the console. Release it and the flywheel stops.
4. Hold the left trigger, then press B while still holding it. The arm returns to vertical and stays there. Pickup needs the trigger released and pulled again.
5. From pickup, pull the right trigger as well. Nothing happens. There is no arrow from pickup to spinUp, so that jump does not exist.