The Theory of our Autonomous Modes

This one will be short, I promise.  But I wanted to write it down where we can all get to it.

As you know, there are autonomous and driver controlled opmodes in FTC robotics. 
The obvious way to think about programming an autonomous mode is to treat it very linearly - the opmode starts, different bits of logic are put in place, and eventually you drop out the bottom like on a bobsled run or a ski slope. 
In our case, we can think of it as four ski slopes - Red "left", Red "right", Blue "left", and Blue "right."

That ski slope approach works, but it means a lot of repeated code - every sensor read, every motor drive, every turn, each one of these has some code duplicated from elsewhere.  And if you find a bug in how you handle turns in one "slope" you get to fix it for all the turns in that slope plus for all the turns in all the other slopes (or "autonomous opmodes") too. 

That can be a lot.   On the Thursday before a competition it can be too much.

Last season we took a little different approach.  We weren't doing radically different things for most of these steps.  In fact, we were really only doing 5-6 different things for even a really complicated autonomous mode.

Team 11959 had the more complicated autonomous mode last year, and it really was a stitched together sequence of "drive straight," "turn," "detect something on the ground," "stop," "detect something on the side," and "move a servo or motor."   I count that as 6 items.



BUT ... by the time you worked it all together you had 3 turns, 5 straight segments, 4 detect somethings, and 2 move a servo or motor.   14-15 steps altogether.

Here's the approach we ended up using.
Create a variable that tells you what "segment" you're in. 
Create a set of arrays.  One array for turns, one array for straight segments.  The turn array says your "turn" is how many degrees, how forceful, and whether it is clockwise or counter clockwise.

 Let's pretend we have a 6 section autonomous mode that 1) goes forward 10 inches at 25% power, turns right (clockwise) to a heading of 45 degrees at 40% power, goes straight for 50 inches at 30% power, then turns counterclockwise (left) to a heading of 2 degrees at 30% power, then sits still for other things to happen:
TurnArray =  [0, 45,  0,   2, 0,0];
TurnPower =  [0, 40,  0, -30, 0,0];
StraightPwr= [25, 0, 30,   0, 0,0];
StraightDist=[10, 0, 50,   0, 0,0];

Inside the autonomous, we run one loop that steps through each of our states.  It would look a little like this:
static final NUMSTEPS = 4;
int i;
while( i < NUMSTEPs)
{    
  stageComplete = 0;
  switch ( i ) {
    case 0,2:
      //
      stageComplete = driveForward(StraightPwr[i],StraightDist[i]); 
      break;
    case 1,3:
      stageComplete = gyroTurn(TurnArray[i], TurnPower[i]);
      break;
    case 4:
      stageComplete = detectItem();
      break
    case 5:
      stageComplete = moveLever();
      break;
   }
  if (stageComplete) i++;
}


Naturally, we then need ONE instance each of the function to "drive straight," Turn," "detectItem," and "moveLever" ... AND each one of those needs to return a boolean indicating whether the task has been done successfully.  The clever engineer puts a time limit on each one too, so we never get stuck waiting for it to push against the wall or a stuck 'bot.

This whole thing lends itself nicely to being evaluated every 50 ms inside of a "while(OpmodeIsActive)" type loop.   Another "service" that would be nice to see every 30-50 milliseconds is "readSensors()" so all of the data reads are still in one place, and happening at a speed consistent with the limits of the sensor(s).

The really nice thing is that all of the complicated code for making a turn is in one place.  All of the power compensation or other stuff for going straight is in one section, and there is ONE call to each of them.  If we change the interface, we only have to update ONE call to it in each opmode -- 4 altogether, most likely.  Much better than 30-40.

Adding more straight sections or more turns or more whatever, is a matter of populating 4-5 arrays correctly, changing one constant, and updating the switch statement.