Build and Debug an Arduino Two Wheel Self Balancing Robot

Building an Arduino two wheel self balancing robot requires a fast control loop, a reliable IMU, and a motor driver with minimal voltage drop. This guide targets the Arduino Nano V3 (ATmega328P) paired with a TB6612FNG dual motor driver and an MPU-6050 (GY-521 breakout). We will cover the exact wiring, a compilable PID control sketch with I2C error handling, and the specific debugging steps to take when the robot inevitably falls over on the first test.

Project Overview & Difficulty Rating

The core challenge of a self-balancing robot is latency. The microcontroller must read the IMU, calculate the pitch angle, run a PID (Proportional-Integral-Derivative) algorithm, and update the motor PWM signals in under 10 milliseconds. While many tutorials suggest the L298N motor driver, its 2V internal voltage drop and slow switching speed make it a poor choice for balancing. We use the TB6612FNG, which features MOSFET-based H-bridges with a mere 0.5V drop and supports high-frequency PWM, giving the motors the immediate torque response required to catch a fall.

Hardware Spec Sheet & Pin Mapping

Before wiring, verify you have the exact variants listed below. Using a clone Nano with an ATmega168P will result in memory errors when compiling the sensor libraries.

Component	Exact Variant / Specification	Why This Variant?
Microcontroller	Arduino Nano V3.0 (ATmega328P, CH340G)	Compact footprint, 5V logic, 32KB flash for PID math.
IMU Sensor	GY-521 Breakout (MPU-6050)	Integrated 3-axis accel/gyro with hardware DMP.
Motor Driver	TB6612FNG Dual Motor Driver	1.2A continuous, 0.5V dropout, high PWM frequency.
Motors	N20 6V DC Gear Motor (200 RPM, 1:150 ratio)	Metal gears, low backlash, fast spool-up time.
Power Supply	2S LiPo (7.4V, 1000mAh) + LM2596 Buck	High C-rating for current spikes; buck protects Nano.
Wheels	65mm diameter silicone/rubber tires	High friction coefficient to prevent slip on hard floors.

Pin Mapping Table

TB6612FNG Pin	Arduino Nano Pin	Notes
VCC / Vmotor	5V (Buck) / 7.4V (LiPo)	Logic vs Motor power separation.
GND	GND	Ensure common ground with Nano and LiPo.
STBY	D4	Pull HIGH to enable the driver.
PWMA / PWMB	D5 / D6	Must be hardware PWM capable pins.
AIN1 / AIN2	D7 / D8	Motor A direction control.
BIN1 / BIN2	D9 / D10	Motor B direction control.
AO1/AO2, BO1/BO2	Motors	Swap wires if motor spins backward.

MPU-6050 (GY-521) connects to Nano SDA (A4) and SCL (A5). VCC must connect to the Nano's 5V pin, not 3.3V.

Assembly & Power Distribution Steps

1. Configure the Buck Converter: Connect the 2S LiPo to the LM2596 input. Use a multimeter on the output terminals and adjust the potentiometer until the output reads exactly 5.0V. Disconnect the battery.
2. Mount the IMU: Secure the GY-521 breakout board to the exact center of your chassis, parallel to the wheel axis. The X-axis arrow on the silkscreen must point directly forward. Use double-sided foam tape to dampen high-frequency motor vibrations.
3. Wire the Motor Driver: Solder the TB6612FNG header pins. Connect the LiPo main positive to the driver's VMOT pin and the buck converter's input. Connect all grounds (LiPo, Buck, Nano, Driver) to a single common ground bus.
4. Establish the Center of Mass: Mount the battery as high as possible on the chassis. A higher center of mass increases the pendulum period, giving the PID loop more time to react to a fall. This is counter-intuitive but physically accurate for inverted pendulums.

The PID Control Code (Arduino Nano V3)

This sketch uses the Adafruit MPU6050 library and a complementary filter to fuse the accelerometer and gyroscope data. It includes a raw PID implementation to avoid external library dependencies and features I2C error handling to prevent silent failures.

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

// --- Pin Definitions ---
#define STBY_PIN 4
#define PWMA_PIN 5
#define PWMB_PIN 6
#define AIN1_PIN 7
#define AIN2_PIN 8
#define BIN1_PIN 9
#define BIN2_PIN 10

// --- PID Tuning Parameters ---
float Kp = 35.0;   // Proportional: Reacts to current error
float Ki = 0.5;    // Integral: Eliminates steady-state offset
float Kd = 1.2;    // Derivative: Dampens oscillation

float targetAngle = -1.5; // Calibrate this for your specific chassis
float integral = 0;
float prevError = 0;
float pitch = 0;

Adafruit_MPU6050 mpu;
unsigned long previousTime = 0;

