We split our Autonomous mode control into 2 discrete sections.
There is an 'AutonomousModeController', which is the thing that executes the selected autonomous mode. The controller itself is pretty dumb. It has a pretty simple interface
Code:
void addCmd(autoCmd_t newCmd);
void reset();
void handle();
You push in commands, which are housed in a struct (this holds a command ID, and a couple arguments... this could be an object if you want) and they get queued up and held onto. (You can use a linked list or a deque or whatever.. any kind of data structure that can act as a FIFO)
When you call handle, it pops the the newest command off the top and calls a specified function over and over again until that function returns a true, indicating that it is done. When the individual command is done, it pops the next one off and does that. Similar to 1519, we have functions for driving distances, driving to ball acquired, spinning, delaying, etc. The functions handle the logic of when they are complete. These can usually be reused from year to year. (These live in our AutoModeController, but they could really live anywhere, and they should probably be in a layer between the robot and the automodecontroller)
We also have an AutoModeSelector which keeps track of the Auto Mode 'scripts', and pushes them into the AutoModeController when asked to. The idea here is that you can use any kind of mechanism to create the auto modes (selection switches, script file, driver station input), and the thing that actually executes the auto mode should remain unchanged. We were a bit lazy and just hard coded the instructions into the selector and kept track of which one we wanted to use with an incremented variable, but there's no reason you couldn't do something fancy here.
More importantly than this though, we constructed our robot code in a manner such that everything mechanical could be controlled through a high level interface. We have functions like Robot::driveSpeedTurn(float speed, float turn) and Kicker::kick() that do exactly what you think they would do. This way, sharing robot functions between auto and teleop is super simple.