Code For Mecanum Wheels

[Adam Schuman]

I thought it was about time that Team 357 posted some of their control mixing code for Jester Drive. So here goes...

void JesterDrive(signed int JOYX, signed int JOYY, signed int JOYZ)
{
char reduction_factor = 3;
if (JOYX == 0)
{
reduction_factor--;
}
if (JOYY == 0)
{
reduction_factor--;
}
if (JOYZ == 0)
{
reduction_factor--;
}
else
{
reduction_factor = 3;
}
if(reduction_factor == 0)
{
LF = 0;
LR = 0;
RF = 0;
RR = 0;
return;
}
else
{
LF_X = JOYX /reduction_factor;
LF_Y = JOYY /reduction_factor;
LF_Z = JOYZ /reduction_factor;
}

RR_X = LF_X;
RF_X = -LF_X;
LR_X = -LF_X;

LR_Y = RF_Y = RR_Y = LF_Y;

RR_Z = -LF_Z;
RF_Z = -LF_Z;
LR_Z = LF_Z;

LF = (LF_X + LF_Y + LF_Z);
LR = (LR_X + LR_Y + LR_Z);
RF = (RF_X + RF_Y + RF_Z);
RR = (RR_X + RR_Y + RR_Z);
}


And thats about it! In order for this code to work there are several things that still need to be done...

1) Check and verify that there is no under or over flow. This means that the values for LF, LR, RF, RR must be in the range of -127 to +127.

2) Be very careful with the signed vs. unsigned numbers. Data from the OI
is unsigned data with a 127 center value and must be converted with a simple algorithm to signed data with a 0 center value.
ie. JOYX = p2_x - 127;

3) Same as number two but backwards. Data from this JesterDrive function needs to be converted back into appropriate values for output to the victors.
ie. pwm01 = LF + 127;

4) Don't forget joystick deadbands.... They help.

Also, there are some nifty simple filters to protect the motors and gearboxes but this post is too long already. I'll post it if someone asks. Team 357 Royal Assault uses the chicklet and a 3 axis joystick FYI. And if you haven't figured it out already the JesterDrive function yields complete holonomic drive capabilities.

GOOD LUCK to all... Feel free to use the code just make sure whenever borrowing any body's code to give credit where credit is due.

[fimmel]

thank you. im hoping that my team will build a mecanum drive soon and having the code really helps.

also im assuming the x and y axises are for translating and the z axis is for rotating....am i correct?

thanks
/forest

[Jake M]

If you mind my asking.....what are Mecanum Wheels? It's probably just my lack of vocabulary....

[Adamskiy]

Originally Posted by Jake M

If you mind my asking.....what are Mecanum Wheels? It's probably just my lack of vocabulary....

http://www.chiefdelphi.com/media/photos/tags/mecanum

Depending on what direction you rotate each wheel, they allow you to drive and turn like a normal tank drive, but also to slide/swerve in any direction

[Jake M]

Ahhhhhh.....I see.

[Lesman]

Could you please explain this code? Ours is a bit uglier (and slower)... I am our team lead for the mechanical aspect of mecanum, but I'm also a bit of a coder (java, but this code is all basic math anyway) so I wrote up our mecanum code. Here is what I have (no rotation in this version):

public static double[] printMotorOutputs(int x, int y)
{
/*
* When not turning mecanum drive has 2 logical pairs of wheels located at opposite corners.
* One of the pairs runs at a constant speed throughout a quadrant, and is scaled only on the
* basis of magnitude. The other pair of wheels change speed based on angle and magnitude.
*/

int angleDrive, magDrive, ratio;

int absX= x<127 ? -(x-127): (x-127); //make 0-255 into 0-127
int absY = y<127 ? -(y-127)y-127);

