Built-in project · Arduino Uno R3

Balance bot on an Arduino Uno

A two-wheel robot that balances itself: its MPU6050 through a complementary filter, a PID with three knobs for live Kp, Ki and Kd, and the angle and gains on an OLED. The whole build, an Arduino Uno and 5 more parts, runs here in your browser on the firmware below; open it in the editor to change the wiring or the code and run it again.

Teaching? Assign it to your class in one click.

Advanced About 40 minutes to build and run. Runs in your browser. Free, and no account needed.

Build it as the Balance Bot Kit: the parts in numbered bags and a booklet that checks every step. Retails for about $81, free on Mokxi.

Step by step, with the wiring and the common mistakes: Arduino Self-Balancing Robot: MPU6050 and PID Code

Balance bot · Arduino Uno R3live0.000 s 0.00x
Click to open it in the editor
The circuit itself, running here on the simulator. Press what can be pressed; click anything else to open it in the editor.

What you'll learn

  • Complementary filter: blending a gyroscope and an accelerometer
  • PID control: proportional, integral and derivative terms
  • Driving two motors with a TB6612 driver and PWM
  • Reading potentiometers to tune a controller live
  • Keeping a control loop on a fixed time step
  • Difficulty: Advanced
  • Time: About 40 minutes
  • Board: Arduino Uno R3
  • Parts: 9

How it works

The robot's MPU6050 reports which way is down and how fast the body is turning. Every 10 ms the sketch blends the two into one tilt angle. The gyro moves the angle smoothly and the accelerometer pulls it back to the truth.

A PID turns that tilt into a motor command. When the robot leans forward, both wheels drive forward to get back under it. The size of the command sets the PWM, and the sign picks the direction.

Three potentiometers on A0, A1 and A2 set Kp, Ki and Kd while it runs. The OLED shows the angle and the gains. Past 45 degrees the robot counts as fallen and the motors brake.

Parts list

9 parts, plus the jumper wires. They retail for about $81; on Mokxi they're free.

Wiring table

Every connection, pin by pin, read from the circuit itself. Each row joins the pin on the left to the pin on the right, by a jumper wire or a breadboard strip.

Part and pinConnects to
Self-balancing robot pin GNDArduino Uno R3 pin GND
Self-balancing robot pin SCLArduino Uno R3 pin 12
Self-balancing robot pin AIN1Arduino Uno R3 pin 10
Self-balancing robot pin AIN2Arduino Uno R3 pin 9
Self-balancing robot pin PWMAArduino Uno R3 pin 6
Self-balancing robot pin BIN1Arduino Uno R3 pin 5
Self-balancing robot pin BIN2Arduino Uno R3 pin 4
Self-balancing robot pin PWMBArduino Uno R3 pin 3
Self-balancing robot pin SDAArduino Uno R3 pin 2
Potentiometer (1) pin 3Arduino Uno R3 pin 5V
Potentiometer (2) pin 3Arduino Uno R3 pin 5V
Potentiometer (3) pin 3Arduino Uno R3 pin 5V
Potentiometer (1) pin 1Arduino Uno R3 pin GND
Potentiometer (2) pin 1Arduino Uno R3 pin GND
Potentiometer (3) pin 1Arduino Uno R3 pin GND
Potentiometer (1) pin 2Arduino Uno R3 pin A0
Potentiometer (2) pin 2Arduino Uno R3 pin A1
Potentiometer (3) pin 2Arduino Uno R3 pin A2
OLED display, 128x64 pin SDAArduino Uno R3 pin A4
OLED display, 128x64 pin SCLArduino Uno R3 pin A5
Self-balancing robot pin VCC5 V
Self-balancing robot pin STBY5 V
OLED display, 128x64 pin GNDGround
OLED display, 128x64 pin VCC5 V

The code

The firmware exactly as the editor opens it. Change a line there and press Run: it compiles in the browser.

