Make a Self Balancing Robot Using Arduino (Complete Technical Guide)

A self-balancing robot is a classic control systems problem: the inverted pendulum.

If the center of mass is above the wheel axis, gravity creates torque that increases the fall angle. That means the system is inherently unstable. Stability must be actively created using feedback control.

Your robot consists of:

Arduino Uno
MPU6050 (Gyroscope + Accelerometer with DMP)
L298N motor driver
Two DC motors
PID controller

The brain of the system is the PID equation.

System Overview

The control loop works like this:

MPU6050 measures orientation.
We extract pitch angle.
Calculate error = setpoint − current angle.
PID computes corrective output.
Motors move forward or backward.
Loop repeats every few milliseconds.

This loop runs continuously. The robot never “rests.” It constantly corrects.

Pin Configuration and Connections

1. MPU6050 to Arduino (I2C Connection)

VCC → 5V (or 3.3V depending on module)

GND → GND

SDA → A4 (Arduino Uno)

SCL → A5 (Arduino Uno)

INT → Digital Pin 2

Important: The interrupt pin must connect to Pin 2 because attachInterrupt(0, ...) uses Digital Pin 2 on Arduino Uno.

2. L298N Motor Driver to Arduino

ENA → Pin 11

IN1 → Pin 7

IN2 → Pin 6

IN3 → Pin 5

IN4 → Pin 4

ENB → Pin 10

Note: ENA and ENB must connect to PWM pins. Pins 10 and 11 support PWM on Arduino Uno.

3. Motors to L298N

Left Motor → OUT1 and OUT2

Right Motor → OUT3 and OUT4

Warning: If the robot moves in the wrong direction when tilted, swap the motor wires of that side.

4. Power Connections (Critical)

External Battery (7V–12V) → L298N 12V Input

L298N GND → Arduino GND

MPU6050 GND → Arduino GND

Important: All grounds must be connected together. Without a common ground, the system will behave unpredictably.

Understanding the Angle Reference in Your Code

You are using:

input = ypr[1] * 180/M_PI + 180;

Why +180?

Because MPU DMP pitch output ranges roughly from -180 to +180 degrees.

By adding 180, you shift it into:

0 to 360 degrees.

Your setpoint is:

double originalSetpoint = 180.0;

Which means upright position = 180 degrees.

So error becomes:

[
e(t) = 180 - measuredAngle
]

This is perfectly valid — just a shifted reference frame.

The PID Equation

The complete equation:

[
u(t) = K_p e(t) + K_i \int e(t) dt + K_d \frac{de(t)}{dt}
]

Where:

( e(t) ) = setpoint − input
( u(t) ) = motor PWM output

Each term has a physical meaning.

Deep Explanation of P, I, and D

Proportional (P)

[
P = K_p \cdot e(t)
]

This reacts to how far the robot is tilted right now.

If robot tilts forward 5 degrees:

Larger Kp → stronger correction.

Too low:

Robot falls slowly.

Too high:

Robot oscillates violently.

P controls stiffness.

Integral (I)

[
I = K_i \int e(t) dt
]

This accumulates past errors.

If robot slightly drifts forward continuously:

I builds up correction.

Too high:

Slow oscillations.
Sudden instability after a few seconds.
Motors surge unexpectedly.

Integral windup is a real problem in balancing robots.

In your code, Ki = 200 is aggressive. It works only if tuning is precise.

Derivative (D)

[
D = K_d \frac{de(t)}{dt}
]

This predicts future motion.

If robot begins falling fast:

D applies braking force.

Too low:

Oscillation continues.

Too high:

Jitter.
Sensitive to sensor noise.

Derivative is your shock absorber.

How to Tune PID Correctly (No Guessing)

Step 1
Set Ki = 0
Set Kd = 0

Increase Kp slowly until robot starts oscillating but does not immediately fall.

Step 2
Increase Kd until oscillation reduces and system becomes stable.

Step 3
Add small Ki only if robot drifts over time.

Correct tuning order:
P → D → I

Never start with I.

Understanding the Real-Time Loop in Your Code

This part is critical:

while (!mpuInterrupt && fifoCount < packetSize)
{
    pid.Compute();
    motorController.move(output, MIN_ABS_SPEED);
}

This means:

Even if no new sensor data arrives, PID still runs.

That keeps motor response continuous.

When new data arrives:

FIFO is read.
Quaternion calculated.
Gravity vector computed.
Yaw/Pitch/Roll extracted.
Input updated.

