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.