LuisLlamas Logo
arduino-robot-equilibrista-pid

How to Build a Self-Balancing Robot with Arduino and PID

  • 9 min
  • Beginner

A self-balancing robot is an inverted pendulum that moves its wheels to keep the body upright.

The concept is simple: keep a stick vertical on two wheels. It is the classic Inverted Pendulum problem. However, physics is cruel. It is an intrinsically unstable system; if the Arduino falls asleep for a fraction of a second, the robot falls.

To keep it upright, the code must measure the tilt, calculate the correction, and act on the motors with a stable period. In this example, we will use a cycle of approximately 100 Hz.

This project introduces two pillars of modern engineering:

  1. Sensor Fusion: Combining an accelerometer and a gyroscope to obtain a reliable angle.
  2. PID Control: The mathematical algorithm that maintains stability.

How we estimate the tilt

We need an IMU (Inertial Measurement Unit) sensor, specifically the famous MPU6050. This chip provides two types of data:

  • Accelerometer: Measures gravity. It tells us where “down” is. Problem: It is very noisy and sensitive to motor vibrations.
  • Gyroscope: Measures angular velocity (how fast we are rotating). Problem: It has “drift”. Over time, the error accumulates and it thinks it is rotating when it is still.

Complementary filter

To obtain a clean angle, we mix both. We trust the gyroscope for fast changes and the accelerometer for long-term stability.

PID Controller

Once we know the angle, how much should we accelerate?

  • P (Proportional): “I’m falling a little, I accelerate a little. I’m falling a lot, I accelerate a lot.”
  • I (Integral): “I’ve been slightly tilted for a while and I’m not straightening up, I’ll add more power to compensate.”
  • D (Derivative): “Watch out! I’m falling very fast, I’ll apply force in the opposite direction to slow the fall.”

Components

  • [1x|blue] Arduino Uno | Or Nano
  • [1x|orange] MPU6050 Module | Accelerometer + Gyroscope {I2C}
  • [2x|green] DC Motors with Gearbox | Pololu or N20 recommended {TT Motors work}
  • [1x|red] Motor Driver | L298N for testing; TB6612FNG reduces losses
  • [1x|red] Protected Battery | Voltage compatible with driver and motors
  • [Various|gray] Vertical Chassis | Rigid, sensor firmly fixed

TT motors work, but they have considerable play. For a more repeatable response, use motors with a gearbox that has less backlash and, if possible, encoders.

Wiring Diagram

The assembly must be rigid. If the MPU6050 sensor vibrates loosely relative to the chassis, the robot will never balance.

MPU6050 Module

The MPU6050 communicates via I2C. Many modules accept 5V because they have a built-in regulator, but the bare chip operates at 3.3V.

[ { “from”: “MPU6050 VCC”, “to”: “Arduino 5V”, “color”: “red”, “note”: “Only if the module supports 5V” }, { “from”: “MPU6050 GND”, “to”: “Arduino GND”, “color”: “black” }, { “from”: “MPU6050 SDA”, “to”: “Pin A4”, “color”: “blue”, “note”: “I2C Bus” }, { “from”: “MPU6050 SCL”, “to”: “Pin A5”, “color”: “purple”, “note”: “I2C Bus” } ]

L298N Driver

The driver receives two direction signals and one PWM signal per motor.

[ { “from”: “L298N ENA (Speed A)”, “to”: “Pin 9 (PWM)”, “color”: “green” }, { “from”: “L298N IN1 / IN2 (Dir A)”, “to”: “Pin 4 / Pin 5”, “color”: “orange” }, { “from”: “L298N ENB (Speed B)”, “to”: “Pin 10 (PWM)”, “color”: “green” }, { “from”: “L298N IN3 / IN4 (Dir B)”, “to”: “Pin 6 / Pin 7”, “color”: “orange” } ]

Motor Power Supply

The motors are powered from the battery through the driver. The GND reference must be common with the Arduino.

[ { “from”: “L298N Power 12V/Bat”, “to”: “Driver”, “color”: “red”, “note”: “GND shared with Arduino” }, { “from”: “Common GND”, “to”: “Battery GND + Arduino”, “color”: “black” } ]

The Code

This code is complex. We will read the MPU6050 “bare metal” (accessing registers directly) for maximum control and speed, and then apply the PID.

We will use the Wire library, included with the Arduino core.

Variables and Constants

#include <Wire.h>

// --- PID Configuration (Adjusting these values is THE KEY) ---
double Kp = 20.0; 
double Ki = 0.8;  
double Kd = 0.5;  

// --- L298N Pins ---
const int ENA = 9;
const int IN1 = 4;
const int IN2 = 5;
const int IN3 = 6;
const int IN4 = 7;
const int ENB = 10;

// IMU Variables
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;