This is efficient and correct.

Why FIFO Overflow Happens

If this message appears:

FIFO overflow!

It means:

Your loop is too slow.
I2C communication unstable.
Serial printing too heavy.
Interrupt not processed fast enough.

Balancing robots require fast loops. Slow debug printing can destabilize system timing.

Live PID Tuning in Your Code

Your serial block allows runtime tuning:

q / a → increase / decrease Kp
w / s → increase / decrease Kd
e / d → increase / decrease Ki

This is excellent practice.

Real control engineers tune systems live.

Why Your Tall Frame Makes It Harder

Physics matters.

A taller robot:

Has higher center of mass.
Produces larger torque for same angle.
Requires lower Kp than short robots.

Moment of inertia increases with height. That slows rotational response but increases fall torque.

Control gains must match mechanical design.

Common Failure Causes

If robot falls instantly:

Motors wired reversed.
Kp too low.
Wrong setpoint.

If robot shakes violently:

Kp too high.
Kd too low.

If robot balances then slowly drifts:

Ki too low.

If robot balances then suddenly jumps:

Ki too high.

If robot vibrates constantly:

Kd too high.
Noisy sensor.

See Source Code on GitHub and Contribute

Make sure to Download Libraries and put in your Arduino Directory

Click here to Download Libraries

You can just clone this github

git clone https://github.com/akash2v/Self-Balanced-Robot

Complete Source Code

#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

#define MIN_ABS_SPEED 30

MPU6050 mpu;

bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

Quaternion q;
VectorFloat gravity;
float ypr[3];

double originalSetpoint = 180.0;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

double Kp = 60;
double Kd = 2.2;
double Ki = 200;

PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.9;
double motorSpeedFactorRight = 0.9;

int ENA = 11;
int IN1 = 7;
int IN2 = 6;
int IN3 = 5;
int IN4 = 4;
int ENB = 10;

LMotorController motorController(
  ENA, IN1, IN2, ENB, IN3, IN4,
  motorSpeedFactorLeft,
  motorSpeedFactorRight
);

volatile bool mpuInterrupt = false;

void dmpDataReady()
{
  mpuInterrupt = true;
}

void setup()
{
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24;
#endif

  Serial.begin(115200);
  mpu.initialize();

  devStatus = mpu.dmpInitialize();

  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  mpu.setZAccelOffset(0);

  if (devStatus == 0)
  {
    mpu.setDMPEnabled(true);
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();

    pid.SetMode(AUTOMATIC);
    pid.SetSampleTime(10);
    pid.SetOutputLimits(-255, 255);

    Serial.println("DMP initialized successfully");
  }
  else
  {
    Serial.print("DMP Initialization failed (code ");
    Serial.print(devStatus);
    Serial.println(")");
  }
}

void loop()
{
  if (!dmpReady) return;

  while (!mpuInterrupt && fifoCount < packetSize)
  {
    pid.Compute();
    motorController.move(output, MIN_ABS_SPEED);
  }

  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();

  if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    mpu.resetFIFO();
    Serial.println("FIFO overflow!");
  }
  else if (mpuIntStatus & 0x02)
  {
    while (fifoCount < packetSize)
      fifoCount = mpu.getFIFOCount();

    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    input = ypr[1] * 180/M_PI + 180;
  }

  if (Serial.available())
  {
    char c = Serial.read();

    if (c == 'q') Kp += 1;
    if (c == 'a') Kp -= 1;
    if (c == 'w') Kd += 0.05;
    if (c == 's') Kd -= 0.05;
    if (c == 'e') Ki += 1;
    if (c == 'd') Ki -= 1;

    Kp = constrain(Kp, 0, 100);
    Ki = constrain(Ki, 0, 100);
    Kd = constrain(Kd, 0, 10);

    pid.SetTunings(Kp, Ki, Kd);

    Serial.print("Kp=");
    Serial.print(Kp);
    Serial.print(" Ki=");
    Serial.print(Ki);
    Serial.print(" Kd=");
    Serial.println(Kd);
  }
}

Final Insight

This robot is not a coding project.

It is a control theory project.

Once you understand that PID is managing torque, angular velocity, and gravitational acceleration in real time, you stop tuning randomly and start engineering deliberately.

And that is the difference between a project that works sometimes and a system that works consistently.

Self-Balancing Robot Using Arduino and MPU6050 with PID Control – Complete Explanation and Source Code - Akash

Comments