Holonomic Drive Mathmatics Discussion

[robotaddict]

Have we all heard of holomonic drives here? If not let me expound... A holomonic drive uses things called "omniwheels" which are sort of like normal wheels, but with rollers mounted along the wheel which allow it to move perpendicular to the plane of the wheel. If you mount four of these with one pair perpendicular to the other. This allows motion in any direction from any starting position.

Now comes the interesting part, how on earth do you control this darn thing?! Sure you can move in any direction, but this would confuse any mortal driver. So I had the "brilliant" idea of using a gyro to give the robot absolute direction, that is when you push the stick away and it goes away, and vice versa, but in uncomplicating the drivers' job I have complicated my own. Now not only must the joystick inputs be scaled and realigned, but now they must be modified by the gyro's output, too. Oi. So here are some rudimentary equations...

true y input = (cos(angle from origin) * original y input) + (sin(angle from origin + 90) * original x input)

true x input = (cos(angle from origin + 90) * original x input) + (sin(angle from origin) * original y input)

You can see my dillema, especially after you add the z axis, though it should be a good deal simpler... PLEASE ADVISE! :ahh:

[The Lucas]

I developed a simple kiwi drive (wheels mounted at 45 degree angles) code for a prototype; let’s see if I remember it correctly. I used code very similar to the default 1 joystick code for translational (no spinning) movement. You can put your angle adjusted p1_x and p1_y axis into the translational code. Obviously, the motors have to be linked diagonally.

LF/---\RF
....| |
LR\---/RR

LF - left front motor
LR - left rear motor
RF - right front motor
RR - right rear motor

RF = LR = Limit_Mix(2000 + p1_y + p1_x - 127);
LF = RR = Limit_Mix(2000 + p1_y - p1_x + 127);

Now for spin:
Still it is pretty simple and much like the default code. You need a spin joystick (p2_x). Now left and right sides are pretty much linked.

RF = Limit_Mix(2000 + RF + p2_x - 127);
RR = Limit_Mix(2000 + RR + p2_x - 127);
LF = Limit_Mix(2000 + LF - p2_x + 127);
LR = Limit_Mix(2000 + LR - p2_x + 127);

As I remember, this works pretty well. Now just keep track of your gyro. Watch out for drift over time, as the error compounds. I would put some filter caps in the gyro to reduce the error. Also make sure your motors are all running in the same direction. GOOD LUCK!

[Don Wright]

Another thought I had when thinking about controlling this type of drive system using a gyro to note rotation would be a button on the OI to "Reset" the gyro to zero or something if it got out of whack.

You drive the robot so it faces the correct rotation for zero, for example, and the hit the "Reset" button and it knows that is supposed to be zero again.

Also, I was thinking that if you just used a pot for rotation and a joystick for movement, and instead of using gyros for absolute movement, you still have a front, you could do really cool thinks like effortlessly drive in a perfect circle. You would just turn the pot to rotate a little, and then drive left or right. As the robot drives left or right, it will rotate and cause you to drive in a circle. Thought that might be kind've cool. We would also have a foot switch or something to "Disable Rotation" so you could quickly stop the rotation and drive normally.

Good luck.

[Adam Y.]

You can see my dillema, especially after you add the z axis, though it should be a good deal simpler... PLEASE ADVISE!
Why do you need to be able to control the robot in the z axis? If x and y are on the ground then that would the z axis pointing up in the air.

[seanwitte]

A pretty good decription is available in the article titled "Closed Loop Motion Control for Mobile Robots" by Rich LeGrand. You can buy it for $1.50 here:

http://www.circuitcellar.com/magazine/169toc.htm

Includes all of the kinematics equations you need for a four-wheel holonomic robot.

[Rick TYler]

Why do you need to be able to control the robot in the z axis? If x and y are on the ground then that would the z axis pointing up in the air.

I believe John meant x and y to be movement in the horizontal plane and z to be rotation of orientation. Unless he wanted the robot to fly, which, knowing John, might be his intention...

[Andrew Rudolph]

Im not the programmer for our team, but we have had holonomic robots the last 2 years. We have found the best way is to have one joystick sort of the strafe joystick. push the stick forward and it makes the bot drive to the front, and pretty much it goes where ever you move the stick. Then another stick the controls the rotation. We have been devolping a system using a gyro to help correct for slippage. SPAM had a system last year that they could push the stick away from the driver and tbe bot would go away from you.

[KTorak]

wow. I wanted to know how teams did this as I saw a few at GLR with 4 omni wheels that rotated...We used them on the front of the robot and 2 drive wheels in the back and it works out well..except omni wheels crack and break after rough use (competition)...so to anyone who uses them, watch them closely for cracking and sperating...and good luck :D

