Re: Holomonic Drive mathmatics discussion
 

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

Re: Holomonic Drive mathmatics discussion
 

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

Re: Holomonic Drive mathmatics discussion
 

:) :) :)
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... ;)

Re: Holomonic Drive mathmatics discussion
 

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);
}

Re: Holomonic Drive mathmatics discussion
 

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

Re: Holomonic Drive mathmatics discussion
 

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. 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.

Re: Holomonic Drive mathmatics discussion
 

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

Re: Holomonic Drive mathmatics discussion
 

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

Re: Holomonic Drive mathmatics discussion
 

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):

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

Re: Holomonic Drive mathmatics discussion
 

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!

Re: Holomonic Drive mathmatics discussion
 

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.

Re: Holomonic Drive mathmatics discussion
 

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/sh...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.

Re: Holomonic Drive mathmatics discussion
 

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


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

Re: Holomonic Drive mathmatics discussion
 

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.

Re: Holomonic Drive mathmatics discussion
 

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.