How to use an MPU6050 to make a self-balancing robot (the hard project)

The classic hard project. Beautiful in theory, brutal in tuning. Took me 3 evenings to get it balancing for more than 5 seconds — and another evening of tuning before it could be pushed and recover.

Parts

Arduino Nano (or any 16MHz AVR)
MPU6050 6-axis accel + gyro module (I²C)
2 × NEMA 17-style stepper motors (steppers are better than DC motors here — they give clean torque control at low speeds)
2 × A4988 stepper drivers
2 × wheels with rubber tyres (smooth wheels slip)
11.1V 3S LiPo + buck converter to 5V
Tall, narrow chassis — about 250mm tall, with the battery at the top (lengthens the natural period, makes it easier to balance)

The control loop

1. Read accelerometer X and gyroscope Y from MPU6050.
2. Apply a complementary filter: angle = 0.98 × (angle + gyroY × dt) + 0.02 × accelAngle.
(Pure accel is noisy but absolute; pure gyro drifts but is smooth. The filter combines them.)
3. Run a PID loop with the angle as input, target = 0° (upright), output = motor speed.
4. Output → step rate for both motors. Forward step rate when leaning forward, backward when leaning back.

Tuning PID (here be dragons)

Start with Ki = 0, Kd = 0. Slowly raise Kp until the robot oscillates.
Then add Kd to damp the oscillation.
Finally add Ki — small! — to compensate for any standing tilt.
My final values: Kp = 32, Ki = 280, Kd = 1.2. These will be wildly different for your chassis.

Code structure (loop runs at 200Hz fixed)

unsigned long lastUs = 0;
void loop() {
  while (micros() - lastUs < 5000) {} // 5ms cycle
  lastUs = micros();
  readMPU();
  computeAngle();
  pidOutput = Kp*err + Ki*integ + Kd*deriv;
  setStepperSpeed(pidOutput);
}

Tips that took me too long to learn

Use stepper motors, not DC. DC motors have dead-band at low PWM and no torque at zero speed.
Put the heaviest thing (battery) at the top, not the bottom.
The MPU6050 needs to be calibrated — average 1000 readings while stationary and subtract those offsets.
Use a high-quality breadboard or solder it down. A loose wire = robot face-plant.

When it works: the feeling of pushing a robot and watching it lurch then recover is genuinely magical. Just don't let the cat anywhere near it.

advanced arduino mpu6050 i2c robot stepper pid