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.