[cmd3] Add a declarative state machine API on top of commands v3

[SamCarlberg]

This provides an API for writing a finite state machine compatible with the commands v3 framework. Individual states in the state machine are wrappers around command objects (which may themselves be state machines). Transitions between states are defined with a staged builder DSL similar to command builders, and uses @NoDiscard to catch partially configured transitions.

The FSM API is meant to handle highly complex cases that the fluent command chaining DSL and coroutine-based imperative commands cannot easily represent; specifically, where a command sequence may want to go back to an arbitrary previous state or skip forward to an arbitrary future state.

Here's an example from the design doc for a command that will drive to a known scoring location, aim at a scoring target, and repeatedly shoot balls until a storage hopper is empty. It also has conditions to stop shooting and move back to the scoring location if it's jostled away, and then automatically resume firing.

public Command autoWithStateMachine() {
  // Declare the state machine
  StateMachine stateMachine = new StateMachine("Auto With State Machine");

  // Define states
  State getInPosition = stateMachine.addState(drivetrain.driveToScoringLocation());
  State aiming = stateMachine.addState(turret.aimAtGoal());
  State scoring = stateMachine.addState(shooter.fireOnce());
  State celebrating = stateMachine.addState(leds.celebrate());

  // Set the initial state. Neglecting this will cause a runtime exception when the state machine starts.
  // Teams using the WPILib compiler plugin will get a compiler error if they do not set this
  stateMachine.setInitialState(getInPosition);

  // Switch to aiming when we reach the scoring location.
  getInPosition.switchTo(aiming).whenComplete();
  // Set the swerve wheels in an X shape after reaching the scoring location to resist being pushed away.
  getInPosition.onExit(() -> Scheduler.getDefault().fork(drivetrain.setX()));

  // Then start scoring once the turret is aimed at the goal.
  aiming.switchTo(scoring).when(turret::aimedAtGoal);

  // Loop the scoring state as long as the hopper has a ball.
  scoring.switchTo(scoring).whenCompleteAnd(() -> hopper.hasBall());

  // Automatically interrupt any part of the aiming or scoring sequence if
  // the robot is moved away from the scoring location and move back into position.
  stateMachine.switchFromAny(aiming, scoring).to(getInPosition).when(atScoringLocation.negate());

  // Start celebrating once the final ball has been scored.
  scoring.switchTo(celebrating).whenCompleteAnd(() -> !hopper.hasBall());

  return stateMachine;
}

A compiler check is added to detect object construction that's not followed by post-construction initializer methods (as defined by the class by placing @PostConstructionInitializer on such methods). StateMachine.setInitialState uses this to detect team code that creates a state machine but does not set its initial state.

[Daniel1464]

Looks good - I have a couple of suggestions:

1. When the javac plugin gets merged, a plugin should be added that makes sure the return types of switchTo(), switchFromAny(), etc. aren't dropped.
2. Add a whenCompleteOr().
3. Change whenComplete() to whenDone()(conveys the same meaning in less characters).

[SamCarlberg]

When the javac plugin gets merged, a plugin should be added that makes sure the return types of switchTo(), switchFromAny(), etc. aren't dropped.

This is already in place. The @NoDiscard annotations on the builder classes are enforcing it

Add a whenCompleteOr()

Seems unnecessary. You can use when and whenComplete separately for that behavior. whenCompleteAnd is only needed because there is no other way to express that behavior.

Change whenComplete() to whenDone()(conveys the same meaning in less characters).

I'm not sure if I like this. It reads a little weirdly, and is a little awkward with whenDoneAnd