sketch.ino · Arduino Uno R3
// Balance bot: a two-wheel robot that stands up on its own.
//
//   robot SCL -> 12   robot SDA -> 2      the robot's own MPU6050, at 0x68
//   AIN1 -> 10  AIN2 -> 9   PWMA -> 6     left motor, on the robot's TB6612
//   BIN1 -> 5   BIN2 -> 4   PWMB -> 3     right motor
//   STBY -> 5 V                           the driver runs whenever it has power
//   A0, A1, A2   three knobs: Kp, Ki and Kd, live
//   A5 -> SCL   A4 -> SDA                 the OLED, angle and gains
//
// Every 10 ms the loop does three things. That is about as fast as this one
// can go: reading the IMU over a software I2C bus takes about 5 ms and the
// float math about 3 more, on the board as here.
//
// # 1. Work out the tilt
//
// The accelerometer knows which way is down, but it also feels every shove the
// wheels give the body, so on its own it is noisy and wrong while the robot
// moves. The gyroscope knows how fast the body is turning, smoothly, but adding
// up a rate drifts. The complementary filter takes the best of each:
//
//   angle = 0.98 * (angle + rate * dt) + 0.02 * accelAngle
//
// The gyro moves the angle from moment to moment; the accelerometer pulls it
// slowly back to the truth.
//
// # 2. PID
//
// The error is how far the body leans from upright. P drives the wheels under
// the lean, D (on the gyro rate) stops it overshooting, and I makes up the
// steady push a motor needs just to hold a speed, which is what stops this
// robot creeping off at full speed and falling. Turn the knobs while it runs.
//
// # 3. Drive
//
// Leaning forward, the wheels drive forward to get back under the body. The
// sign of the PID output picks AIN1/AIN2 and the size goes to analogWrite.
// Past 45 degrees it has fallen: the motors brake and the integral resets.
// Until a hand lets go of it (at power-up, and after standing it up) the
// motors stay braked, so the integral does not wind up while it is held.
// Press the stand-up button on the robot to try again.

#include "mokxi_i2c.h"
#include "mokxi_mpu6050.h"
#include "mokxi_ssd1306.h"

const uint8_t IMU_SCL = 12;
const uint8_t IMU_SDA = 2;
const uint8_t AIN1 = 10;
const uint8_t AIN2 = 9;
const uint8_t PWMA = 6;
const uint8_t BIN1 = 5;
const uint8_t BIN2 = 4;
const uint8_t PWMB = 3;
const uint8_t KNOB[3] = {A0, A1, A2};

// Full scale of each knob: the middle is a tune that balances.
const float KP_MAX = 80.0;
const float KI_MAX = 600.0;
const float KD_MAX = 2.0;

const unsigned long PERIOD_US = 10000;
const float ALPHA = 0.98;
const float FALLEN = 45.0;
const float UPRIGHT = 10.0;
// The gyro rate, deg/s, that says the hand has let go.
const float RELEASED = 1.0;
// The most PWM the I term may hold, so it cannot wind up without limit.
const float I_LIMIT = 150.0;

SoftI2c imuBus(IMU_SCL, IMU_SDA, 400);
SoftI2c oledBus(A5, A4, 400);
Mpu6050 imu(imuBus);
Ssd1306 oled(oledBus, SSD1306_ADDRESS);

float kp = KP_MAX / 2;
float ki = KI_MAX / 2;
float kd = KD_MAX / 2;
float angle = 0;
float integral = 0;
int output = 0;
bool fallen = false;
bool started = false;
// True from power-up, and from being stood up, until the hand lets go.
bool held = true;
bool reported = false;
bool disturbed = false;
unsigned long steadySince = 0;

unsigned long lastUs = 0;
unsigned long printedAt = 0;
unsigned long drawnAt = 0;
uint8_t knob = 0;
uint8_t row = 0;

// What each OLED row last showed, so an unchanged row costs no bus time.
long shown[5] = {-99999, -99999, -99999, -99999, -99999};

void drive(int pwm) {
  bool forward = pwm >= 0;
  int duty = abs(pwm);
  if (duty > 255) duty = 255;
  digitalWrite(AIN1, forward ? HIGH : LOW);
  digitalWrite(AIN2, forward ? LOW : HIGH);
  digitalWrite(BIN1, forward ? HIGH : LOW);
  digitalWrite(BIN2, forward ? LOW : HIGH);
  analogWrite(PWMA, duty);
  analogWrite(PWMB, duty);
}

void brake() {
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);
}

// A value in hundredths, drawn with the given number of decimals. Only the
// value is cleared: the label never changes, and every pixel cleared and drawn
// again is bus time the loop is not balancing. Clearing the whole row sent the
// label again with every change, and that stall was enough to let a push, or
// a slow creep forward, throw the robot over.
void field(uint8_t r, const char *label, long hundredths, uint8_t decimals) {
  if (shown[r] == hundredths) return;
  shown[r] = hundredths;
  int16_t y = 16 + r * 10;
  oled.fillRect(48, y, 80, 8, false);
  oled.text(0, y, label);
  char buf[12];
  uint8_t n = 0;
  long v = hundredths;
  if (v < 0) {
    buf[n++] = '-';
    v = -v;
  }
  long whole = v / 100;
  long frac = v % 100;
  char digits[8];
  uint8_t d = 0;
  do {
    digits[d++] = (char)('0' + whole % 10);
    whole /= 10;
  } while (whole > 0 && d < 7);
  while (d > 0) buf[n++] = digits[--d];
  if (decimals > 0) {
    buf[n++] = '.';
    buf[n++] = (char)('0' + frac / 10);
    if (decimals > 1) buf[n++] = (char)('0' + frac % 10);
  }
  buf[n] = 0;
  oled.text(48, y, buf);
}

