ESP32 Self-Balancing Robot Build: A Practical Guide for DIY Robotics Projects

Building a self-balancing robot is the ultimate stress test for DIY robotics projects. It forces you to simultaneously manage real-time control loops, high-current power distribution, and strict I2C timing. If your code blocks for more than a few milliseconds, the robot falls over. If your power rails sag, the microcontroller brownouts. This guide strips away the outdated tutorials and provides a modern, 2026-ready blueprint using the ESP32-WROOM-32 and a high-efficiency MOSFET motor driver.

Why the TB6612FNG Wins for Modern DIY Robotics Projects

For years, the L298N H-bridge was the default for hobby robotics. In 2026, using an L298N for a balancing robot is a guaranteed way to fail. The L298N uses bipolar junction transistors (BJTs), which drop roughly 2.0V to 2.5V across the chip. If you feed it a 7.4V LiPo, your 6V motors only see ~5V, starving them of the torque needed to catch a fall.

Instead, we use the TB6612FNG. It uses MOSFETs, dropping only about 0.5V, and supports much higher PWM frequencies, which translates to smoother motor control and less audible whining. Below is a spec-sheet comparison to justify the swap.

Hardware BOM and Exact Pin Mapping

This build targets the ESP32-WROOM-32 DevKit V1 (38-pin variant). Do not use the 30-pin variant; the pinout for the upper GPIOs differs, which will break the I2C and PWM assignments below.

Parts List

• MCU: ESP32-WROOM-32 DevKit V1 (38-pin) — $6.00
• IMU: MPU6050 Breakout (GY-521 variant) — $3.50
• Driver: TB6612FNG Dual Motor Driver breakout — $4.50
• Motors: 2x N20 Gear Motors (6V, 1000 RPM, metal gear) — $14.00/pair
• Power: 2S LiPo Battery (7.4V nominal, 1000mAh, 30C discharge) — $16.00
• Wiring: 22 AWG silicone wire for logic, 18 AWG for battery/motor feeds.

Pin Mapping Table

Assembly and Power Distribution Steps

1. Establish the Common Ground: Solder the GND wire from the 2S LiPo, the GND pin of the ESP32, the GND of the TB6612FNG, and the GND of the MPU6050 to a single central bus or terminal block. If you skip this, your I2C bus will float, and the ESP32 will hard-fault.
2. Wire the High-Current Path: Connect the LiPo positive lead to the TB6612FNG VM pin using 18 AWG wire. Do not route motor power through the ESP32's breadboard power rails; the thin copper traces will overheat and melt at 2A+ draw.
3. Logic Power: Power the ESP32 via its micro-USB/USB-C port for debugging, or wire the LiPo through a buck converter (set to exactly 5.0V) into the ESP32 VIN pin. Wire 3.3V from the ESP32 to the MPU6050 VCC and the TB6612FNG VCC logic pins.
4. Mount the IMU: Secure the MPU6050 breakout to the exact center of rotation of your chassis using double-sided foam tape. The foam dampens high-frequency motor vibrations that will otherwise inject noise into your accelerometer readings.
5. Verify with a Multimeter: Before plugging in the ESP32, use your multimeter to check for continuity between the battery negative terminal and the ESP32 GND pin. It should read < 1 ohm.

The Control Loop: Self-Contained ESP32 PID Code

Most tutorials rely on heavy external libraries (like Jeff Rowberg's I2Cdev) or the Arduino PID_v1 library. To eliminate "library not found" compilation errors and reduce loop latency, the code below implements a raw complementary filter and a custom PID struct using only the native Wire.h library. It utilizes the ESP32's native LEDC (LED Control) peripheral for hardware PWM, which is far more stable than analogWrite().

#include <Wire.h>

// --- PIN DEFINITIONS (ESP32 DevKit V1 38-Pin) ---
const int PIN_SDA = 21;
const int PIN_SCL = 22;
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 & FILTER CONSTANTS ---
const uint8_t MPU_ADDR = 0x68;
const float DT = 0.005; // 5ms loop time (200Hz)
const float ALPHA = 0.98; // Complementary filter coefficient
float pitch = 0.0;

// --- PID TUNING PARAMETERS ---
// Adjust these based on your specific chassis mass and motor torque
float Kp = 35.0; 
float Ki = 0.5;  
float Kd = 1.2;  
float targetAngle = -1.5; // Mechanical offset (find empirically)
float integral = 0.0;
float prevError = 0.0;

// --- LEDC PWM CONFIG ---
const int LEDC_CH_A = 0;
const int LEDC_CH_B = 1;
const int LEDC_FREQ = 1000;
const int LEDC_RES = 10; // 10-bit resolution (0-1023)

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); // Enable TB6612FNG

  // Configure ESP32 LEDC Hardware PWM
  ledcSetup(LEDC_CH_A, LEDC_FREQ, LEDC_RES);
  ledcSetup(LEDC_CH_B, LEDC_FREQ, LEDC_RES);
  ledcAttachPin(PIN_PWMA, LEDC_CH_A);
  ledcAttachPin(PIN_PWMB, LEDC_CH_B);

  // Initialize I2C at 400kHz Fast Mode
  Wire.begin(PIN_SDA, PIN_SCL, 400000);
  
  // Wake up MPU6050 (it starts in sleep mode)
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0);    // Set to 0 to wake
  Wire.endTransmission(true);
  
  delay(100);
  Serial.println("System Initialized. Balancing...");
}