//we did the math, this SHOULD fit in 16bit integers,
if(absY<absX)
{
ratio = (absY<<7)/absX; // calculate ratio, shift to avoid floats
angleDrive = ((176-((48*ratio)>>7))*ratio)>>7; // = (176*x/y)-(48*x^2/y^2) + magic (tan(y/x) approximation)
angleDrive-=127; // shift 0-255 to -127 to 127 for scaling
angleDrive = (angleDrive*absX)>>7; // scale angleDrive * (x/128)
angleDrive+=127; // shift back
magDrive = absX; // (absX/maxValue)*outputRange = (absX/128)/128
}
else
{
ratio = (absX<<7)/absY;
angleDrive = 255-(((176-((48*ratio)>>7))*ratio)>>7);
angleDrive-=127;
angleDrive = (angleDrive*absY)>>7;
angleDrive+=127;
magDrive = absY;
}

double r1, r2, l1, l2;

if(x > 127)
{
if(y > 127)
{
//System.out.println("Q1");
r1 = angleDrive;
l2 = angleDrive;
r2 = 127+magDrive;
l1 = 127+magDrive;
}
else
{
//System.out.println("Q4");
r1 = 127-magDrive;
l2 = 127-magDrive;
r2 = 255-angleDrive;
l1 = 255-angleDrive;
}
}
else
{
if(y < 127)
{
//System.out.println("Q3");
r1 = 255-angleDrive;
l2 = 255-angleDrive;
r2 = 127-magDrive;
l1 = 127-magDrive;
}
else
{
//System.out.println("Q2");
r1 = 127+magDrive;
l2 = 127+magDrive;
r2 = angleDrive;
l1 = angleDrive;
}
}

return new double[] {(r1-127)/127, (l1-127)/127, (r2-127)/127, (l2-127)/127};
}

the doubles and such are there only because it was integrated with a gui which took a double array, for the real code the doubles were simply replaced with ints. The code is completely commented. It took me a lot of tweaking to make this run fast (yay bit shift). But the code posted looks AMAZINGLY fast, but makes absolutely no sense to me. If you could explain it I would be very grateful.

[Steve_Alaniz]

Originally Posted by Lesman

Could you please explain this code? Ours is a bit uglier (and slower)... I am our team lead for the mechanical aspect of mecanum, but I'm also a bit of a coder (java, but this code is all basic math anyway) so I wrote up our mecanum code. Here is what I have (no rotation in this version):

public static double[] printMotorOutputs(int x, int y)
{
/*
* When not turning mecanum drive has 2 logical pairs of wheels located at opposite corners.
* One of the pairs runs at a constant speed throughout a quadrant, and is scaled only on the
* basis of magnitude. The other pair of wheels change speed based on angle and magnitude.
*/

int angleDrive, magDrive, ratio;

int absX= x<127 ? -(x-127): (x-127); //make 0-255 into 0-127
int absY = y<127 ? -(y-127)y-127);

//we did the math, this SHOULD fit in 16bit integers,
if(absY<absX)
{
ratio = (absY<<7)/absX; // calculate ratio, shift to avoid floats
angleDrive = ((176-((48*ratio)>>7))*ratio)>>7; // = (176*x/y)-(48*x^2/y^2) + magic (tan(y/x) approximation)
angleDrive-=127; // shift 0-255 to -127 to 127 for scaling
angleDrive = (angleDrive*absX)>>7; // scale angleDrive * (x/128)
angleDrive+=127; // shift back
magDrive = absX; // (absX/maxValue)*outputRange = (absX/128)/128
}
else
{
ratio = (absX<<7)/absY;
angleDrive = 255-(((176-((48*ratio)>>7))*ratio)>>7);
angleDrive-=127;
angleDrive = (angleDrive*absY)>>7;
angleDrive+=127;
magDrive = absY;
}

double r1, r2, l1, l2;

if(x > 127)
{
if(y > 127)
{
//System.out.println("Q1");
r1 = angleDrive;
l2 = angleDrive;
r2 = 127+magDrive;
l1 = 127+magDrive;
}
else
{
//System.out.println("Q4");
r1 = 127-magDrive;
l2 = 127-magDrive;
r2 = 255-angleDrive;
l1 = 255-angleDrive;
}
}
else
{
if(y < 127)
{
//System.out.println("Q3");
r1 = 255-angleDrive;
l2 = 255-angleDrive;
r2 = 127-magDrive;
l1 = 127-magDrive;
}
else
{
//System.out.println("Q2");
r1 = 127+magDrive;
l2 = 127+magDrive;
r2 = angleDrive;
l1 = angleDrive;
}
}

