[JefferMC]

We originally did something very much like this final solution. Then we did something more like the command group idea:

Currently we have commands for Do, Undo and Toggle. On Toggle we ask the subsystem what the state is, then do a Start() on the appropriate Do or Undo command to go the other way.

We did this because we want the command to indicate which operation is ongoing (Do or Undo) for such reasons as display on the SmartDashboard and logging.