Notes on Mecanum Wheels

Spoiler Alert: It works!  Dizzy bat, anyone?

This post is basically a context rich requirements document for the next evolutionary stage of our software. That means it contains some rambling thoughts on what we need to do -- and these notes might turn out not to be very accurate.  The disclosure is now out of the way, and we can move on.

The last post discussed a "botMotors" class.  This class allows us to create methods and "variables" of type botMotors and move commands for 4 motors plus a status in one variable or return statement.

That becomes important when we want to use mecanum wheels (the link goes to the Wikipedia page) because while forward and backward are just the same, these wheels provide the ability to slew or crab left and right.  You'll see this as "strafe" in other print, but that term seems both too violent insufficiently accurate for our needs.

There are three areas we'll need to address:
1) Control of the crabbing.
2) Human interface - what control movements result in what robot movements
3) Some more sophisticated possibilities

Control of Crabbing:
With the mecanum wheels, if you drive the left wheels the same way as the right wheels, then you can turn or go straight just like today.  BUT, if you make one set of wheels turn "inward" or toward each other, and the other set of wheels turn "outward" or away from each other, then the robot will move side to side.
If we think of the robot as pointing north, what we've described so far accounts for moving it North, South, East, and West.   It stands to reason that we can make it move other directions, like North -East, by varying the power to one or more wheels.That is, instead of thinking of them in sets of two (left/right, turning inward/turning outward), we figure out what combinations are necessary for movement in any arbitrary direction between 0 and 90 degrees. The other quadrants should be reflections of this so there's no need to figure it out for the whole circle.
We can do this a couple of different ways.  We can either use trigonometry in our java code to make the calculations on the fly or calculate a basic set of vectors and add them together to get the result we need.   For example,  we can generate a table like this by using cosine and sine to calculate the x & y motion needed for any given crab angle then summing the vectors to figure out the left-front through right-rear relative power settings:

direction in degrees	Direction (in radians)	x = cos(D)	y = sin(D)	Left front	left rear	right rear	Right front	Notes
0	0	1	0	1	1	1	1	fwd: all 100%
11.25 (or pi/16 radians)	0.196	0.981	0.195	1.176	0.786	1.176	0.786	each step is pi/16 radians
22.5	0.393	0.924	0.383	1.307	0.541	1.307	0.541
33.75	0.589	0.831	0.556	1.387	0.276	1.387	0.276
45	0.785	0.707	0.707	1.414	0.001	1.414	0.001
56.25	0.982	0.555	0.832	1.387	-0.276	1.387	-0.276
67.5	1.178	0.383	0.924	1.307	-0.541	1.307	-0.541
78.75	1.374	0.196	0.981	1.176	-0.785	1.176	-0.785
90 or pi/2	1.571	0.000	1.000	1.000	-1.000	1.000	-1.000	crab straight
101.25	1.767	-0.195	0.981	0.786	-1.176	0.786	-1.176
112.5	1.963	-0.382	0.924	0.542	-1.306	0.542	-1.306
123.75	2.16	-0.556	0.831	0.276	-1.387	0.276	-1.387
135	2.356	-0.707	0.707	0.000	-1.414	0.000	-1.414
146.25	2.553	-0.832	0.555	-0.277	-1.387	-0.277	-1.387
157.5	2.749	-0.924	0.383	-0.541	-1.307	-0.541	-1.307
168.75	2.945	-0.981	0.195	-0.785	-1.176	-0.785	-1.176
180 (or pi radians)	3.142	-1.000	0.000	-1.000	-1.000	-1.000	-1.000	going back - all at -100%
191.25	3.338	-0.981	-0.195	-1.176	-0.786	-1.176	-0.786
202.5	3.534	-0.924	-0.382	-1.306	-0.542	-1.306	-0.542
213.75	3.731	-0.831	-0.556	-1.387	-0.275	-1.387	-0.275
225	3.927	-0.707	-0.707	-1.414	0.000	-1.414	0.000
236.25	4.123	-0.556	-0.831	-1.387	0.275	-1.387	0.275
247.5	4.32	-0.382	-0.924	-1.306	0.542	-1.306	0.542
258.75	4.516	-0.195	-0.981	-1.176	0.786	-1.176	0.786
270	4.712	0.000	-1.000	-1.000	1.000	-1.000	1.000	crab left
281.25	4.909	0.195	-0.981	-0.785	1.176	-0.785	1.176
292.5	5.105	0.383	-0.924	-0.541	1.307	-0.541	1.307
303.75	5.301	0.555	-0.832	-0.277	1.387	-0.277	1.387
315	5.498	0.707	-0.707	0.000	1.414	0.000	1.414
326.25	5.694	0.831	-0.556	0.276	1.387	0.276	1.387
337.5	5.89	0.924	-0.383	0.541	1.307	0.541	1.307
348.75	6.087	0.981	-0.195	0.786	1.176	0.786	1.176
360 (2pi radians)	6.283	1.000	0.000	1.000	1.000	1.000	1.000	... and fwd again at 100%

We'll have to normalize the power so all wheels are between -1.0 and 1.0, of course.

Each x = COS('Heading (in radians)' ) and each y is the corresponding SIN. Each front motor is either (x+y) or (x-y), I think. I haven't really hammered this out yet. Either way the actual setup will depend on how the wheels are installed on the robot.