void setup() {
  Serial.begin(115200);
  pinMode(STBY_PIN, OUTPUT);
  digitalWrite(STBY_PIN, HIGH); // Enable TB6612FNG

  pinMode(PWMA_PIN, OUTPUT); pinMode(PWMB_PIN, OUTPUT);
  pinMode(AIN1_PIN, OUTPUT); pinMode(AIN2_PIN, OUTPUT);
  pinMode(BIN1_PIN, OUTPUT); pinMode(BIN2_PIN, OUTPUT);

  // Initialize I2C and MPU6050 with error handling
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C fast mode for lower latency
  
  if (!mpu.begin()) {
    Serial.println(F("Failed to find MPU6050 chip"));
    while (1) {
      // Halt and blink LED to indicate hardware failure
      digitalWrite(LED_BUILTIN, HIGH); delay(250);
      digitalWrite(LED_BUILTIN, LOW); delay(250);
    }
  }
  
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
  
  previousTime = micros();
}

void loop() {
  unsigned long currentTime = micros();
  float dt = (currentTime - previousTime) / 1000000.0;
  previousTime = currentTime;

  // Prevent division by zero or massive dt spikes on first loop
  if (dt > 0.1) return; 

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Complementary Filter (Gyro for fast changes, Accel for drift correction)
  float accelAngle = atan2(a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180 / PI;
  pitch = 0.98 * (pitch + g.gyro.y * dt) + 0.02 * accelAngle;

  // PID Calculations
  float error = targetAngle - pitch;
  integral += error * dt;
  integral = constrain(integral, -150, 150); // Anti-windup clamping
  
  float derivative = (error - prevError) / dt;
  prevError = error;

  float output = (Kp * error) + (Ki * integral) + (Kd * derivative);
  
  // Motor Mixing and Deadzone Compensation
  int motorSpeed = constrain(output, -255, 255);
  driveMotors(motorSpeed);
  
  delay(5); // ~200Hz control loop
}

void driveMotors(int speed) {
  // TB6612FNG deadzone is roughly PWM < 30
  int absSpeed = abs(speed);
  if (absSpeed < 30 && abs(speed) > 0) absSpeed = 30; 

  if (speed > 0) { // Forward
    digitalWrite(AIN1_PIN, HIGH); digitalWrite(AIN2_PIN, LOW);
    digitalWrite(BIN1_PIN, HIGH); digitalWrite(BIN2_PIN, LOW);
  } else if (speed < 0) { // Backward
    digitalWrite(AIN1_PIN, LOW); digitalWrite(AIN2_PIN, HIGH);
    digitalWrite(BIN1_PIN, LOW); digitalWrite(BIN2_PIN, HIGH);
  } else { // Brake
    digitalWrite(AIN1_PIN, LOW); digitalWrite(AIN2_PIN, LOW);
    digitalWrite(BIN1_PIN, LOW); digitalWrite(BIN2_PIN, LOW);
  }
  
  analogWrite(PWMA_PIN, absSpeed);
  analogWrite(PWMB_PIN, absSpeed);
}

Debugging: When the Robot Falls Over Immediately

If your robot accelerates violently into the floor or instantly tips over, do not immediately change the PID values. Hardware and sensor orientation issues are the culprit 90% of the time.

The First Three Things to Check

1. IMU Orientation: Verify the GY-521 X-axis points forward. If it points sideways, the robot will read roll as pitch and spin in circles.
2. Target Angle Offset: No chassis is perfectly symmetrical. Hold the robot upright, open the Serial Plotter, and note the resting pitch angle. Update the targetAngle variable in the code to match this exact number (e.g., -2.1).
3. Motor Direction: If one motor is wired backward, the PID loop will fight itself. Lift the wheels off the ground, tilt the robot forward, and ensure both wheels spin forward.

Exact Error String: Failed to find MPU6050 chip

If the Serial Monitor prints this exact string and the Nano's onboard LED begins blinking, the I2C bus has failed to initialize. Ranked causes:

1. Under-voltage on VCC: The GY-521 breakout has an onboard 3.3V LDO regulator. If you wire the VCC pin to the Nano's 3.3V pin, the LDO lacks the headroom to operate, and the MPU-6050 will not boot. Fix: Wire VCC to the Nano's 5V pin.
2. Missing I2C Pull-ups: While the GY-521 has 4.7kΩ pull-ups, long jumper wires introduce capacitance that degrades the 400kHz I2C signal. Fix: Keep SDA/SCL wires under 10cm, or add external 2.2kΩ pull-up resistors to 5V.
3. Swapped SDA/SCL: On the Arduino Nano V3, SDA is strictly A4 and SCL is strictly A5. Swapping them will cause a silent timeout in the Wire library.

Extending and Simplifying the Build

Depending on your budget and skill level, you can modify this architecture.

How to Simplify (The L298N Route):
If you cannot source a TB6612FNG, you can use an L298N driver. However, you must compensate for its 2V voltage drop and massive deadzone. Increase your motor supply to 9V (3S LiPo or 6x AA) and add a deadzone mapping function in the code that jumps the PWM from 0 directly to 80. Expect a much more jittery balance.

How to Extend (WiFi PID Tuning):
Swap the Arduino Nano for an ESP32-DevKitC V4. The ESP32's dual-core 240MHz processor allows you to run the PID loop on Core 1 while hosting a WebSocket server on Core 0. This lets you build a web dashboard on your phone to adjust Kp, Ki, and Kd via sliders in real-time without recompiling the firmware.

Frequently Asked Questions