[EricS-Team180]

SPAM had a system last year that they could push the stick away from the driver and tbe bot would go away from you.

yep...
That was what we called "steadySPAM", here's how it worked:

steadySPAM:
Given the joystick X and Y,
1) find the joystick angle of rotation, theta
2) find the joystick magnitude

Given the robot angular orientation psi, subtract it from theta.

From this new theta' and the magnitude,
find a new joystick X and Y

Use the new joystick X and Y to determine the inputs
for the motors.

so:
...given joyx, joyy and robot angular orientation wrt the field, theta.

1) First, I'd flip the joystick x-axis, since it hurts my head to have values
increasing to the left, then, move the origin to 0,0:

joyy1 = joyy-127
joyx1 = (255-joyxx)-127

Next, get the joystick angle of rotation:
note: ...must trap divide by zero
if joyy = 127 and joyx = 127, then
joytheta = 0 // neutral
else if joyy = 127, then
if joyx <= 127, then
joytheta = 90 // right
else
joytheta = -90 // left
else
joytheta = arctan(joyx1/joyy1)

and the magnitude:
joyspeed = sqrt(joyy1*joyy1 + joyx1*joyx1)

You can get the robot psi by using a yaw rate sensor and integrating. We defined theta and psi similar to, but not quite like a compass. Straight up field is 0deg. Rotating to the right the angle changes from 0 to 180 deg. Rotating to the left the angle changes from 0 to -180deg.

Subtract psi from theta,
joytheta' = joytheta - psi

The new joystick x and y are:
joyx' = 255 - (joyspeed*sin(joytheta')+127)
joyy' = joyspeed*cos(joytheta')+127

(Use these to calculate the motor outputs. You can use the default code mixing equation, but we included an additional 45deg rotation on top of that.)

However, using robotaddicts equations, you can do something similar and avoid an arctan and a square root:

...given joyx, joyy and robot angular orientation wrt the field, theta.

1) First, adjust the joystick axes:

joyy1 = joyy-127
joyx1 = (255-joyxx)-127

2) Next rotate the joystick axes into the orientation of the robot.

joyy2 = joyy1*cos(theta) + joyx1*sin(theta)
joyx2 = joyx1*cos(theta) - joyy1*sin(theta)

Now, get back to a typical joystick set of axes:

joyy' = joyy2+127
joyx' = 255- (joyx2+127)


This took care of translation. For rotation, we had a separate stick and applied it similar to "The Lucas"

Thanks,
Eric

[probizzle]

One quick comment that I would have is to also make a back-up drive system, which drives whatso-ever direction from the point of view of the robot. Your drivers I hope played enough video games to get this figured out (easily).

Just in case....
(and because your drivers actually might like this better, and because your absolute positioning system is going to drift, and it will be a hassle for drivers to figure out when to reset. etc.)

[seanwitte]

One quick comment that I would have is to also make a back-up drive system, which drives whatso-ever direction from the point of view of the robot. Your drivers I hope played enough video games to get this figured out (easily).

Just in case....
(and because your drivers actually might like this better, and because your absolute positioning system is going to drift, and it will be a hassle for drivers to figure out when to reset. etc.)

By using the driver's reference frame instead of the robots (move the joystick forward and the robot drives away from you, no matter which direction its facing) allows you to translate AND rotate at the same time. The other holonomic implementations don't allow you to have this "frisbee" motion. All you need is a pot or a set of buttons to adjust the robot's perceived heading if it drifts. It has the added bonus of making it easier to use the gyro as feedback to cancel unwanted rotation.

[Donut]

This drive system is so weird to program. We tried it with a normal robot this year, but didn't have enough time to finish. If you write your own code for finding the magnitude and direction of the joystick, avoid using arctan and square root functions at all costs (or make your own versions of them). We wrote our own square root function, but found out a little too late that an arctan2() call chews up 12% of your code space.

[EricS-Team180]

If you write your own code for finding the magnitude and direction of the joystick, avoid using arctan and square root functions at all costs...

We decided at the outset to use integer math, so I don't have any data on using floating point. Your experience, however, is insightful. We wrote table lookups for sine/cosine and arctan, and, like you, wrote an integer square root. We had no timing problems with the main loop.

Eric

[gc02]

You can see my dillema, especially after you add the z axis, though it should be a good deal simpler... PLEASE ADVISE! :ahh:

Here's (http://www-2.cs.cmu.edu/~reshko/PILOT/physics.html) the equations for a three wheel holonomic base. Since you're using 4 wheels that are aligned on primary axises, you should be able to simplify it somewhat.

