The Evolution of Autonomous

As mentioned earlier, these are largely notes to myself.  However, I'm thinking they may be useful to some other mentor(s) so what the heck.

Anyway, I've been thinking of some things we might try out in our autonomous mode.   To that end, I've made a copy of the FTC 3.5 app, and am stitching Kernel Panic's software into it.  Only thing is, I can't just sew the arm back on, I have to add an extra arm.  And a second head.  

Here's a UML diagram that outlines what I have in mind.  These arrows show inheritance.  That is, botMotors provides something to both Drive and Autonomous, Drive provides something to Autonomous:

    
FIRST (see what I did there?) I've wanted a method to return a whole bunch of values at once. Across class boundaries.  Without doing really weird things with variables' scopes.

Traditional languages use something like a data structure or a record; Java uses a class.  So ... let's create a class, call it "botMotors" just because, and make it look like this:

public class botMotors
{
    /* botMotors is a structure used to pass motor command information from

     a gyro turn or a fwd method back to the calling routine. 

     It is set up to support four independent motors. */
    float leftFront;
    float rightFront;
    float leftRear;
    float rightRear;
    int status;

    /* status of 0 means the "turn" or "fwd" method is proceeding OK 

       status of -1 should be used to indicate the arrival has been reached 

       status of -2 should be used to indicate the activity has timed out.*/}

That's it. Four motors and a status. That way, we'll be able to support the four motors that the Mechanum wheels will need.  And from either the Driver Controlled OR the Autonomous mode, all we'll have to do is give a vector and a timeout and let it worry about which wheel is going what direction (to crab, the front wheels have to turn one direction, and the rear wheels the other ... I really only want to figure that out once, and I'd like us to do the guts of it in 10-20 lines of code.  Dream big or go home).

 Now we've put some things in a class called "Drive" so let's use that. Only I'm easily distracted, so we're going to rewrite it (no offense to Jim, who earned solid cred by creating it in the first place).  All of the loops are out, all of the motor commands, all of the calls to gyros and everything else.  What we'll try now is just a real simple "go forward" and "turn" and we'll build our way up to "crab."   

Here's the "Fwd5" method, as drafted, and kindly notice that it is of type "botMotors." It's our fifth "forward" iteration, so "Fwd5" it is:

public botMotors Fwd5(double traveled,double goal,float pwr, double duration, double maxT)
{
    botMotors mPower = new botMotors();
    float cmdPwr = 0;
    float multiplier = (duration > (maxT*0.75)) ? (float) 1.2 : (float) 1.0;
    cmdPwr = pwr * multiplier / (float) 100.0;
    cmdPwr = Range.clip(cmdPwr, -1, 1);
    mPower.status = 0;
    if (traveled >= goal) mPower.status = -1;
    if (duration >= maxT) mPower.status = -2;
    if (mPower.status < 0) cmdPwr = 0;
    mPower.leftFront  = cmdPwr;
    mPower.rightFront = cmdPwr;
    mPower.leftRear   = cmdPwr;
    mPower.rightRear  = cmdPwr;
    return mPower;
}

I *think* this should work OK.  Notice this method is going to handle time-out and "arrived" slightly differently, and return a status indicating whether or not the thing is done.  

The blue stuff says that when we're 3/4 through the max allowed time, we're going to bump up the power to 20% more than requested, yet still clipped to +/-100%.

This method is invoked (see yellow hi-lighted code below) from our Autonomous class, and we do it from within a kludged state machine that uses a Case (switch) statement to figure out what happens when:

botMotors dPwr = new botMotors();

 ...

if (CurrentTime - LastNav > FRAME_PERIOD) {
    LastNav = CurrentTime;
    boolean stageComplete = false; 

    double maxT = stateDur[CurrentAutoState];
    stageTimer += FRAME_PERIOD;
    switch ( thisCase[CurrentAutoState] ) {
        case CLM:  /*Open or Close clamp on cube */ 

            leftClamp_Cmd = LEFTUNCLAMPED;
            rightClamp_Cmd = RIGHTUNCLAMPED;
            if (clampArray[CurrentAutoState] == CL) {
                leftClamp_Cmd = LEFTCLAMPED;
                rightClamp_Cmd = RIGHTCLAMPED;
            }
            if (stageTimer > stateDur[CurrentAutoState]) stageComplete = true;
            break;
        case STR:   /* Drive forward */ 

            double traveled = Math.abs(startPos - currentPos);
            double goal = StraightDist[CurrentAutoState];
            float pwr  = StraightPwr[CurrentAutoState];
            dPwr = myDrive.Fwd5(traveled, goal, pwr, stageTimer, maxT);
            leftDriveCmd = dPwr.leftFront;
            rightDriveCmd = dPwr.rightFront;
            if (dPwr.status < 0) {
                rightDriveCmd=0;
                stageComplete = true;
            }
            break;

      ...

    }

    if (stageComplete) {
        startPos = currentPos;
        startHeading = currentHeading;
        stageTimer= 0;
        CurrentAutoState++;
    }

There's lots more to it, of course,  but there's also a lot going on in just this snippet.  One of the fun things will be figuring out what "currentPos" means with four encoders that are going different ways.   We might want to use "botMotors" for that too. Hmmm...

Anyway, first note that we have ONE stage timer, and ONE array that provides the max duration for any stage.  Also, and maybe more importantly, note that we're not looking at case 0, case 1, etc. but at some sort of enumeration.  If we tracked a little earlier up in the file, we'd see this:

// states for the NAV switch statement 

final int CLM = 0;
final int TRN = 1;
final int STR = 2;
final int LFT = 3;
final int WAIT = 4;

int[] thisCase =   {  CLM,  LFT,  TRN,  STR,  CLM,  TRN, WAIT};
int[] clampArray=    { CL,    0,    0,    0,   UC,    0,    0};
double[] stateDur =  {500, 2000, 3000, 1000, 1000, 2000,  500};
int[] TurnArray =    {  0,    0,   45,    0,    2,    0,    0};
int[] TurnPower =    {  0,    0,   40,    0,  -30,    0,    0};
float[] StraightPwr= { 25,    0,    0,   30,    0,    0,    0};
int[] StraightDist=  { 10,    0,    0,   50,    0,    0,    0};

IF we can identify the actual types of discrete tasks (lift, clamp, turn, straight, and wait, for example, and IF we understand that "open" and "close" are just variations on "Clamp", and that "Go straight Forward for 50 counts at 30% power" isn't really all that different from "Go straight backward for 2000 counts at -50% power" we can define "STR" (for STRaight) as a 2, and just walk through a sequence of arrays and leave the switch statement alone. 

In other words, once the actual needs and abilities of the robot are abstracted out, adding steps to autonomous involves changing these arrays but ZERO new code.  

Pause for a few seconds, then please read that again.  Zero new code.

I haven't proved this out yet, but we've used some primitive variants and my gut feel is that this will be powerful.  We've lopped between 80-100 lines of code from the previous Autonomous - closer to 800 from last year's, and while there will certainly be a little debugging, we should be set up to support movement in whatever kind of direction we want.  

With some serious concentration and a couple of caffeine fueled coding sessions, we ought to be able to make the robot rotate about its center of mass while tracing an arc across the floor - and change it to a counterclockwise spiral with just a tweak to a couple of array values. 

I'd pay money to see that.  :-)