does anyone have any three wheel holonomic drive code. I can’t get mine to work properly.
Post it and we’ll fix it!
The basic idea behind controlling a Kiwi drive robot is to take the sine and cosine of each wheel angle and multiply that by the x and y joystick values to give you the value for that wheel.
Let’s go at it from this point of view:
A /…\ B
…__
…C
("…" are just for spacing)
Wheel C doesn’t require any calculations, you simply set it equal to the left and right joystick because it’s pointing left and right, with no forward force.
Wheel B is offset 30 degrees from horizontal, meaning that most of its power is in the y direction, but some of it is in the x direction. To figure out how much it is in each, you do this:
sin(30)*joysticky - cos(30)*joystickx;
Since there are no easy trig functions in Vex and you only have to do this for 3 wheels, it’s best to just insert the values:
1/2 * joysticky - 1.73/2 * joystickx; //1.73 is an approximation for the square root of 3
Unfortunately, there are no decimals for Vex, either, so your final code would have to do something like this:
1/2 * joysticky - 173/200 * joystickx;
Wheel A is offset from horizontal by 150 degrees, so you do the same thing with slightly different numbers:
1/2 * joysticky + 173/200 * joystickx; //Notice the plus instead of the minus
This makes sense because if you’re moving the joystick to the right, the value is going up, and you want the right-facing wheel to move right also.
To make this all simpler, it may help to define these values at the top:
#define cos(30) 173/200
#define sin(30) 1/2
#define sin(150) 1/2
#define cos(150) -173/200
This is so you could use the sin and cos words in the equations and have an idea what you’re doing. Therefore it would look like this:
sin(30)*joysticky - cos(30)*joystickx;
sin(150)*joysticky - cos(150)*joystickx;
If all of the motors are oriented the same way, you have to invert 1 or two motors. Let’s say that B is pointed forward, so the only one you have to invert is A:
255 - (sin(150)*joysticky - cos(150)*joystickx);
This is the point-and-shoot code, but if you want to be able to rotate also, you have to add one more joystick into the mix. Then you simply add that joystick value to each of the motor values:
sin(30)*joysticky - cos(30)*joystickx + rotate; // B
255 - (sin(150)*joysticky - cos(150)*joystickx) + rotate; //A
joystickx + rotate; //C
One more point, the 0-255 range will not work for this because you’re multiplying values against the joystick inputs. Because of this, each of the joystick values has to be reduced by 127, and as such, the inversion we did with motor A becomes invalid. To fix this, you simply replace the 255 with a 0:
0 - (sin(150)*joysticky - cos(150)*joystickx) + rotate;
Attached is the full code in EasyC or WPILib.
kiwi.c (1.04 KB)
how do you post the code?
is there another way to do this without the trig functions? I am only in geometry.
Sure - lookup tables! You can use a spreadsheet to pre-calculate all the values you care about and then just use an array to map from input conditions to motor speed. This neatly avoids trig and floating point.
As for the size of the tables, they don’t need to include all 255 possible input values for each axis. The rx values span 0-255, but they don’t smoothly cover all the values - only about 15 or so values ever get reported. If you divide the range into around 31 values (needs to be odd so you get a “center” value), you should get sufficiently fine control and each table would take about 1KB.
You’d probably need one lookup table for each motor, though you may be able to share a single table for two motors by doing some clever axis inverting, depending on your wheel geometry.
Cheers,
- Dean
If you look at the code, he’s not really using sin() and cos(), they end up being constants in the program.
#define cos(30) 173/200
So everywhere you see “cos(30)” it subsitutes 173/200
On the other hand, Dean is right, a short trig table that you can look up the values works well. I’ve been doing this since my first microchip with pretty decent results. Back when I started doing mainframe programming the math routines use lookups to get a starting point and then did some more calculations to get it more exact. For most things in Vexland close is good enough.