I tried simulating a 3 wheel holonomic base, and it was virtually (no pun intended) uncontrollable without some sort of PID. I guess 4 wheels shouldn't be quite as bad though.

[The Lucas]

[URL=http://www-2.cs.cmu.edu/~reshko/PILOT/physics.html]I tried simulating a 3 wheel holonomic base, and it was virtually (no pun intended) uncontrollable without some sort of PID. I guess 4 wheels shouldn't be quite as bad though.

I think Wildstang built a 3 wheel holonomic back in 2001 or 2002. They didn't use it in competition, they just distributed a really cool video (as Raul and those guys tend to do). If they got that thing to work on one of those old PBASIC Stamps, so it shouldn’t require to much processing. Of course they wrote StangPS on a Stamp, so it might require a lot of processing.

[robotaddict]

Sorry to post and run like that, I've been a bit buzy. If you take a look above at the equations I posted your robot would go very screwy. This is turning out to be much harder than it sounded... and it sounded pretty tough to begin with! >sigh< I guess I'll just have to beat my head on it some more... at least a robot POV holonomic would be easy to program...

Oh well. Time to get the team members with more years of math than me and bend their heads around it. Untill then, however, thanks for your help, and these equations will make the third concussion of my life... :D

[Joe Ross]

I think Wildstang built a 3 wheel holonomic back in 2001 or 2002. They didn't use it in competition, they just distributed a really cool video (as Raul and those guys tend to do). If they got that thing to work on one of those old PBASIC Stamps, so it shouldn’t require to much processing. Of course they wrote StangPS on a Stamp, so it might require a lot of processing.

You're thinking of 857's Kiwi drive in 2002. http://stuweb.ee.mtu.edu/~alkrajew/FIRST/kiwi.mpg. The control was extremly simple.
http://www.chiefdelphi.com/pics/bin/1065667277stick.jpg

[robotaddict]

:) :) :)
Hooray! I have made a major breakthrough. Using my old ideas the robot could think it had an input greater than 255. What I have done is this:

cos(gyro_angle) * P1_y - sin(gyro_angle) * p1_x
cos(gyro_angle) + sin(gyro_angle)

What the bottom part does is makes it so that if both inputs are say, 255, then it boils down to (1*255)/1! This makes the scaled inputs never equal more or less than the maximum or minimum, respectively. I have left out the truncate function and the 0-centering of the inputs (and the extra +127 at the end) to simplify things, but you get the idea. No more of this :confused: here!

The conversion of these new, spiffy inputs to drive commands was a bit perplexing, too. Because the wheels look like this,

