|
|
|
![]() |
|
|||||||
|
||||||||
|
|
Thread Tools | Rate Thread | Display Modes |
|
#6
|
||||
|
||||
|
Re: Code For Mecanum Wheels
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. |
| Thread Tools | |
| Display Modes | Rate This Thread |
|
|
Similar Threads
|
||||
| Thread | Thread Starter | Forum | Replies | Last Post |
| Mecanum wheels | akash155 | General Forum | 12 | 20-02-2007 22:15 |
| Friction coefficients for Omni wheels and Mecanum wheels from AndyMark | Andy Baker | Technical Discussion | 11 | 16-12-2006 19:40 |
| Looking for Mecanum Wheels | Armando Gonzalez | General Forum | 30 | 24-05-2006 12:54 |
| Mecanum Wheels | chaolin2007 | Control System | 1 | 25-02-2006 19:16 |
| Mixing Joystick Values for mecanum wheels | Leav | Programming | 10 | 15-01-2006 14:51 |