return new double[] {(r1-127)/127, (l1-127)/127, (r2-127)/127, (l2-127)/127};
}

the doubles and such are there only because it was integrated with a gui which took a double array, for the real code the doubles were simply replaced with ints. The code is completely commented. It took me a lot of tweaking to make this run fast (yay bit shift). But the code posted looks AMAZINGLY fast, but makes absolutely no sense to me. If you could explain it I would be very grateful.

Wow... This is very intense! Are you calculating angles for tracking? My drive coding is really short so I think I may be missing something here. I Scale my inputs with a simple Square of the numbers and then I have ten lines of code that mix x,y,& z.
In a nutshell, what does this code do?

[RyanW]

Lesman,

The Jester Drive code posted by Adam takes three inputs instead of 2 - strafe (X) , forward/back (Y), and turn (Z), from -127 (full power backwards in that direction) to 127 (full power forwards in that direction). If any particular direction is 0, it ignores it. Otherwise, it takes the power in each direction, and figures out which way each wheel needs to go (for turning, left wheels and right wheels are opposite; for strafing, diagonal pairs of wheels are opposite).

Each wheel gives a diagonal vector when told to move forward or backwards, so you simply combine the vectors to get the direction you want.
The wheels should give output along the following lines:
/ \
\ /

So, for example,
127 X gives left front and right rear wheels at 127, left rear and right front wheels at -127, which would strafe to the right.
127 Y gives all wheels at 127, which would go forward.
127 Z gives left wheels at -127, right wheels at 127, which would turn left.

The rest of the code simply combines the three directions through addition, then divides by the number of directional inputs you are giving so that the final result goes from -127 to 127. Presumably, the next bit of code adds 127 to make the output an unsigned char (which is what PWMs actually take), and then sets each wheel motor to the respective variable.

[Steve_Alaniz]

I was trying to program my mechanums and was worried while trying to sleep (when I do my best thinking) and reasoned a couple of things out.
First I needed scale my inputs with one of the table schemes or with a Square or Cubic Function.
but after that I came up with THIS code.
The code for single joystick that we use combines the joystick inputs and gives a resultant. If I just treat those resultants as if they were a single joystick input, I could combine them with the same formula adding the Z or Twist axis. Since the effect of the Z axis goes to different wheels I just needed to re group them... I just realized I can further reduce the last four lines to

pwm15 = pwm16 = Limit_Mix(2000 + pwm16 + p1_wheel - 127);
pwm13 = pwm14 = Limit_Mix(2000 + pwm14 - p1_wheel + 127);

and eliminate the interim values to make it a four line code,
But I wanted to give you the code as I'm actually using it.
I need to clean up my code.

The wheels are configured :
LF is pwm13 RF is pwm14
LR is pwm15 RR is pwm16

I think this is the simplest way to implement Mechanums. Since I'm not proficient enough to read everyone
else's code, I hope all this is in line with what you guys are saying.


Steve

 pwm13 = pwm16 = Limit_Mix(2000 + p1_y + p1_x - 127);
  pwm15 = pwm14 = Limit_Mix(2000 + p1_y - p1_x + 127);


  intpwm13 = pwm13;    // set to interim variable to do the twist calculation
  intpwm14 = pwm14;    // I originally set this up for debugging purposes but 
  intpwm15 = pwm15;    // this is actually an unnecessary step.  SO I can
  intpwm16 = pwm16;    // eliminate these four lines by just using the  values
                                 // to define themselves as mentioned in the post

pwm16 = Limit_Mix(2000 + intpwm16 + p1_wheel - 127); //These can be shortened but I was in a hurry and 
pwm15 = Limit_Mix(2000 + intpwm15 + p1_wheel - 127); // I'm not really a programmer anyway.
pwm14 = Limit_Mix(2000 + intpwm14 - p1_wheel + 127);
pwm13 = Limit_Mix(2000 + intpwm13 - p1_wheel + 127);