(imagine that they're at 90 degree angles to each other)
/....\

\..../

the computations are a bit tricky. I boiled it down to two major situations and two minor situations in one major. The major situations are whether or not the inputs are more along the drive axis of that set of wheels, or more not-along it. A deviation of 45 degrees or less means that the drive is more along the axis, but a deviation of the inputs by more that 45 degrees is not along the drive axis. The minor situations are whether the wheel set should be forwards or backwards when the drive is more-aligned with the axis. A topographical-style look of an inputs-to-outputs graph makes it out like so:

non-aligned w/axis:
\ \ \
\ \ \
\ \ \

aligned w/axis:
---|
--||
-|||

If you get what I mean. In the aligned w/axis case the motors drive at the greatest (or least, depending on direction) input. In the not-aligned w/axis case it becomes a simple "what's-this-angle?" and "what's this-cos?" problem.

Thank you for listening, I hope you enjoyed this. If you have any ideas on how to improve this process, just post it here and I'll get back to you. Maybe. I hope... ;)

[seanwitte]

This is based on information in the article I posted in a previous thread.

If you have four omni wheels at right-angles to one another, the following psuedo code will give you basic control of the robot. If you fix Phi, the robot's heading, at 0 then the movement will be relative to the robot. If you use a gyro and integrate the heading then the movement will be relative to the driver. Assume the wheels are labeled NORTH, EAST, SOUTH, WEST in the clockwise direction looking down on the robot. The X axis is perpendicular to the wheel labeled EAST and the Y axis is perpendicular to the wheel labeled NORTH. That means the Y axis is parallel to the axel for the NORTH/SOUTH wheels and the X axis is parallel to the axel for the EAST/WEST wheels. This is only a simplified version of the algorithm to get you started, you will have to work at it to get to to run really smoothly. Using feedback from encoders on the wheels or a gyro is almost a necessity.

A very simple way to apply the gyro feedback to remove unwanted rotation would be to compare the current gyro value to the commanded rotational velocity and add the difference to r. Not having tried this I don't know how well it will work. You can divide the error by 2 or 3 to damp is slightly without adding the differential and integral terms.

sint16 g = (sint16)((gyro >> 2) - 127);
sint16 error = r - g;
r += error;

(Ex. r = 0, g = 10. This means you want to go straight but are rotating slightly clockwise. error = (0 - 10) = -10; r += error = 0 - 10 = -10; You will be increasing the counter-clockwise rotation because you want to go straight but are rotating slightly in the clockwise direction. This is a proportional-only feedback loop with a gain of 1)

Assumptions:

1. Heading is in integer degrees in the range [0,359]
2. You have a math library that can calculate sine and cosine (I can give you one if you need it, using fixed-point decimals with 10 bits of precision).
3. You have a type library that defines sint16 as a signed int and sint32 as a signed long.

Inputs:

Vx - X axis component of the desired velocity, range [0,254]
Vy - Y axis component of the desired velocity, range [0,254]
Vr - Desired rotational velocity
Phi - Current heading

Outputs:

PWM_North - PWM value applied to the wheel labeled NORTH
PWM_South - PWM value applied to the wheel labeled SOUTH
PWM_East - PWM value applied to the wheel labeled EAST
PWM_West - PWM value applied to the wheel labeled WEST

Algorithm:

// convert inputs to signed integers
sint32 x = (sint32)pc_p1_x;
sint32 y = (sint32)pc_p1_y;
sint16 r = (sint16)pc_p2_x;
sint16 phi = robot.heading; // set phi to 0 to use robot's reference frame

// declare local working variables
sint32 sinPhi = sind(phi);
sint32 cosPhi = cosd(phi);
sint32 x2;
sint32 y2;
sint16 vNorth = 127;
sint16 vSouth = 127;
sint16 vEast = 127;
sint16 vWest = 127;

// transform velocity inputs to range [-127,127]
x -= 127;
y -= 127;
r -= 127;

// apply rotational transform to x and y velocity
x2 = (cosPhi * x) + (sinPhi * y);
y2 = -(sinPhi * x) + (cosPhi * y);

// drop the decimal from the new x and y components
x2 = ftoi(x2);
y2 = ftoi(y2);

// calculate wheel velocities
vNorth = x2 - r;
vSouth = -x2 - r;
vEast = -y2 - r ;
vWest = y2 - r;

// adjust values to match valid PWM range
pwm01 = AdjustPwm(vNorth);
pwm02 = AdjustPwm(vSouth);
pwm03 = AdjustPwm(vEast);
pwm04 = AdjustPwm(vWest);

END

// simple helper function to keep a signed PWM value inside
// the valid range

uchar AdjustPwm(sint32 val)
{
if (val > 127)
return 254;
else if (val < -127)
return 0;
else
return (uchar)(val + 127);
}

[robotaddict]

Sorry seanwitte. All that went right over my head.

Crud. The :confused: 's are back... DARN TANGENTS!!! Sin and cos are so easy to work with, so why am I getting numbers over 20,000?! Here are some various permutations of equations I have been trying... assuming that this is for inputs of robot_y > 127 and robot_x < 127.

tan(45-tan^(-1)((robot_y-127)/(robot_x-127)))*(robot_y-127)+127=FL-BR_drive

tan(90-...
tan(45-tan^(-1)((robot_y-127)/(254-robot_x)))...
tan(tan^(-1)((robot_y-127)/(robot_x-127))+45)...

You get the idea. I am soooo very lost. Maybe I should do some converting to sin's and cos's that I seem to do OK with... Luckily though, once I solve one non-aligned quadrant all the others are easy. I think I may be having some problems with tan not liking the parameters I have, but how would I know? Tan is the crazy one here, not me... right?! I may also have the problem of being the only one on my team who wants to keep going 'till at least the end of the school year, but that's for another thread... Anyhoo.

Could someone out there tell me if I got the tail anywhere near the donkey with these equations? :o

[seanwitte]

Could someone out there tell me if I got the tail anywhere near the donkey with these equations? :o

Sorry, I don't fully understand where you're going, but that doesn't mean its wrong. In the algorithm I posted above there are a few things I need to clarify. The code uses our implementation of math.c, which you can download here: math.zip (http://seanandpaige.com/math.zip). The zip file includes the math header and source files as well as our type library. Its much easier and less confusing to use "uint16" for an unsigned 16-bit integer than "unsigned int".

The sind() and cosd() functions return the sine and cosine of an angle measured in integer degrees, between 0 and 359, as a fixed-point decimal (FPD). That just means that its an integer where the lowest 10 bits are the decimal. sind() and cosd() are implemented using a lookup table of sine values for angles 0-89, where the cosine is derived from the sine. atand() is implemented as a binary search through the arctangent values of angles 0 to 90. The square root implementation is based on a algorithm I found on Microchip's web site.

You can add and subtract FPD values normally, but multiplication and division are different. Multiplication causes the decimal in the result to be as long as the sum of the decimals in the two operands. So multiplying two FPD with 10 bits of precision will result in a number with a 20 bit decimal. Division results in a decimal with the difference between the two operands, so a 10 bit decimal divided by a 10 bit decimal results in an integer. The functions mulf() and divf() handle multiplication and division for 10-bit FPD values. The funtion itof() converts an integer to a FPD and ftoi() converts a FPD back to an integer. A side effect is that multiplying an integer by a FPD results in a FPD without having to shift the result, because the product will have (0 + 10) = 10 bits of precision.

Anyways, these source files may help you make sense of the code I posted up above. If you have a spare robot controller you can try implementing the code without hooking up any motors and view the results as printf's or with a dashboard viewer. For now, just ignore the gyro integrator and set the heading, phi, to 0 all the time. Heres what happens when phi is 0:

sinPhi = 0
cosPhi = 1

x2 = (cosPhi * x) + (sinPhi * y) = x;
y2 = -(sinPhi * x) + (cosPhi * y) = y;

vNorth = x2 - r = x - r;
vSouth = -x2 - r = -x - r;
vEast = -y2 - r = -y - r;
vWest = y2 - r = y - r;

Assume you're using a joystick on port 1 for your X and Y movement (forward/backward and strafe left/right) and port 2 for your rotational component.

x = p1_x;
y = p1_y;
r = p2_x;

CASE 1: DRIVE FORWARD/BACKWARD
--------------------------------------------------------
If you want to drive straight forward or backward the inputs and outputs are as follows:

x = 127 - 127 = 0; y = 254 - 127 = 127; r = 127 - 127 = 0;
VNorth = x - r = 0;
VSouth = -x - r = 0;
VEast = -y - r = -127;
VWest = y - r = 127;

PWM_North = VNorth + 127 = 127;
PWM_South = VSouth + 127 = 127;
PWM_East = VEast + 127 = 0;
PWM_West = VWest + 127 = 254;

So, as expected, you'll be driving only the EAST and WEST wheels.

CASE 2: STRAFE RIGHT AND LEFT
--------------------------------------------------------
If you want to strafe left or right the inputs and outputs are as follows:

x = 255 - 127 = 0; y = 127 - 127 = 127; r = 127 - 127 = 0;
VNorth = x - r = 127;
VSouth = -x - r = -127;
VEast = -y - r = 0;
VWest = y - r = 0;

PWM_North = VNorth + 127 = 254;
PWM_South = VSouth + 127 = 0;
PWM_East = VEast + 127 = 127;
PWM_West = VWest + 127 = 127;

So, as expected, you'll be driving only the NORTH and SOUTH wheels.

CASE 3: ROTATE IN PLACE
--------------------------------------------------------
If you want to rotate in place, the inputs are as follows:

x = 127 - 127 = 0; y = 127 - 127 = 127; r = 254 - 127 = 127;
VNorth = x - r = -127;
VSouth = -x - r = -127;
VEast = -y - r = -127;
VWest = y - r = -127;

PWM_North = VNorth + 127 = 0;
PWM_South = VSouth + 127 = 0;
PWM_East = VEast + 127 = 0;
PWM_West = VWest + 127 = 0;

So, as expected, you'll be driving all four wheels in the same direction.

--------------------------------------------------------

If you use a gyro to integrate your current heading and apply that value as angle phi, then the control method will completely change. The robot will no longer have a "front", rather the robot will move with repect to the driver. Push the joystick away and the robot moves away, no matter which direction its facing. Push the joystick away and apply some rotation and it will "frisbee" (stole that term from Rich LeGrand) away from you.

If you apply the simple feedback loop I mentioned in my previous post, you would use the adjusted r value when you set the values of VNorth, VSouth, VEast, and VWest. In order for this system to really work well you would need to have precise control over the wheel velocities. The wheels on our robot are not evenly spaced, so the extra drag on one side makes it very difficult to drive in a straight line. Good luck.

[robotaddict]

Thanks seanwitte, it's very helpful, though I think I may have come up with a solution today at school. Here's how it goes.

Take your joystick inputs, p1_x, p1_y, and, just for kicks, let's say the joystick has a z axis too, p1_aux.

First off comes the task of putting p1_x and p1_y into headings the robot can understand. To do this, the robot needs an arbitrary front. Let's say the front is at 45 degrees to all the wheels. We'll say robot_x_approx drives the robot towards the front right and robot_y_approx drives the robot towards the front left. We get something like this:

robot_x_approx = sin(gyro_angle) * (p1_y - 127) + cos(gyro_angle) * (p1_x - 127) + 127
robot_y_approx = cos(gyro_angle) * (p1_y - 127) - sin(gyro_angle) * (p1_x - 127) + 127

Well, this is very close to 100% accurate. The problem however, is this, if p1_x and p1_y are too high or too low, we wind up with outputs greater than 255 or less than 0! This stems from sin(x) + cos(x) can be greater than 1. So then I asked, how does one go about getting this back to 1? well anything divided by itself is one, so I divided the whole thing by sin(gyro_angle) + cos(gyro_angle). This means that an input coming in at 45 degrees goes 50% one way and 50% the other instead of 70% and 70%. Take a look:

true_robot_x = (sin(gyro_angle) * (p1_y - 127) + cos(gyro_angle) * (p1_x - 127))/(sin(gyro_angle) + cos(gyro_angle)) + 127
true_robot_y = (cos(gyro_angle) * (p1_y - 127) - sin(gyro_angle) * (p1_x - 127))/(sin(gyro_angle) + cos(gyro_angle)) + 127

However, this will often give you a decimal, so you should run it through a truncate function, too. Now comes the tricky part. How does one give maximum drive in any direction without compromising simplicity of driving? Here comes our friend the if statement to the rescue. You use if statements to determine whether the robot is being driven within 45 degrees of one of its diagonal axis. Well, it always has to be driven within 45 degrees of one of its diagonal axis, so that isn't too big of a problem.

Now, however, we must ask, how do I get maximum power on the straight and diagonal axis? To do this we set the axis being driven "along" to the input with the greatest difference from zero. Some absolute value functions may be required.

We know what to do if the axis is being driven along itself, but what do we do if it isn't being driven along itself? Well we know that if a drive perpendicular to an axis is required, the axis must not drive at all, a.k.a. a value of 127. We come to the conclusion that an axis that is not being driven along must follow the other axis' lead. ENTER THE TANGENT.

More later, I have to go now. :(
But I'll be back!!! :D

[robotaddict]

Ah, where was I? Ah yes...

This is the equation for a drive axis not being driven along its drive axis.

drive_towards_FL = int(tan(45 - tan^(-1)((127 - true_robot_x)/(true_robot_y))) * drive_towards_FR) + 127

or if the FR axis is not being driven along:

drive_towards_FR = int(tan(45 - tan^(-1)((true_robot_x - 127)/(true_robot_y))) * drive_towards_FL) + 127

Isn't that just so fun?!?! Ahh... sadly though, as I understand it, the CPU is really slow at doing this sort of math, so I guess this would have to be converted into look-up tables. Oi. Oh well. That's the extent of my inspiration. Hope you can decipher what I was saying, because I would really like to know... :D

[seanwitte]

When you use the driver's reference frame you end up with a situation where the joystick limits the velocity components in certain situations. The easiest example is when the robot is rotated 45 degrees counter-clockwise and you push the stick straight up. They joystick is only letting you apply +127 as the magnitude of the velocity vector (square root of Vx^2 + Vy^2). At this orientation the robot would normally accept a velocity with magnitude up to 180 (Vx = 127 and Vy = 127). This means that the motors will max out at 70% of their full speed in certain orientations. You can either accept it or try to figure out a way to correct it.

Here is one solution I came up with. First you need to figure out the magnitude of the input vector and the angle formed by Vx and Vy (theta). Now you figure out what the maximum input vector magnitude is at angle theta and use it to calculate the percentage of the current input vector. This will give you a scaling factor, s.

theta = atand(joy_x, joy_y);
v_mag = sqrt(joy_x^2 + joy_y^2);
s = v_mag/MaxMagnitude(theta);

Now figure out what the input vector would be assuming the robot hadn't rotated. The angle will be beta = theta - phi. Now figure out what the maximum magnitude of the input vector is at angle beta, call it h. The value of h is now the maximum vector magnitude at the robot's home orientation.

beta = theta - phi;
if (beta < 0) beta += 360;
h = MaxMagnitude(beta) * s;

Your scaled input vector magnitude is now h * s. This is the hypotenuse of the triangle formed by Vx and Vy. Your new X and Y will be:

scaled_x = h * cos(theta)
scaled_y = h * sin(theta)

Use these scaled values as the inputs for Vx and Vy and the motor outputs should map for all robot orientations. The MaxMagnitude() function will be something like this VB function I used for testing (angle is in radians, not degrees):


Private Function vMax(angle As Double)
Dim a As Double

'find the equivalent angle in quadrant 1
If (angle <= (PI / 2)) Then
a = angle
ElseIf (angle <= PI) Then
a = PI - angle
ElseIf (angle <= (3 * PI / 2)) Then
a = angle - PI
Else
a = (2 * PI) - angle
End If

'find the max magnitude where x and y are in the
'range [0,127]. for angles less than 45 degrees
'the x component is 127 and for angles larger than
'45 degrees the y component is 127
If a <= PI / 4 Then
vMax = Round(127 / Cos(a), 0)
Else
vMax = Round(127 / Sin(a), 0)
End If
End Function


Attached is a VB simulator that shows the motor outputs based on the information I've posted in this thread.

[ajlapp]

You're thinking of 857's Kiwi drive in 2002. http://stuweb.ee.mtu.edu/~alkrajew/FIRST/kiwi.mpg. The control was extremly simple.

i was the team leader for 857 back then. we did compete with that drivetrain in 2002. it worked wonderfully.....and operated on unmodified code!

[greencactus3]

we have never built a omnidriven robot but if our team ever was to do that. i would just make the controls easy to operate. as in. um. think of four 1axis joysticks. for lack of a better word... as in like a throttle joystick. have them mounted to a board in a shape like

...../......\

.....\....../


basically like your omniwheels are mounted on your robot (assuming your robot is 45 degrees 4 omni robot)..
then have a relatively flexible plate connect the tops of each stick. then have another solid stick go up vertically from that plate. this is the stick the driver would be holding to drive.
so the concept is that the robot goes forwards when you press the stick forwards because each joystick would move towards the front.... hmm... i should label the sticks to make it easier to explain.

.........+...+
....A.../.....\...B
.......-........-

.......+........+
....C..\...../...D
.......-.....-

so when the stick which is connected to the plate goes forwards, ABCD all go in the + direction. so all motors go in the forwards direction.
and when you pull backwards everything goes in the - direction so it moves backwards.
and move the stick left, and A goes to -, B goes to +, C goes to +, D goes to -, and the robot goes left. same with going right.
now when you twist the stick to the clockwise direction,, you get A+,B-,C+,D-, and the whole robot rotates in the clockwise direction.

and if you havent noticed yet, the joysticks nad the plate should be binding every time you move the top plate.. which is why you need a flexible plate.... or a flexible connectors from the sticks to the plate.maybe rubber tubing?
but well its a concept. which i think is a lot easioer to make work than these crazy programming things i dont understand lol.
i hope you understood all that blabble,


-edit-
i should probably mention that each stick A,B,C,D is each controlling a victorin the common sense. + in one direction, - in the other, in between is no current.

[CJO]

A quick note,

While a have little to add to the methods of actual driving, people have been noting the problem with using trig and inverse trig functions, namely, they take up lots of processing power, or, the lookups take up lots of space.

In this thread http://www.chiefdelphi.com/forums/showthread.php?threadid=39973 Kevin Watson explains how you can write lookup table to the permanent EEPROM, so that one would no need to worry about actually calculating in real time, or filling the program space with tables.

[ajlapp]

we have never built a omnidriven robot but if our team ever was to do that. i would just make the controls easy to operate. as in. um. think of four 1axis joysticks. for lack of a better word... as in like a throttle joystick. have them mounted to a board in a shape like


http://www.chiefdelphi.com/pics/bin/1065667277stick.jpg


did this in 2002. exactly what you are talking about!

[greencactus3]

http://www.chiefdelphi.com/pics/bin/1065667277stick.jpg


did this in 2002. exactly what you are talking about!
ahh EXACTLY lol. except i was explaining for 4 sticks but same concept. how exactly are the pipes connected to the joystick? do you have a different angled picture? so yea. awseom. so why are other teams going all fancy with their programming? if i were gonna drive the omnied robot id want controls like this.

[Billfred]

ahh EXACTLY lol. except i was explaining for 4 sticks but same concept. how exactly are the pipes connected to the joystick? do you have a different angled picture? so yea. awseom. so why are other teams going all fancy with their programming? if i were gonna drive the omnied robot id want controls like this.

I've got a couple of theories....

1) That's three joystick ports taken, unless you start hacking up wires and getting some crazy soldering going. And joysticks cost money to get more and more each year (as I imagine it's hard to impossible to do such a thing and easily keep the joysticks in COTS form).

2) Going fancy with programming has the added benefits of being able to know where you are on the field, which in recent years (since that kiwi drive was built) has become quite handy for autonomous mode.

