mecanum

Mecanum Drive

1. Choose Way to Get Input Depending on Controller

// pseudo-code for mecanum or omni,
// with or without field-centric control (gyro)
// first define your driver interface:
// for a 3-axis joystick (Z axis is twist), do this:
forward = -Y; // push joystick forward to go forward
right = X; // push joystick to the right to strafe right
clockwise = Z; // twist joystick clockwise turn clockwise

// ... or for two 2-axis joysticks do this (Halo):
forward = -Y1; // push joystick1 forward to go forward
right = X1; // push joystick1 to the right to strafe right
clockwise = X2; // push joystick2 to the right to rotate clockwise

// or this ("tank drive" interface plus strafe):
forward = -(Y1+Y2)/2; // push both joysticks forward to go forward
right = X2; // push joystick2 to the right to rotate right
clockwise = -(Y1-Y2)/2; // push joystick1 forward and pull joystick2 backward
// to rotate clockwise

// ... or for a single 2-axis joystick:
forward = -Y;
right = (button1)? X:0;
clockwise = (button1)? 0:X;
// note: the above can only do 2 degrees at a time.
// Button1 selects Halo or Arcade.
// ... or any other driver interface scheme you like.


// If desired, use the gyro angle for field-centric control.
// "theta" is the gyro angle, measured CCW from the zero reference:
temp = forward*cos(theta) - right*sin(theta);
right = forward*sin(theta) + right*cos(theta);
forward = temp;

1. For Any Input:

// now apply the inverse kinematic tranformation
// to convert your vehicle motion command
// to 4 wheel speed commands:
front_left = forward + clockwise + right
front_right = forward - clockwise - right;
rear_left = forward + clockwise - right;
rear_right = forward - clockwise + right;

// finally, normalize the wheel speed commands
// so that no wheel speed command exceeds magnitude of 1:
max = abs(front_left);
if (abs(front_right)>max) max = abs(front_right);
if (abs(rear_left)>max) max=abs(rear_left);
if (abs(rear_right)>max) max=abs(rear_right);
if (max>1)
{front_left/=max; front_right/=max; rear_left/=max; rear_right/=max;}

// you're done. send these four wheel commands to their respective wheels