Mecanum 4WD Robot with Arduino and Servos

An Mecanum robot is a platform that combines four wheels with diagonal rollers to translate and rotate without first orienting the chassis.

Mecanum wheels (invented by Bengt Ilon in 1973) are an engineering marvel. Unlike conventional Omni wheels, their rollers are mounted at 45 degrees to the main axis.

This changes the game. While in the triangular Omni robot the forces canceled out in an obvious way, here we have a much more stable rectangular configuration capable of carrying weight, but requiring a very specific vector combination to move sideways.

The Theory: The Force in X

The secret of Mecanum wheels lies not only in the wheel itself, but in how they are mounted. Not all wheels are the same; there are “Left Wheels” and “Right Wheels” (mirrored).

For the system to work, we must mount them so that, viewed from above, the rollers form an X pointing towards the center of the robot.

• The front wheels push the ground inward.
• The rear wheels also push inward.

Vector Decomposition

When rotating, a Mecanum wheel generates a diagonal force at 45º.

• If we rotate all of them forward, the lateral components cancel out and the robot moves forward.
• If we rotate the wheels on one side inward and the wheels on the other side outward, the longitudinal components cancel out and the robot slides sideways.

The kinematics to calculate the speed of each wheel () based on the motion inputs () and rotation () is:

Watch out for this. 4 servos starting up at the same time can reset any small battery.

Connection Diagram

The wiring is similar to the previous project, but adds one motor. Servo.h can use normal digital pins; it doesn’t need them to have hardware PWM.

Servo Signals

Each wheel has its own control signal.

Servo Power Supply

Four servos starting up at the same time draw significant current. Power them from an external battery or source, with a common GND.

The Code

We will create a “Mixer” that receives our commands (go forward, go sideways, rotate) and calculates what each individual wheel must do.

A critical point that many tutorials overlook is normalization. If you ask the robot to go 100% forward () and 100% rotate (), the formula will say the FL wheel must go at speed 2. That’s impossible. If we clip to 1, we lose the proportion and the robot will make strange movements. The solution is to divide all values by the maximum if it exceeds 1.

Definitions and setup

#include <Servo.h>

// Define the 4 servos
Servo servoFL; // Front Left
Servo servoFR; // Front Right
Servo servoRL; // Rear Left
Servo servoRR; // Rear Right

// Pins
const int PIN_FL = 6;
const int PIN_FR = 9;
const int PIN_RL = 10;
const int PIN_RR = 11;

void setup() {
  Serial.begin(9600);
  
  servoFL.attach(PIN_FL);
  servoFR.attach(PIN_FR);
  servoRL.attach(PIN_RL);
  servoRR.attach(PIN_RR);
  
  stopRobot();
  delay(1000);
}

Kinematic Mixer

void moverMecanum(float x, float y, float w) {
  // x: Lateral movement (-1.0 to 1.0)
  // y: Forward movement (-1.0 to 1.0)
  // w: Rotation (-1.0 to 1.0)

  // 1. Apply the vector formulas
  // Note: Depending on your wiring, you may need to invert (+/-) some term
  float fl = y + x + w;
  float fr = y - x - w;
  float rl = y - x + w;
  float rr = y + x - w;

  // 2. Normalization (Maintains correct direction if motors saturate)
  // Find the highest absolute value
  float maxVal = max(abs(fl), max(abs(fr), max(abs(rl), abs(rr))));
  
  // If any exceeds 1.0, divide all by that maximum
  if (maxVal > 1.0) {
    fl /= maxVal;
    fr /= maxVal;
    rl /= maxVal;
    rr /= maxVal;
  }

  // 3. Send to servos
  escribirServo(servoFL, fl);
  escribirServo(servoFR, fr);
  escribirServo(servoRL, rl);
  escribirServo(servoRR, rr);
}

void escribirServo(Servo &s, float val) {
  // Map -1.0 to 1000us and 1.0 to 2000us
  // 1500us is the neutral point (stop)
  // Sometimes you need to invert (val * -500) if the wheel spins backwards
  int pwm = 1500 + (val * 500);
  s.writeMicroseconds(pwm);
}

void stopRobot() {
  moverMecanum(0, 0, 0);
}

Demonstration loop

Let’s perform the signature Mecanum maneuver: The square without turning. The robot will draw a square on the floor while always keeping its “front” facing the same direction.

void loop() {
  // 1. Forward
  moverMecanum(0, 1.0, 0); 
  delay(1000);
  
  // 2. Strafe Right
  moverMecanum(1.0, 0, 0);
  delay(1000);
  
  // 3. Backward
  moverMecanum(0, -1.0, 0);
  delay(1000);
  
  // 4. Strafe Left
  moverMecanum(-1.0, 0, 0);
  delay(1000);
  
  // 5. Spin
  moverMecanum(0, 0, 1.0);
  delay(2000);
  
  stopRobot();
  delay(2000);
}

Troubleshooting Common Issues

In this project, 99% of failures are physical.

1. The robot doesn’t move sideways, turns weird: You mounted the wheels incorrectly. Remember the X rule. The rollers must point to the center of the chassis. If they form an “O” (diamond), the robot won’t work properly.
2. A wheel doesn’t spin or the Arduino resets: Lack of current. 4 servos draw a lot. Check your batteries and the thickness of your power cables.
3. The robot drifts sideways when I want to go straight: Continuous rotation servos are not precise. One likely spins slightly faster than another. You will need to adjust the values in the code (multiplying by a correction factor, e.g., fr * 0.95).