I've run this through 360 degrees just to sanity check, and through one full cycle, our front and rear power commands seem like they may look like this (heading is on the right axis ... and these power settings are NOT yet normalized to +/- 1):

Or I might have badly miscalculated things and that's all wrong.  It looks right at 0, 90, 180, 270, and 360 degrees, but we'll give it a little more thought.  The good news is that it isn't as complicated as it looks. 

Sophisticated combinations

That's fine as far as it goes, and we could certainly leave the trig functions in place. But I'm stingy with processor time and I always feel that calculating cosine on the fly is going to cost a lot.  Also ... suppose we want the robot to do something cool, like a Rockford, a Bat-turn, or a J-turn.  After grabbing a block, imagine the driver punches a button and the 'bot begins to head backwards, swing a 180 and ends up pointed at the matrix, all in one smooth, continuous motion. That should be possible. But that also means combining the crab with yaw, and continually varying yaw at that.  This is where it becomes useful to express this in terms of vectors rather than carry on with the sin/cos calculations.  When we start combining types of motions - fore/aft, yaw, and crab, then the trig calculations become complicated.  The vector arithmetic stays more or less sane.
At this point, I expect we'll end up with an array like the table above, and do a little interpolation if we land between values.  Due to the expected slop in our mecanum system (friction on the rollers, friction on the playing field, and some variability in motors keep this from being super precise), we can probably get away with 16 or 32 rows on our lookup table and won't need 360.  That will be part of the voyage of discovery :-).  Here's a simpler view of what I expect after things have been normalized[1] to +/-1.  Will think about it some more, then code it up this evening or tomorrow AM.

To summarize: the numbers as presented should be a good starting point for using the mecanum wheels on their own.  When we want to add a turn or other rotational motion to them, we're going to start adding numbers together to make that happen.   But by thinking about it first as a set of vectors, we should be able to integrate that capability into this without a lot of anger management sessions. 

That leaves us with the human interface.   For both driver and autonomous modes, we need to think of what data needs to be moved from one place to another.  For example, what stick inputs translate to what motor values, and how do we make this more or less intuitive to the driver.  The immediately obvious idea is to use the Y axis of the drive joysticks to run fore/aft in tank mode just like before, then use the X axis as "crab vectors" ... the sum of the vectors of the two sticks ought to map reasonably well to the vectors as sent to the individual motor controls. Figuring out what this mapping is probably ought to be really early on our to do list, because we won't know if it works if we don't know what we expect it to do.  I want to allow the drivers all kinds of freedom, but I think they should expect to bang the sticks to the left and see the 'bot crab off that direction, same for the right.  It gets much more interesting when you start combining that with normal y-axis inputs ... which takes us back to the vectors from the "Sophisticated Combinations" paragraph.

I expect we'll see some left/right, front/back mixups, and other things like that, but on the whole, this promises to be a lot of fun.

H

[1] by normalized, I'm meaning something like this: https://en.wikipedia.org/wiki/Unit_vector
basically just restricting the min and max to -1 and +1, and keeping all the other things to the same proportions they had originally.  Again, I *think* this will suit our needs.

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.  :-)

Center Grove Competition notes - 16 Dec 2017

The 2017 Center Grove FTC Robotics competition is now history.  It was a very peculiar competition for us, as we didn’t have any hard mechanical faults nor any software “what the what!?” moments.   We also didn’t have a team make it into the finals and didn’t bring home any hardware although one team was a finalist in the “Controls” award and the other nominated for the “Rockwell Collins” award.

So …

What went right.
A whole lot of stuff actually.
1) This is the first competition I recall where we didn't need to do open heart surgery on at least one 'bot during the day.  There have been times when we've barely made it to the queue in time.
2) Also, the drivers and coaches did outstanding work today.  Really appreciate the maturity and overall ownership these students are displaying.  That's the sort of thing that makes it easy to keep coming back as a mentor.
3) I think our new approach to making code has legs.  We’re pretty close to something that is generally extensible into future seasons regardless of what those robots look like.  Further, we have the potential to do two teams with a grand total of maybe 2k lines of our own code, which really comes down to 4x500 (autonomous and driver, Juden Ki and Kernel Panic). 

Simplifying this stuff while adding functionality isn’t easy, but it’s satisfying as [expletive redacted].  So naturally there’s some more simplifying I want to try.  We’ll see how that works out.  There's lots of other stuff I want to try outside of robotics too.

Mechanically, I feel there are a couple of things we need to re-think.  These robots stayed bolted together OK, but we need to figure out how to score more points faster and repeatably … by like a factor of 4. 

Speaking of which, we have on order some Mechanum wheels ( https://www.andymark.com/PhotoDetails.asp?ShowDESC=N&ProductCode=am-3026L  ) which will allow our robots to move sideways like a crab.  This means there’s some rebuilding on the hardware side … AND, some brand new software as well.  

… and since we’re talking about software again, I created some issues on JK’s GitHub.  See them here:
https://github.com/judenkirobotics/columbusFTC_app_baseline-JudenKi/issues
One issue is about adding logic to support violent crabbing.  Hmmm… “Violent Crabs” sounds like maybe a Cajun Ska band …

We’ll have a competition debrief at this Wednesday’s practice.  I’m confident a couple of us will be doing some work between Christmas and New Year’s Day.