float gyroAngle = 0, accelAngle = 0, currentAngle = 0;
float error, previousError = 0, sumError = 0;

// Time variables for PID
unsigned long currentTime, previousTime;
float dt;

// Equilibrium Point (SetPoint)
// Adjust if the robot tends to lean to one side mechanically
float setPoint = 0;
Copied!

setup and MPU Initialization

We need to wake up the MPU6050 in the setup().

void setup() {
  Serial.begin(9600);
  
  // Configure Motor Pins
  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  
  // Start I2C and MPU6050
  Wire.begin();
  Wire.beginTransmission(0x68);
  Wire.write(0x6B); // Power Management Register
  Wire.write(0);    // Wake up the MPU
  Wire.endTransmission(true);
  
  previousTime = millis();
}
Copied!

Control Loop

void loop() {
  currentTime = millis();
  dt = (currentTime - previousTime) / 1000.0; // Time in seconds
  
  // If the loop runs very fast, dt could be 0. Force a minimum.
  if(dt < 0.01) return; 
  previousTime = currentTime;

  // 1. READ DATA (Helper function defined below)
  readMPU();

  // 2. COMPLEMENTARY FILTER
  // Angle from accelerometer (Basic trigonometry)
  accelAngle = atan2(accelY, accelZ) * 180 / PI;
  
  // Angle from gyroscope (Integration: speed * time)
  // 0.98 and 0.02 are the filter weights.
  currentAngle = 0.98 * (currentAngle + rotX * dt) + 0.02 * accelAngle;

  // 3. PID CALCULATION
  error = setPoint - currentAngle;
  sumError += error * dt;            // Integral term
  // Limit the integral to prevent "windup" (infinite accumulation)
  sumError = constrain(sumError, -200, 200); 
  
  float rateError = (error - previousError) / dt; // Derivative term
  
  // PID Output
  float output = (Kp * error) + (Ki * sumError) + (Kd * rateError);
  
  previousError = error;

  // 4. MOVE MOTORS
  if (abs(currentAngle) > 45) {
    sumError = 0;
    previousError = 0;
    stopMotors();
    return;
  }

  moveMotors(output);
}
Copied!

Helper Functions

Here we read the raw registers and control the H-bridge.

void readMPU() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B); // Starting register for accelerometer data
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true); 
  
  // The values come in two bytes, we need to combine them
  accelX = (Wire.read() << 8 | Wire.read());
  accelY = (Wire.read() << 8 | Wire.read());
  accelZ = (Wire.read() << 8 | Wire.read());

  // Now read gyroscope (Register 0x43)
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true);
  
  gyroX = (Wire.read() << 8 | Wire.read());
  gyroY = (Wire.read() << 8 | Wire.read());
  gyroZ = (Wire.read() << 8 | Wire.read());
  
  // Convert to degrees/second (Divisor 131.0 is standard for default range)
  rotX = gyroX / 131.0;
}

void moveMotors(float speed) {
  // speed will be a value ranging from -infinity to +infinity
  // Map to PWM (0-255)
  
  int pwm = constrain(abs(speed), 0, 255);
  
  // Dead zone: Cheap DC motors don't start with PWM < 30-40
  // If there is a signal but it is low, force a minimum to overcome friction
  if (pwm < 40 && pwm > 5) pwm = 40; 

  if (speed > 0) {
    // FORWARD (Adjust IN1/IN2 according to your wiring)
    digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
    digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
  } else {
    // BACKWARD
    digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
    digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
  }
  
  analogWrite(ENA, pwm);
  analogWrite(ENB, pwm);
}

void stopMotors() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}
Copied!

PID Tuning

If you copy the code and turn it on, the robot will fall. I guarantee it. Every robot has different weight, height, and motors. You need to adjust Kp, Ki, and Kd.

Do the first tests with the robot held in a support and the wheels free, away from fingers and cables. First, check that the sign of the correction is correct. Then you can adjust the terms with small increments:

  1. Set Ki = 0 and Kd = 0.
  2. Increase Kp gradually.
  • If it is low, the robot falls slowly (it doesn’t have the strength to rise).
  • Increase until the robot starts to oscillate frantically on its axis.
  1. Once it oscillates, start increasing Kd.
  • Kd acts as a damper. You should see the oscillations calm down and the robot become more stable.
  1. If the robot stays upright but slowly drifts to one side until it falls, increase Ki a little. Ki corrects that constant error.

Troubleshooting

  • The robot shoots off in one direction: The motor logic is inverted with respect to the tilt. If it tilts forward, the wheels must accelerate forward (to get under the center of gravity), not backward. Reverse the IN pins.
  • Dead Zone: If the robot makes a high-pitched noise (the motor PWM) but doesn’t move until it is already very tilted, you need to increase the minimum PWM value in the moveMotors function.

On this page