// One row of the screen per call, and one call every 200 ms. A row that
// changed costs milliseconds of drawing and software I2C, all of it time the
// loop is not balancing, so the screen is the slow part: drawing a row every
// 40 ms held the loop up for a fifth of the time and left the tune on a knife
// edge.
void drawRow() {
  switch (row) {
    case 0: field(0, "angle", (long)(angle * 100), 1); break;
    case 1: field(1, "Kp", (long)(kp * 100), 1); break;
    case 2: field(2, "Ki", (long)(ki * 100), 0); break;
    case 3: field(3, "Kd", (long)(kd * 100), 2); break;
    default:
      if (shown[4] != (long)fallen) {
        shown[4] = fallen;
        oled.fillRect(0, 56, 128, 8, false);
        oled.text(0, 56, fallen ? "FALLEN: stand me up" : "balancing");
      }
      break;
  }
  oled.display();
  row = (row + 1) % 5;
}

void readKnob() {
  float x = analogRead(KNOB[knob]) / 1023.0;
  if (knob == 0) kp = x * KP_MAX;
  if (knob == 1) ki = x * KI_MAX;
  if (knob == 2) kd = x * KD_MAX;
  knob = (knob + 1) % 3;
}

void setup() {
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  brake();
  Serial.begin(115200);
  imuBus.begin();
  oledBus.begin();
  if (!imu.begin()) {
    Serial.println("No MPU6050 at 0x68: check SCL and SDA");
  }
  imu.setRanges(0, 0);
  oled.begin();
  oled.clear();
  oled.text(0, 0, "BALANCE BOT");
  oled.hLine(0, 10, 128);
  oled.display();
  for (uint8_t i = 0; i < 3; i++) readKnob();
  Serial.println("Balance bot ready");
  lastUs = micros();
}

void loop() {
  unsigned long now = micros();
  if (now - lastUs < PERIOD_US) return;
  float dt = (now - lastUs) / 1000000.0;
  lastUs = now;

  Mpu6050Reading r;
  if (!imu.read(r)) return;
  float accelAngle = atan2(-(float)r.ax, (float)r.az) * 57.29578;
  float rate = r.gy / 131.0;
  if (!started) {
    angle = accelAngle;
    started = true;
  }
  angle = ALPHA * (angle + rate * dt) + (1 - ALPHA) * accelAngle;

  if (fallen) {
    brake();
    // Lying still, gravity is the whole story: trust the accelerometer, so
    // the moment it is stood up the angle is right.
    angle = accelAngle;
    if (fabs(angle) < UPRIGHT) {
      fallen = false;
      held = true;
      Serial.println("Upright again");
    }
  } else if (held) {
    // Held still by a hand: wait for it to let go before balancing. Driving
    // now would only fill the integral with a lean the wheels cannot fix,
    // and that stored push would throw it over the moment it was let go.
    // The gyro tells: held, the body cannot turn.
    brake();
    angle = accelAngle;
    if (fabs(rate) > RELEASED) {
      held = false;
      integral = 0;
      steadySince = millis();
      Serial.println("Let go: balancing");
    }
  } else if (fabs(angle) > FALLEN) {
    fallen = true;
    integral = 0;
    output = 0;
    reported = false;
    disturbed = false;
    brake();
    Serial.println("FALLEN");
  } else {
    integral += angle * dt;
    if (ki > 0.01) {
      float limit = I_LIMIT / ki;
      if (integral > limit) integral = limit;
      if (integral < -limit) integral = -limit;
    }
    float pid = kp * angle + ki * integral + kd * rate;
    output = (int)constrain(pid, -255, 255);
    drive(output);
    // A shove that tips it past 3 degrees, and the moment it is back.
    if (fabs(angle) > 3) {
      disturbed = true;
    } else if (disturbed && fabs(angle) < 1) {
      disturbed = false;
      Serial.println("Caught it: upright again");
    }
    if (fabs(angle) > 2) {
      steadySince = millis();
    } else if (!reported && millis() - steadySince >= 3000) {
      reported = true;
      Serial.println("Balanced for 3 s");
    }
  }

  readKnob();
  if (millis() - drawnAt >= 200) {
    drawnAt = millis();
    drawRow();
  }
  if (millis() - printedAt >= 250) {
    printedAt = millis();
    Serial.print("t=");
    Serial.print(millis());
    Serial.print(" angle=");
    Serial.print(angle, 2);
    Serial.print(" out=");
    Serial.println(output);
  }
}

