PID Control Explained With a Self-Balancing Robot

A self-balancing robot is one of the best ways to learn PID control because the feedback is immediate and physical. If your gains are wrong, the robot falls over. If they are right, it stands and balances. There is no ambiguity.

This guide explains PID control using a two-wheeled self-balancing robot built on the TM4C123, with an MPU-6050 IMU for tilt measurement and DC motors with encoders for wheel control.

What PID Is Trying to Do

The goal is simple: keep the robot upright. The robot measures its tilt angle, computes how far it is from vertical (the error), and drives the motors to correct that error.

Without any control, if the robot tilts forward, it falls forward. With perfect control, it drives forward just enough to put its base back under its center of mass — like you do naturally when you walk.

PID is the algorithm that computes “how hard to drive the motors” based on the current error. It has three terms:

P (Proportional) — react to the current error
I (Integral) — react to accumulated past error
D (Derivative) — react to how fast the error is changing

The Proportional Term

The simplest approach: motor output = Kp × error.

If the robot tilts forward by 5 degrees, drive the motors forward with force proportional to 5 degrees. More tilt, more motor output.

Problem: with only P control, the robot oscillates. It corrects, overshoots, corrects again, overshoots again. It never settles. Increasing Kp makes oscillations faster and more violent. Decreasing Kp makes the robot sluggish and unable to catch itself.

float Kp = 50.0f;

float computeP(float error) {
    return Kp * error;
}

The Derivative Term

The D term looks at how fast the error is changing. If the robot is tilting forward quickly, the D term adds extra correction before the error gets large. If the robot is already moving back toward vertical, the D term reduces the output to prevent overshoot.

Think of it as damping. It smooths out the oscillations that pure P control creates.

float Kp = 50.0f;
float Kd = 2.0f;

float lastError = 0.0f;
float dt = 0.01f; // 10ms loop time

float computePD(float error) {
    float derivative = (error - lastError) / dt;
    lastError = error;
    return (Kp * error) + (Kd * derivative);
}

For a self-balancing robot, PD control is often sufficient. Many working implementations use only P and D.

The Integral Term

The I term accumulates error over time. If the robot leans slightly forward for a long time — maybe because of a slight physical imbalance — the integral builds up and pushes the motors harder until the steady-state error is eliminated.

For a balancing robot, the I term helps when the robot has a weight imbalance that P and D cannot fully correct.

float Kp = 50.0f;
float Ki = 0.5f;
float Kd = 2.0f;

float integral = 0.0f;
float lastError = 0.0f;
float dt = 0.01f;

float computePID(float error) {
    integral += error * dt;
    // clamp integral to prevent windup
    if (integral >  100.0f) integral =  100.0f;
    if (integral < -100.0f) integral = -100.0f;

    float derivative = (error - lastError) / dt;
    lastError = error;

    return (Kp * error) + (Ki * integral) + (Kd * derivative);
}

The integral clamp (anti-windup) is important. If the robot falls over and stays fallen, the integral accumulates to a huge value. When you stand it back up, that huge integral causes the robot to immediately drive at full speed in one direction and fall again. Clamping prevents this.

Getting the Tilt Angle from MPU-6050

The MPU-6050 gives you accelerometer and gyroscope data. Neither alone is ideal for tilt measurement:

Accelerometer gives absolute angle but is noisy and affected by motion
Gyroscope gives rate of change (degrees per second) but drifts over time

The solution is a complementary filter that combines both:

float angle = 0.0f;
float alpha = 0.98f; // weight toward gyroscope

void updateAngle(float accelAngle, float gyroRate, float dt) {
    // integrate gyroscope
    float gyroAngle = angle + gyroRate * dt;
    // blend with accelerometer
    angle = alpha * gyroAngle + (1.0f - alpha) * accelAngle;
}

The complementary filter trusts the gyroscope for fast movements (98% weight) and slowly corrects drift using the accelerometer (2% weight). It is simple, computationally cheap, and works well for balancing applications.

Full Control Loop

#define LOOP_TIME_MS 10
#define SETPOINT     0.0f  // target: 0 degrees (upright)

float Kp = 50.0f;
float Ki = 0.5f;
float Kd = 2.0f;
float integral = 0.0f;
float lastError = 0.0f;
float dt = LOOP_TIME_MS / 1000.0f;

void controlLoop(void) {
    // read sensor
    float accelAngle = getAccelAngle();
    float gyroRate   = getGyroRate();

    // update angle estimate
    updateAngle(accelAngle, gyroRate, dt);

    // compute error
    float error = SETPOINT - angle;

    // PID
    float output = computePID(error);

    // drive motors
    setMotorOutput(output);
}

Run this loop at a fixed rate. Use SysTick or a FreeRTOS task with vTaskDelay(pdMS_TO_TICKS(10)) to maintain 100Hz.

Tuning the Gains

Tuning is the hardest part. Start with all gains at zero and increase one at a time.

Step 1 — Tune Kp alone Increase Kp until the robot tries to balance but oscillates. Note this value. Set Kp to about 60% of the oscillation value.

Step 2 — Add Kd Increase Kd until the oscillations are damped and the robot balances without too much jitter. Too much Kd causes high-frequency noise amplification — the motors will sound like they are buzzing.

Step 3 — Add Ki Only if there is a steady-state lean. Start very small (0.1 or less) and increase slowly. Too much Ki causes slow oscillations that are hard to damp.

Typical starting values for a small (500g–1kg) robot:

Kp: 30–80
Ki: 0.1–1.0
Kd: 1–5

These vary a lot depending on motor power, wheel size, and robot mass distribution. There is no shortcut — you have to tune on the actual hardware.

Physical Balance Point

The control setpoint is not exactly zero degrees. The robot’s center of mass is rarely perfectly centered. Find the natural balance angle by setting all gains high enough for the robot to balance, then observing what angle it settles at. Use that as your setpoint instead of zero.

Even a 0.5 degree offset in the setpoint significantly affects performance.

Common Problems

Robot immediately falls in one direction Setpoint is wrong, or the angle sensor orientation is reversed. Check that positive angle corresponds to forward tilt and positive motor output drives the robot forward.

Robot oscillates even with low Kp The control loop is too slow. Run at 100Hz minimum. Slow loops cannot keep up with the physics.

Motors jitter at high frequency Kd is too high or the gyroscope is noisy. Add a low-pass filter to the derivative term or reduce Kd.

Robot balances but slowly drifts Add a small Ki term or adjust the setpoint.

Summary

PID control for a self-balancing robot comes down to: measure angle with a complementary filter, compute error, apply PID, drive motors. The math is simple. The challenge is tuning and making the control loop fast and consistent. Once it works, it is deeply satisfying to watch a robot balance on two wheels using code you wrote from scratch.