[ajlapp]

1) That's three joystick ports taken, unless you start hacking up wires and getting some crazy soldering going. And joysticks cost money to get more and more each year (as I imagine it's hard to impossible to do such a thing and easily keep the joysticks in COTS form).

we did cut those sticks and wire them into one port..........not too much trouble!

and as for using more sophisticated programming......

in 2002 we didn't have the processing power to properly calculate the coordinates. we came up with the sticks! they worked awesome......but you could do it any way you choose.

my money is on sticks though.......cheap error-proof, effective!

[seanwitte]

we did cut those sticks and wire them into one port..........not too much trouble!

and as for using more sophisticated programming......

in 2002 we didn't have the processing power to properly calculate the coordinates. we came up with the sticks! they worked awesome......but you could do it any way you choose.

my money is on sticks though.......cheap error-proof, effective!

Its a very simple and effective solution to a fairly complex problem. The only downside is that you would be unable to translate and rotate at the same time, which may or may not be worth the extra effort. If that is not a consideration then this would be hard to beat.

[greencactus3]

Its a very simple and effective solution to a fairly complex problem. The only downside is that you would be unable to translate and rotate at the same time, which may or may not be worth the extra effort. If that is not a consideration then this would be hard to beat.

no you defintly could do that. for example. push the stick to the top right while rotating the stick counterclockwise. well a better way to say iot would be to rotate the stick counterclockwise with B as the axis. then according to

