Guide to Simulating and Building a Mecanum-Wheel Omnidirectional Robot using FreeRTOS and ESP32

Simulating and Building a Mecanum‑Wheel Omnidirectional Robot using FreeRTOS and ESP32

A step‑by‑step tutorial for hobbyists and engineers who want to create a smooth‑moving, four‑wheel omnidirectional robot with real‑time multitasking.

Introduction

In recent years, omnidirectional robots have become popular for warehouse automation, research labs, and DIY projects. The Mecanum wheel provides true holonomic motion, allowing the robot to move in any direction without turning. Combining this hardware with the ESP32 microcontroller and FreeRTOS gives you a powerful, low‑cost platform capable of handling sensor fusion, motor control, and communication simultaneously.

This guide covers everything from simulation (so you can test kinematics before soldering) to hardware assembly, FreeRTOS task design, and a complete software stack. All code snippets are ready to copy into the Arduino IDE or ESP‑IDF.

Required Components

Item	Quantity	Notes
ESP32‑DevKitC (or ESP32‑S2)	1	Integrated Wi‑Fi & Bluetooth
Mecanum wheel set (4 pcs)	1	12 mm pitch, 6 mm thickness recommended
DC gear motors (≥30 RPM, 12 V)	4	Compatible with motor driver
L298N or TB6612FNG driver board	1	Dual H‑bridge, 2 A per channel
IMU (MPU‑6050 or BNO055)	1	For orientation feedback
Battery (Li‑Po 7.4 V, 2500 mAh)	1	Power both ESP32 and motors
Chassis (laser‑cut acrylic or 3D‑printed)	1	Designed for Mecanum wheel offsets

1️⃣ Simulating the Robot

Before building, validate the kinematic model in a 3‑D simulation. Gazebo or Webots are free, open‑source options. Below is a minimal Gazebo world that includes a Mecanum robot URDF.

<!-- mecanum_robot.urdf -->
<robot name="mecanum_bot">
  <link name="base_link">
    <visual>
      <geometry><box size="0.15 0.15 0.05"/></geometry>
      <material name="olive"/>
    </visual>
  </link>

  <!-- Four wheels with proper roll axis -->
  <xacro:include filename="$(find mecanum_description)/urdf/wheel.xacro"/>
  <xacro:mecanum_wheel name="wheel_front_left"  xyz="0.075 0.075 0"  rpy="0 0 0"/>
  <xacro:mecanum_wheel name="wheel_front_right" xyz="0.075 -0.075 0" rpy="0 0 0"/>
  <xacro:mecanum_wheel name="wheel_rear_left"   xyz="-0.075 0.075 0" rpy="0 0 0"/>
  <xacro:mecanum_wheel name="wheel_rear_right"  xyz="-0.075 -0.075 0" rpy="0 0 0"/>
</robot>

Run the simulation with:

roslaunch mecanum_description mecanum_world.launch

Use the /cmd_vel topic to send velocity commands and observe the robot sliding sideways, diagonally, and rotating—confirming the Mecanum kinematics.

2️⃣ Understanding Mecanum Kinematics

The core of omnidirectional motion is the inverse kinematic matrix that maps desired robot velocities (Vx, Vy, ω) to individual wheel speeds ωi.

Formula (wheel radius r, robot width w, robot length l):
[ ω1 ] [ 1 -1 -(w+l) ] [ Vx ] [ ω2 ] = [ 1 1 (w+l) ] * [ Vy ] / r [ ω3 ] [ 1 1 -(w+l) ] [ ω ] [ ω4 ] [ 1 -1 (w+l) ]

Implement this matrix in the ESP32 firmware; the result is a set of signed PWM values for each motor driver channel.

3️⃣ Setting Up FreeRTOS on ESP32

FreeRTOS ships with the ESP‑IDF and Arduino core. For this project we will use the ESP‑IDF because it gives direct access to xTaskCreatePinnedToCore and low‑latency timer interrupts.

Project Structure

─ main/
   ├─ mecanum.c      // Kinematics and PWM mapping
   ├─ motor_control.c// FreeRTOS tasks for motor PWM
   ├─ imu_task.c     // Orientation reading (I2C)
   └─ app_main.c     // System initialization

