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.

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

- First, adjust the joystick axes:

joyy1 = joyy-127

joyx1 = (255-joyxx)-127

- 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