.........+...+
....A.../.....\...B
.......-........-

.......+........+
....C..\...../...D
.......-.....-

C would be at -, A towards -. D towards+.
right? with B still at neutral. so then the robot would be doing the same thing.
rotating counterclockwise with B at the axis.
so now comes the place where a little more imagination is req'd.
now move the stick the same way as before(b as the axis counterclockwise) but now move the axis of rotation a little closer to the A stick. so A-,C-,D+ but B is only slightly towards +.
guess what? the robot does the same thing. the axis it is rotating at is a little above B but a littel towards A too. and it is shuffling towards NE too. i think. i confused myself slightly. forgive me. its a friday night. but i just wanted to prove you COULD translate and rotate at the same time. NO problem. just a matter of driver getting used to.

[seanwitte]

no you defintly could do that. for example. push the stick to the top right while rotating the stick counterclockwise. well a better way to say iot would be to rotate the stick counterclockwise with B as the axis. then according to

.........+...+
....A.../.....\...B
.......-........-

.......+........+
....C..\...../...D
.......-.....-

C would be at -, A towards -. D towards+.
right? with B still at neutral. so then the robot would be doing the same thing.
rotating counterclockwise with B at the axis.
so now comes the place where a little more imagination is req'd.
now move the stick the same way as before(b as the axis counterclockwise) but now move the axis of rotation a little closer to the A stick. so A-,C-,D+ but B is only slightly towards +.
guess what? the robot does the same thing. the axis it is rotating at is a little above B but a littel towards A too. and it is shuffling towards NE too. i think. i confused myself slightly. forgive me. its a friday night. but i just wanted to prove you COULD translate and rotate at the same time. NO problem. just a matter of driver getting used to.

