Here is the code that our team uses for mechanum (in C, but easily portable)-
Code:
velocity = (p1_y - 127); //Joystick 1 y
rotation = (p1_x - 127); //Joystick 1 x
strafe = (p2_x - 127); // Joystick 2 x
wFL = velocity - rotation - strafe; //Front Left wheel, etc...
wFR = velocity + rotation + strafe;
wRL = velocity - rotation + strafe;
wRR = velocity + rotation - strafe;
pwm01 = wFL + 127;
pwm02 = 127 - wFR;
pwm03 = wRL + 127;
pwm04 = 127 - wRR;