void loop() {
  // 1. Request 14 bytes from MPU6050 (Accel X,Y,Z + Temp + Gyro X,Y,Z)
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Starting with register 0x3B (ACCEL_XOUT_H)
  uint8_t i2cError = Wire.endTransmission(false); // Send restart condition
  
  // ERROR HANDLING: Check for I2C NACK
  if (i2cError != 0) {
    Serial.printf("MPU6050 I2C read failed. Wire.endTransmission() returned: %d\n", i2cError);
    stopMotors(); // Safety cutoff
    delay(10);    // Prevent serial flood
    return;       // Skip this loop iteration
  }

  Wire.requestFrom(MPU_ADDR, 14, true);
  
  int16_t AcX = Wire.read()<<8 | Wire.read();
  int16_t AcY = Wire.read()<<8 | Wire.read();
  int16_t AcZ = Wire.read()<<8 | Wire.read();
  Wire.read(); Wire.read(); // Skip Temperature
  int16_t GyX = Wire.read()<<8 | Wire.read();
  int16_t GyY = Wire.read()<<8 | Wire.read();
  int16_t GyZ = Wire.read()<<8 | Wire.read();

  // 2. Calculate Angles (Raw values based on default +/- 2g and +/- 250 deg/s scales)
  float accelAngle = atan2(AcY, AcZ) * 180.0 / PI;
  float gyroRate = GyX / 131.0; // deg/s

  // 3. Complementary Filter
  pitch = ALPHA * (pitch + gyroRate * DT) + (1.0 - ALPHA) * accelAngle;

  // 4. PID Calculation
  float error = pitch - targetAngle;
  integral += error * DT;
  // Anti-windup: clamp integral
  if (integral > 200.0) integral = 200.0;
  if (integral < -200.0) integral = -200.0;
  
  float derivative = (error - prevError) / DT;
  prevError = error;

  float output = (Kp * error) + (Ki * integral) + (Kd * derivative);

  // 5. Motor Mixing and Execution
  driveMotors(output);
  
  // Maintain strict 5ms loop timing
  delay(5); 
}

void driveMotors(float pidOut) {
  int speed = abs(pidOut);
  if (speed > 1023) speed = 1023; // Clamp to 10-bit max
  
  // Deadzone compensation (motors won't spin below ~50 PWM)
  if (speed < 50 && abs(pidOut) > 5) speed = 50; 

  if (pidOut > 0) { // Forward
    digitalWrite(PIN_AIN1, HIGH); digitalWrite(PIN_AIN2, LOW);
    digitalWrite(PIN_BIN1, HIGH); digitalWrite(PIN_BIN2, LOW);
  } else {          // Backward
    digitalWrite(PIN_AIN1, LOW);  digitalWrite(PIN_AIN2, HIGH);
    digitalWrite(PIN_BIN1, LOW);  digitalWrite(PIN_BIN2, HIGH);
  }
  
  ledcWrite(LEDC_CH_A, speed);
  ledcWrite(LEDC_CH_B, speed);
}

void stopMotors() {
  ledcWrite(LEDC_CH_A, 0);
  ledcWrite(LEDC_CH_B, 0);
}

Debugging: When the Robot Refuses to Balance

The most common point of failure in I2C-based robotics is bus contention or voltage mismatch. If your serial monitor outputs the exact string: MPU6050 I2C read failed. Wire.endTransmission() returned: 2, your ESP32 is receiving an I2C NACK (Not Acknowledged) on the address phase. The slave device is not answering.

Here are the first three things to check when this error occurs, ranked by probability:

1. Missing Common Ground: If the GND wire between the ESP32 and the MPU6050 is loose or missing, the I2C logic levels have no reference. The ESP32 sends a clock signal, but the MPU6050 doesn't recognize it. Fix: Verify continuity from ESP32 GND to MPU6050 GND with a multimeter.
2. Overvoltage on I2C Lines: The ESP32 is strictly a 3.3V logic device. If you powered the MPU6050 VCC pin with 5V, the breakout board's pull-up resistors will pull the SDA/SCL lines to 5V. This can backfeed the ESP32 GPIOs, causing the internal I2C peripheral to lock up or permanently damage the silicon. Fix: Ensure MPU6050 VCC is wired to the ESP32's 3V3 pin, not VIN or 5V.
3. Floating AD0 Pin: The MPU6050 has an address selection pin (AD0/SDO). If it is left floating, the I2C address can randomly toggle between 0x68 and 0x69. The code above hardcodes 0x68. Fix: Ensure the AD0 pin on the GY-521 breakout is physically tied to GND.

For deeper architectural insights on ESP32 I2C bus recovery, refer to the Espressif I2C API Documentation, which details how to implement software bus resets when the hardware peripheral hangs.

Scaling the Build: Simplify or Extend

Once you have the baseline balancing loop running, you will inevitably want to modify the platform. Here is how to adapt the build based on your parts bin and skill level.

How to Simplify (The Budget Build)

If N20 motors are too expensive or hard to source, you can swap them for the ubiquitous TT yellow gearmotors (often sold with the 2WD smart car chassis kits). Trade-offs: TT motors have massive backlash and high inertia. To make them balance, you must physically increase the height of the battery pack to raise the center of gravity (giving the PID loop more time to react), and you will need to aggressively increase the Kd (derivative) value in the code to dampen the oscillations caused by the sloppy plastic gears.

How to Extend (Wireless Telemetry)

Tuning PID values by plugging in a USB cable while the robot is balancing is a recipe for a snapped USB port. Extend this project by implementing ESP-NOW. Unlike standard WiFi (which introduces 20-50ms latency spikes due to beacon frames and router polling), ESP-NOW is a proprietary 2.4GHz MAC-layer protocol that guarantees sub-5ms latency. Build a second ESP32 with a joystick, map the joystick Y-axis to the targetAngle variable, and transmit it via ESP-NOW. You can effectively "drive" the balancing robot by shifting its virtual center of gravity. Consult the ESP-NOW Protocol Guide for the MAC address pairing sequence.