Build it and prove me wrong. I'm certain that you will not be able to drive along a straight line while rotating at the same time. You may be able to approximate it but it would take a driver savant to keep track of the orientation and keep the translation along the same vector.

[greencactus3]

Build it and prove me wrong. I'm certain that you will not be able to drive along a straight line while rotating at the same time. You may be able to approximate it but it would take a driver savant to keep track of the orientation and keep the translation along the same vector.
i wish i had the resources to do that but i cant. but yes i agree a driver would not be able to drive in a straight line while rotating. but why would he ever need to? the field has many obstacles, and when will you ever need to rotate and move in a straight line for a longer distance than a couple meters? and as for driver? its not hard. for example. a person who flies rc helicopters should have NO problem. there's no Z in this.

[ahecht]

Now comes the interesting part, how on earth do you control this darn thing?! Sure you can move in any direction, but this would confuse any mortal driver. So I had the "brilliant" idea of using a gyro to give the robot absolute direction, that is when you push the stick away and it goes away, and vice versa, but in uncomplicating the drivers' job I have complicated my own.

This is exactly what Team 190 did with their 2005 robot. The field oriented controls allowed us to do things like rotate while translating in a straight line without causing a headache for the drivers. I am not a programmer, so I can't tell you how the code worked. I do know that we had trouble with processing speed, and that we had to convert all our trig functions to use integer math, but I don't know the details. Hopefully someone else from the team can fill you in.