Creating Tasks

We create three core tasks: MotorTask, ImuTask, and CommTask. The motor task runs on CORE_0 for deterministic timing, while communication (Wi‑Fi, WebSocket) runs on CORE_1.

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/mcpwm.h"
#include "esp_log.h"

static const char *TAG = "MECANUM";

/* Motor PWM frequency */
#define PWM_FREQ    20000
#define PWM_RESOLUTION  10   /* 10‑bit */

void motor_task(void *arg) {
    // Configure MCPWM (4 channels)
    // … (setup omitted for brevity)
    while (true) {
        // Pull latest wheel speeds from a shared struct
        xSemaphoreTake(wheel_mutex, portMAX_DELAY);
        float speeds[4] = {wheel[0], wheel[1], wheel[2], wheel[3]};
        xSemaphoreGive(wheel_mutex);

        // Convert to duty cycle (0‑100%)
        for (int i = 0; i < 4; i++) {
            int duty = (int)(fabs(speeds[i]) * 100);
            mcpwm_set_duty(MCPWM_UNIT_0, i/2, i%2, duty);
            // Direction pin
            gpio_set_level(dir_pin[i], speeds[i] >= 0 ? 1 : 0);
        }
        vTaskDelay(pdMS_TO_TICKS(5));   // 200 Hz control loop
    }
}

void app_main(void) {
    ESP_LOGI(TAG, "Initializing Mecanum robot");
    // Init mutex
    wheel_mutex = xSemaphoreCreateMutex();

    // Create tasks pinned to cores
    xTaskCreatePinnedToCore(motor_task, "MotorTask", 4096, NULL, 5, NULL, 0);
    xTaskCreatePinnedToCore(imu_task,   "ImuTask",   3072, NULL, 4, NULL, 1);
    xTaskCreatePinnedToCore(comm_task,  "CommTask",  4096, NULL, 3, NULL, 1);
}

The motor_task runs at 200 Hz, which is fast enough for smooth PWM updates while keeping CPU load low (< 5 %).

4️⃣ Hardware Assembly

1. Mount the Mecanum wheels on the chassis, ensuring the rollers form an “X” pattern on each side.
2. Attach the gear motors to the wheel hubs with set screws. Verify free rotation.
3. Wire the motor driver to the ESP32:
   • ENA/ENB → PWM pins (e.g., GPIO 18, 19, 21, 22)
   • IN1‑IN4 → Direction pins (GPIO 23‑26)
   • Motor power → 12 V battery (use a common ground with ESP32)
4. Connect the IMU to the ESP32 SDA/SCL pins (GPIO 33 & 32) with pull‑up resistors.
5. Secure the ESP32 on the chassis using standoffs, leaving room for a USB‑C connector for programming.

Assembled chassis – wheels, driver board, and ESP32 are ready for soldering.

5️⃣ Full Software Stack

5.1 MQTT Tele‑Operation (Optional)

Expose a /cmd_vel topic over MQTT so you can control the robot from a laptop or smartphone.

#include "mqtt_client.h"

static void mqtt_event_handler(void *handler_args, esp_event_base_t base,
                               int32_t event_id, void *event_data) {
    // Parse JSON payload: {"vx":0.2,"vy":0.0,"omega":0.0}
}

void comm_task(void *arg) {
    esp_mqtt_client_config_t cfg = {
        .uri = "mqtt://broker.hivemq.com",
        .event_handle = mqtt_event_handler,
    };
    esp_mqtt_client_handle_t client = esp_mqtt_client_init(&cfg);
    esp_mqtt_client_start(client);
    while (true) {
        // Keep the task alive
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}

5.2 PID Speed Control (Optional)

If you need precise motion, add a closed‑loop PID that reads motor encoders (or use the IMU for dead‑reckoning). Below is a simple PID implementation.

typedef struct {
    float kp, ki, kd;
    float prev_error;
    float integral;
} pid_t;

float pid_compute(pid_t *pid, float setpoint, float measured, float dt) {
    float error = setpoint - measured;
    pid->integral += error * dt;
    float derivative = (error - pid->prev_error) / dt;
    pid->prev_error = error;
    return pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
}