ESP32 Self-Balancing Robot: Build One of the Coolest Robotics Projects

When makers start looking for cool robotics projects that bridge the gap between basic blinking LEDs and advanced autonomous systems, the inverted pendulum—specifically, a two-wheeled self-balancing robot—is the ultimate rite of passage. It forces you to deal with real-world physics, sensor noise, and high-frequency control loops. In this guide, we are building a highly responsive balancer using the ESP32, an MPU6050 IMU, and a TB6612FNG motor driver, leaving the inefficient L298N chips in the dust.

Project Overview & Difficulty Rating

The core challenge of this build is the control loop. The MPU6050 provides raw accelerometer and gyroscope data. We fuse this data using a complementary filter to find the true tilt angle, then feed that angle into a PID (Proportional-Integral-Derivative) controller running at 200Hz. The PID output dictates the PWM duty cycle sent to the motors to drive the wheels under the robot's center of gravity.

Component Spec Sheet & Bill of Materials

Selecting the right components is where most cool robotics projects fail before they even start. The TB6612FNG is mandatory here; its MOSFET-based H-bridge offers a voltage drop of only ~0.5V compared to the 2.0V+ drop of the bipolar L298N, which is critical when running on a 2S Li-ion pack.

Pin Mapping and Wiring Sequence

The ESP32's capacitive touch pins and strapping pins can cause boot failures if loaded incorrectly. The mapping below avoids GPIO 0, 2, 12, and 15 to ensure clean boot sequences and stable PWM output.

Assembly Steps

1. Prepare the Power Rail: Wire the 2S BMS output (7.4V) directly to the TB6612FNG VM and VCC pins. Tie all grounds (Battery GND, ESP32 GND, TB6612 GND, MPU6050 GND) together into a single star-ground point to prevent motor noise from resetting the ESP32.
2. Mount the IMU: The MPU6050 must be mounted exactly on the robot's center-line, as close to the axle height as possible. Use silicone standoffs to dampen high-frequency motor vibrations, which will otherwise alias into your accelerometer readings.
3. I2C Pull-ups: The GY-521 breakout has weak internal pull-ups. Solder external 4.7kΩ resistors between SDA/SCL and 3.3V to ensure clean signal edges at 400kHz I2C speeds.

Complete PID Balancing Code

This firmware targets the ESP32 DevKit V1 (ESP32-WROOM-32E). It utilizes the modern ESP32 Arduino Core v3.x ledcAttach() API for PWM generation. The code reads raw I2C registers from the MPU6050, applies a complementary filter, and executes a PD (Proportional-Derivative) loop. We omit the Integral (I) term because, in a fast-balancing system, I-windup causes violent overshoot when the robot catches itself.

#include <Wire.h>

// --- Pin Definitions ---
const int PIN_PWMA = 27;
const int PIN_AIN1 = 26;
const int PIN_AIN2 = 25;
const int PIN_PWMB = 33;
const int PIN_BIN1 = 32;
const int PIN_BIN2 = 14;
const int PIN_STBY = 13;

// --- IMU Constants ---
const int MPU_ADDR = 0x68;
const float ACCEL_SCALE = 16384.0; // +/- 2g
const float GYRO_SCALE = 131.0;    // +/- 250 deg/s

// --- PID Tuning Parameters (Adjust these for your specific chassis) ---
float Kp = 35.0; 
float Kd = 1.2;  
float targetAngle = -1.5; // Offset to account for CG shift

float angle = 0;
float lastError = 0;
unsigned long lastTime = 0;

void setup() {
  Serial.begin(115200);
  
  // Initialize Motor Driver Pins
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_BIN1, OUTPUT);
  pinMode(PIN_BIN2, OUTPUT);
  pinMode(PIN_STBY, OUTPUT);
  digitalWrite(PIN_STBY, HIGH); // Wake up TB6612FNG

  // Initialize ESP32 LEDC PWM (Core v3.x syntax)
  ledcAttach(PIN_PWMA, 1000, 10); // 1kHz, 10-bit resolution (0-1023)
  ledcAttach(PIN_PWMB, 1000, 10);

  // Initialize I2C & MPU6050
  Wire.begin(21, 22, 400000); 
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0);    // Wake up
  byte error = Wire.endTransmission();
  
  if (error != 0) {
    Serial.println("CRITICAL: MPU6050 connection failed. Check I2C wiring and pull-ups.");
    while(1) { delay(1000); } // Halt execution safely
  }
  
  // Set DLPF (Digital Low Pass Filter) to ~44Hz bandwidth to reduce noise
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1A); // CONFIG register
  Wire.write(0x03); 
  Wire.endTransmission();

  Serial.println("System Initialized. Hold robot upright to begin balancing.");
  lastTime = micros();
}

