Holonomic Drive Mathmatics Discussion

[EricS-Team180]

Originally Posted by Andrew Rudolph

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