Code walkthrough

The key functions of the sketch, and what each one does.

drive sketch.ino

Takes a signed PWM value. A positive value sets AIN1 and BIN1 high and AIN2 and BIN2 low, so both wheels go forward. A negative value flips them. The size is capped at 255 and sent to PWMA and PWMB with analogWrite.

void drive(int pwm) {
  bool forward = pwm >= 0;
  int duty = abs(pwm);
  if (duty > 255) duty = 255;
  digitalWrite(AIN1, forward ? HIGH : LOW);
  digitalWrite(AIN2, forward ? LOW : HIGH);
  digitalWrite(BIN1, forward ? HIGH : LOW);
  digitalWrite(BIN2, forward ? LOW : HIGH);
  analogWrite(PWMA, duty);
  analogWrite(PWMB, duty);
}

brake sketch.ino

Sets all four direction pins high and both PWM outputs to 0, which stops the wheels. The sketch calls it at power-up, when the robot has fallen, and while a hand is still holding it.

void brake() {
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);
}

readKnob sketch.ino

Reads one knob per call and scales it from 0 to 1. Knob 0 becomes kp, knob 1 becomes ki and knob 2 becomes kd, each up to its own maximum. Reading one knob at a time keeps each pass of the loop short.

void readKnob() {
  float x = analogRead(KNOB[knob]) / 1023.0;
  if (knob == 0) kp = x * KP_MAX;
  if (knob == 1) ki = x * KI_MAX;
  if (knob == 2) kd = x * KD_MAX;
  knob = (knob + 1) % 3;
}

loop sketch.ino

Waits until 10 ms have passed, then reads the IMU and updates the angle with the filter. While fallen or held it brakes. Otherwise it adds up the integral, limits it, computes the PID and calls drive. Then it reads a knob and sometimes redraws the screen.

void loop() {
  unsigned long now = micros();
  if (now - lastUs < PERIOD_US) return;
  float dt = (now - lastUs) / 1000000.0;
  lastUs = now;

  Mpu6050Reading r;
  if (!imu.read(r)) return;
  float accelAngle = atan2(-(float)r.ax, (float)r.az) * 57.29578;
  float rate = r.gy / 131.0;
  if (!started) {
    angle = accelAngle;
    started = true;
  }
  angle = ALPHA * (angle + rate * dt) + (1 - ALPHA) * accelAngle;

  if (fallen) {
    brake();
    // Lying still, gravity is the whole story: trust the accelerometer, so
    // the moment it is stood up the angle is right.
    angle = accelAngle;
    if (fabs(angle) < UPRIGHT) {
      fallen = false;
      held = true;
      Serial.println("Upright again");
    }
  } else if (held) {
    // Held still by a hand: wait for it to let go before balancing. Driving
  // ... 59 more lines in the editor

Troubleshooting

The usual mistakes with this circuit, and the fix for each.

  • The robot falls over right away or shakes hard This project
    Turn the knobs. The middle of each knob is a tune that balances, so go back there and change one gain at a time.
  • The Serial Monitor says No MPU6050 at 0x68 This project
    Check that pin 12 goes to the robot's SCL and pin 2 goes to its SDA. The IMU uses these pins, not A4 and A5.
  • The wheels never move even though it is tilted This project
    The motors stay braked until a hand lets go of the robot. Pick it up, hold it still, then release it.
  • The robot falls over right away. Self-balancing robot
    Use a fast loop (about 5 ms) and a correct sign: if it tips forward, the wheels must drive forward. Tune P first, then D, and add I last.
  • The angle reading drifts or jumps. Self-balancing robot
    Combine the gyro and accelerometer with a complementary or Kalman filter. Gyro alone drifts, and the accelerometer alone is noisy while moving.
  • The reading jumps around or only covers part of the range. Potentiometer
    Wire the two outer legs to 5 V and ground and the middle leg (the wiper) to an analog pin. Using the wrong two legs gives a dead or partial range.
  • Turning the knob does nothing, or the value is stuck at 0 or 1023. Potentiometer
    Check that the wiper goes to an analog input (A0 to A5 on an Uno), not a digital pin. Make sure both ends are connected, since a floating end gives noise.
  • The OLED stays blank. OLED display
    Check SDA and SCL are not swapped, and power it with 3.3 V or 5 V as its board allows. Most use address 0x3C, some 0x3D. Call display.begin() with the matching address.

Change it and keep it

Open it in the editor, change the circuit or the code, and keep your version in a free account.