Subsystem / Command style code

[Jon Stratis]

It was set up as a command group with a number of commands set up sequentially (you can have sequential and parallel commands) - the idea being that to climb we wanted the arm to go to position 1, then position 2, then position 3, etc. Doing this with the command group and sequential commands utilizing a PID loop was pretty easy - each command would call the PID controller until we hit the target point, then return true (in fact, we only really had 1 command that was passed in different target values).

Now, for the hard part... how do we reset the command group in the middle (either between commands, or in the middle of a command)? As we looked at it, it quickly grew extremely messy to get the type of interrupted behavior we wanted - the option to restart completely, or simply "back up" one or more commands. This was driven mostly by a couple of runs where the climb got messed up and we missed the next bar. We ended up solving that issue a different way, but it still would have been real nice to have a "back up one step" ability in the command group.