(footnote... I believe I can combine everything to this:
pwm13 = pwm16 = Limit_Mix(2000 + p1_y + p1_x - 127); //Combines X & Y Axis
pwm15 = pwm14 = Limit_Mix(2000 + p1_y - p1_x + 127);

pwm15 = pwm16 = Limit_Mix(2000 + pwm16 + p1_wheel - 127);//Combines Z Axis with previously
pwm13 = pwm14 = Limit_Mix(2000 + pwm14 - p1_wheel + 127);//calculated X & Y Axis


I'll test it later today and let you know.
Steve )

Addendum

OK final code is

pwm13 = pwm16 = Limit_Mix(2000 + p1_y + p1_x - 127);
pwm15 = pwm14 = Limit_Mix(2000 + p1_y - p1_x + 127);



pwm16 = Limit_Mix(2000 + pwm16 + p1_wheel - 127); 
pwm15 = Limit_Mix(2000 + pwm15 + p1_wheel - 127); 
pwm14 = Limit_Mix(2000 + pwm14 - p1_wheel + 127);
pwm13 = Limit_Mix(2000 + pwm13 - p1_wheel + 127);

[Lesman]

Ok. Here is a rundown on how my code works. It is based on one major assumption: the motor outputs depend linearly on the angle of the movement (also magnitude but that is obvious). So the simple psuedocode for the first quadrant, that does not take into account magnitude, would look like this (using degrees):

(int joyX, int joyY) from 0 to 255

x = absolute val: joyX-127
y = absolute val: joyY-127

pair1 = (arctan(y/x) / 90)*255-127;
pair2 = 255;

Basically, one pair is always full forward of full backward (depending on the quadrant). The other pair of wheels goes from full forward, to full backward throughout the quadrant. For example: going forward all motors go forward, going right one pair goes forward the other goes backward, and at 45 degrees the same pair still goes forward and the other just idles.

Then you want to scale back based on distance. I found that the best way to do this was to figure out the maximum possible value on one axis, and then create a scaling ration which was equal to actual/maximumPos, and applied this to all of the motors. The maximum value for a give axis is always 127 (this is from -127 to 127) the deviding my maximunPos can be replaced by a 7 bit shift. You want to use the x axis if you are at less that 45 degrees, and y axis for more than 45 degrees, thus:

if(y<x) //that would be less than 45 degrees
multiply all values by x and bit shift 7
else
multiply all values by y and bit shift 7