void loop() {
  unsigned long now = micros();
  float dt = (now - lastTime) / 1000000.0;
  lastTime = now;

  // Read IMU Data (Accel X/Z and Gyro Y)
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true);
  
  if (Wire.available() < 14) return; // Prevent I2C bus lockup crash

  int16_t ax = Wire.read() << 8 | Wire.read();
  int16_t ay = Wire.read() << 8 | Wire.read(); // Discard
  int16_t az = Wire.read() << 8 | Wire.read();
  int16_t temp = Wire.read() << 8 | Wire.read(); // Discard
  int16_t gx = Wire.read() << 8 | Wire.read(); // Discard
  int16_t gy = Wire.read() << 8 | Wire.read();
  int16_t gz = Wire.read() << 8 | Wire.read(); // Discard

  // Calculate Angles
  float accelAngle = atan2(ax, az) * 180.0 / PI;
  float gyroRate = gy / GYRO_SCALE;

  // Complementary Filter (98% Gyro, 2% Accel)
  angle = 0.98 * (angle + gyroRate * dt) + 0.02 * accelAngle;

  // PD Control Loop
  float error = targetAngle - angle;
  float derivative = (error - lastError) / dt;
  lastError = error;

  float pidOutput = (Kp * error) + (Kd * derivative);

  // Motor Mixing & Deadzone Compensation
  int motorSpeed = (int)pidOutput;
  if (abs(motorSpeed) > 1023) motorSpeed = 1023 * (motorSpeed > 0 ? 1 : -1);
  
  // Apply deadzone offset to overcome static friction
  int deadzone = 40; 
  if (motorSpeed > 0) motorSpeed += deadzone;
  else if (motorSpeed < 0) motorSpeed -= deadzone;

  setMotors(motorSpeed, motorSpeed);
  
  // Maintain ~200Hz loop rate
  while(micros() - now < 5000); 
}

void setMotors(int speedA, int speedB) {
  // Motor A (Left)
  if (speedA > 0) {
    digitalWrite(PIN_AIN1, HIGH);
    digitalWrite(PIN_AIN2, LOW);
  } else {
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, HIGH);
  }
  ledcWrite(PIN_PWMA, abs(speedA));

  // Motor B (Right)
  if (speedB > 0) {
    digitalWrite(PIN_BIN1, HIGH);
    digitalWrite(PIN_BIN2, LOW);
  } else {
    digitalWrite(PIN_BIN1, LOW);
    digitalWrite(PIN_BIN2, HIGH);
  }
  ledcWrite(PIN_PWMB, abs(speedB));
}

Debugging: First Three Things to Check When It Fails

Robotics projects rarely work on the first power-up. If your robot immediately falls over, oscillates violently, or the ESP32 reboots, check these three specific failure modes:

1. I2C Bus Lockup & Watchdog Panics
   Symptom: The serial monitor spits out Guru Meditation Error: Core 1 panic'ed (Interrupt wdt timeout) or the code hangs after a few seconds.
   The Fix: This happens when motor EMI corrupts the I2C clock line, causing the Wire library to wait infinitely for a clock stretch. Ensure you have 4.7kΩ physical pull-up resistors on SDA/SCL, and verify your motor power wires are routed away from the I2C sensor wires. The code above includes a Wire.available() < 14 check to prevent hard locks, but clean wiring is the only permanent fix.
2. High-Frequency Oscillation (The 'Jitter' Effect)
   Symptom: The robot stands up but the wheels vibrate back and forth rapidly, eventually shaking the robot apart.
   The Fix: Your Derivative (Kd) gain is too high, or your IMU is picking up mechanical chassis resonance. Lower Kd to 0.5. If that fails, increase the MPU6050 Digital Low Pass Filter (DLPF) bandwidth setting in the setup function from 0x03 (44Hz) to 0x04 (21Hz) to filter out motor vibration noise. Read more on IMU filtering in the TDK InvenSense MPU6050 datasheet.
3. Immediate Forward/Backward Dump
   Symptom: The robot accelerates full speed in one direction the moment you let go.
   The Fix: Your targetAngle offset is wrong. Because the battery and wiring shift the center of gravity, the true 'zero' point is rarely 0.0 degrees. Hold the robot perfectly still in your hand, read the angle variable via Serial, and update the targetAngle variable to match that resting offset.

Extending or Simplifying the Build

Depending on your skill level and end-goal, you can adapt this platform. Here is a decision matrix for modifying the build:

Building a self-balancing robot is one of those cool robotics projects that teaches you more about control theory and embedded systems than any textbook. Take your time tuning the PID values, respect the Li-ion safety protocols, and ensure your I2C bus is physically robust against motor noise.