Code For Mecanum Wheels

[Adam Schuman]

I thought it was about time that Team 357 posted some of their control mixing code for Jester Drive. So here goes...

void JesterDrive(signed int JOYX, signed int JOYY, signed int JOYZ)
{
char reduction_factor = 3;
if (JOYX == 0)
{
reduction_factor--;
}
if (JOYY == 0)
{
reduction_factor--;
}
if (JOYZ == 0)
{
reduction_factor--;
}
else
{
reduction_factor = 3;
}
if(reduction_factor == 0)
{
LF = 0;
LR = 0;
RF = 0;
RR = 0;
return;
}
else
{
LF_X = JOYX /reduction_factor;
LF_Y = JOYY /reduction_factor;
LF_Z = JOYZ /reduction_factor;
}

RR_X = LF_X;
RF_X = -LF_X;
LR_X = -LF_X;

LR_Y = RF_Y = RR_Y = LF_Y;

RR_Z = -LF_Z;
RF_Z = -LF_Z;
LR_Z = LF_Z;

LF = (LF_X + LF_Y + LF_Z);
LR = (LR_X + LR_Y + LR_Z);
RF = (RF_X + RF_Y + RF_Z);
RR = (RR_X + RR_Y + RR_Z);
}


And thats about it! In order for this code to work there are several things that still need to be done...

1) Check and verify that there is no under or over flow. This means that the values for LF, LR, RF, RR must be in the range of -127 to +127.

2) Be very careful with the signed vs. unsigned numbers. Data from the OI
is unsigned data with a 127 center value and must be converted with a simple algorithm to signed data with a 0 center value.
ie. JOYX = p2_x - 127;

3) Same as number two but backwards. Data from this JesterDrive function needs to be converted back into appropriate values for output to the victors.
ie. pwm01 = LF + 127;

4) Don't forget joystick deadbands.... They help.

Also, there are some nifty simple filters to protect the motors and gearboxes but this post is too long already. I'll post it if someone asks. Team 357 Royal Assault uses the chicklet and a 3 axis joystick FYI. And if you haven't figured it out already the JesterDrive function yields complete holonomic drive capabilities.

GOOD LUCK to all... Feel free to use the code just make sure whenever borrowing any body's code to give credit where credit is due.