So that is now scaled for distance. The last major step is replacing arctan (because it doesn't exist). With the help of excel I made an approximation of arctan(y/x)/90*127. It looks like this:
(176*x/y)-(48*x^2/y^2)
which is equivalent to:
ratio= (absY << 7) / absX
((176 - ((48 * ratio) >> 7)) * ratio) >> 7
except loads faster. Unfortunately a simple approximation like that can't do the entire arctan domain, it only does up to 45. This is where it gets nifty. An angle over 45 can be calculated by reflecting across the y=x axis (ie, flipping the x and y) and the subtracting that from 90. So the final code looks like this:

int angleDrive, magDrive, ratio;

int absX= x < 127 ? -(x - 127) : (x - 127); // make 0-255 into 0-127
int absY= y < 127 ? -(y - 127) : (y - 127);

// we did the math, this SHOULD fit in 16bit integers,
if(absY < absX)
{
ratio= (absY << 7) / absX; // calculate ratio, shift to avoid
// floats
angleDrive= ((176 - ((48 * ratio) >> 7)) * ratio) >> 7; // =
// (176*x/y)-(48*x^2/y^2)
// + magic
angleDrive-= 127; // shift 0-255 to -127 to 127 for scaling
angleDrive= (angleDrive * absX) >> 7; // scale angleDrive *
// (x/128)
angleDrive+= 127; // shift back
magDrive= absX; // (absX/maxValue)*outputRange = (absX/128)/128
}
else
{
ratio= (absX << 7) / absY;
angleDrive= 255 - (((176 - ((48 * ratio) >> 7)) * ratio) >> 7);
angleDrive-= 127;
angleDrive= (angleDrive * absY) >> 7;
angleDrive+= 127;
magDrive= absY;
}

The end.

Also! I added my original method, and jesterDrive to my gui. It shows a little gui, move the mouse around the square (or circle) to simulate the joystick. The four little graphics are the four motor outputs. To change the calculation method hit the following keys:
j for jesterDrive
o for originalMecanum
m for my final mecanum code

http://www.badongo.com/file/7796058

It is in java, packaged with complete source. I noticed that jesterDrive behaves funny, specifically, since it devides by the number of used inputs, y = 255 and x = 127 produces a significantly different result than y = 255 and x = 128.

Wow, that was long.

[jacobhurwitz]

See this thread for an explanation of some mecanum drive code:

http://www.chiefdelphi.com/forums/sh...ad.php?t=61482

The thread doesn't give any actual code, but it nicely (and simply) describes how it should work.

[JonA]

Controlling any holonomic-omnidirectional drive system is the same whether it is any number of omni-wheels or mecanum-wheels. The output of the motors are related by a sin curve. Phase shifting the sin curve by the desired heading results in the proper motor outputs for each of the wheels.

Having had a mecanum drive the last two years, we have attempted a number of control algorithms and have settled on this one. Please note that this algorithm takes advantage of Binary-Radians (256 b-rads per revolution) to simplify the trig math.

I hope that you find this useful in your attempts to control your onmi bot.

[Lesman]

Which way is more 'correct'

Basically, my method calculates the angle of motion, and uses that to generate motor outputs. Then scales everything back based on the magnitude of the motion. Afterwards it adds the rotation by simply adding the z axis to the four motor outputs, and then, if necessary, scales them back to fit in 0-255.

The other methods I have seen appropriately add up all the x,y,z inputs and and then, one way or another, scale it back to fit into 0-255 (some do it better than others).

Which of these methods produce a more desirable result (ignoring code complexity and execution time)? I personally feel that my method is more 'correct'. For example: sliding at a 45 degree angle at full magnitude is not full forward + full sideways, as that would result in root(2) magnitude (on a scale of -1 to 1) in that direction, which is obviously not possible. As opposed to my code, which handles angles, and always outputs an appropriate magnitude.

I'm no expert, but that's just my gut feeling.

[Steve_Alaniz]

Originally Posted by Lesman

Which way is more 'correct'

Without considering complexity and execution time I would say without a doubt your method is superior in that it is mathematically most correct. This would, to me, lend itself better to creating a virtual model of the motions involved in a computer or PDA display. It's a well organized and logical method for defining the motor speeds and motions.
That being said, if your code and a simpler code both give the desired results I'm not sure the word "correct" can apply. Both work so both would be "correct" but I personally admire the work and math you've applied in this problem. I just am not the brightest bulb in the hallway so I must be content with a lesser solution. (Ask me about cooking instead... I have a killer cheesecake recipe that's REALLY correct!)

Steve

[JonA]

Originally Posted by Lesman

Which way is more 'correct'

Well, neither method is more correct. The only true measures are of usability, controllability and accuracy. You can look at all sorts of data and generate all sorts of graphs and you could probably choose variations on algorithms that allow the most control and accuracy at a given range of speeds. In the end, it comes down to "does it do what you need it to do?"

Here is one easy way to look at it:

Usability is usually subjective. This includes how complicated it is for the driver to drive and, perhaps, how complicated it is for the programmers to write.

Controllability is closely tied to usability but focuses more on consistency. (ie: when I put the stick in this position, will it do the same thing?)

Accuracy is more objective and measurable. (ie: when I put my stick at 45 degrees, does the robot travel at 45 degrees?)

Originally Posted by Lesman

...sliding at a 45 degree angle at full magnitude is not full forward + full sideways, as that would result in root(2) magnitude (on a scale of -1 to 1) in that direction, which is obviously not possible.

In the end, for controllability and usability, it is usually best to limit the maximum speed in any heading to be the same. Just because you can you can drive faster in the forward direction as opposed to 45 degrees does not mean you should as when you start to turn, your speed is not proportional.