Self-Balancing Robot v1

Learning Goals 5 min

Cluster E's capstone — the iconic Arduino build. An inverted-pendulum robot that stays upright through constant micro-corrections, like a Segway. Combines IMU + complementary filter + PID + encoder motors. By the end of this lesson you will:

1. Build the mechanical frame: two motors on the bottom, IMU mid-height, battery on top, all balanced over the wheels.
2. Write the PID-driven balance sketch using yesterday's complementary filter as input.
3. Tune K_p, K_i, K_d until the robot stays upright when nudged.

Warm-Up 10 min

Hardware:

• 2 × TT motors with encoders.
• 2 × wheels.
• L298N motor driver.
• MPU6050 IMU.
• Arduino UNO or Nano.
• Battery (2S LiPo or 4 × AA — needs to handle motor peaks).
• Build frame: 2–3 stacked levels of acrylic / plywood, ~25 cm tall, narrow enough to balance on the two wheels.

Why it's hard

An inverted pendulum is fundamentally unstable. Without the controller, the robot falls. With perfect PID, the robot tilts slightly, the wheels move into the fall, the tilt corrects, the wheels stop. 100 times per second. If the loop is too slow, or PID is mistuned, the robot oscillates and falls.

New Concept · The balance loop 25 min

The control architecture

1. Read tilt angle (fused from IMU).
2. Compute error = 0 − tilt (we want 0° = upright).
3. PID output = motor speed.
4. If tilting forward, drive wheels forward (catches up under the centre of mass).
5. If tilting backward, drive wheels backward.
6. Repeat at 100+ Hz.

The sketch shape

#include <Adafruit_MPU6050.h>
#include <Wire.h>
#include "PID.h"
#include "motor.h"

Adafruit_MPU6050 mpu;
const Motor motorL = {9, 8, 5};
const Motor motorR = {7, 6, 3};

// PID gains — TUNE THESE EXPERIMENTALLY
PID balancePID(30.0, 0.0, 2.0);

float fusedAngle = 0;
float gyroOffsetY = 0;
const float alpha = 0.98;
unsigned long lastT = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.begin();
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);

  motorInit(motorL);
  motorInit(motorR);

  // Calibrate gyro
  long sum = 0;
  for (int i = 0; i < 500; i++) {
    sensors_event_t a, g, t;
    mpu.getEvent(&a, &g, &t);
    sum += g.gyro.y * 1000.0;
    delay(2);
  }
  gyroOffsetY = sum / 1000.0 / 500;

  lastT = millis();
}

void loop() {
  unsigned long now = millis();
  if (now - lastT < 10) return;       // 100 Hz
  float dt = (now - lastT) / 1000.0;
  lastT = now;

  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);

  float accelAngle = atan2(a.acceleration.x,
                           sqrt(a.acceleration.y * a.acceleration.y +
                                a.acceleration.z * a.acceleration.z))
                     * 180.0 / PI;
  float gyroRate = (g.gyro.y - gyroOffsetY) * 180.0 / PI;
  fusedAngle = alpha * (fusedAngle + gyroRate * dt) + (1 - alpha) * accelAngle;

  // Safety: if we&apos;ve fallen, kill motors
  if (abs(fusedAngle) > 45) {
    motorSpeed(motorL, 0);
    motorSpeed(motorR, 0);
    return;
  }

  float u = balancePID.update(0.0, fusedAngle);
  int duty = constrain((int)u, -255, 255);

  motorSpeed(motorL, duty);
  motorSpeed(motorR, duty);
}

Tuning procedure

1. Hold the robot upright by hand. Start with K_p = 10, K_i = 0, K_d = 0.
2. Release. The robot probably oscillates and falls.
3. Increase K_p until the response is more aggressive. Doubles until it overshoots wildly.
4. Add K_d = K_p/15 to dampen. Most balancing robots use K_d ~5–15% of K_p.
5. If the robot tracks but with a persistent angle (always 2° forward), add K_i = K_p/100.

Typical end values: K_p = 30..60, K_i = 0..0.5, K_d = 1..5. Depends entirely on robot weight, wheel size, motor torque.

Worked Example · Build and balance 30 min

Step 1 — frame

3 stacked levels of 5 mm plywood / 3 mm acrylic, 10 × 15 cm each. Bolt them together with M3 hardware + 30 mm spacers. Top level holds the battery (heavy), bottom holds the motors. The IMU goes mid-height, centred over the wheel axis.

Step 2 — wire

• Motors → L298N → battery + UNO common ground.
• MPU6050 → A4/A5 + 3.3V/GND.
• Encoders → D2 + D3 (optional; for v1 you can skip the encoders and just balance).
• Power: UNO via L298N's 5V regulator OR a separate 5V buck on the battery rail.

Step 3 — calibrate before balancing

Lay the robot flat. Read accel — adjust the IMU's mounting until the "upright" angle reads close to 0°. (If it reads −2°, your robot will always lean back; fix mechanically rather than in code.)

Step 4 — first try

Hold robot vertical. Power on. Release. Probably falls. Don't panic — that's expected.

Step 5 — tune iteratively

Start gains low. Bump K_p up. Watch the response. Add K_d. Plot fusedAngle + motor duty on Serial Plotter.

Once the robot balances for ~5 seconds, you've done it. Nudge it gently. Watch it correct. Push harder. See how much disturbance it tolerates.

Step 6 — record + iterate

Take video of the working balance. Note the final gains. The robot is now the foundation for L04-43 (when we'll add wireless control + position holding).

Try It Yourself 15 min

Mini-Challenge · Personal record 10 min

1. Start the robot balanced. Time how long it stays upright without falling.
2. 10+ seconds = working. 30+ seconds = polished. 5 minutes = exhibition-ready.
3. Nudge test: how hard can you push before it falls? Note in degrees of tilt.
4. Record video.

Recap 5 min

Self-balancing robot = fused IMU angle (L04-29) + PID controller (L04-24) + bidirectional motor driver (L03-08) at 100 Hz. K_p drives correction; K_d damps oscillation. Centre of mass over wheels. Fall-safe motors-off if angle exceeds 45°. Cluster E done. Cluster F (Edge AI) starts tomorrow.

Inverted pendulum

A pendulum balanced upside-down. Unstable equilibrium. Self-balancing robots and SpaceX rocket landings are both inverted-pendulum control problems.

100 Hz loop

Updating 100 times per second. Each cycle: read IMU, fuse, PID, drive motors. Faster = better stability.

Centre of mass

The point through which gravity effectively acts. Must be vertically aligned over the wheel axis for the robot to balance.

Fall detection

Safety check: if tilt exceeds a threshold (45°), kill motors. Prevents the robot driving madly when fallen.

Tune iteratively

Adjust gains one at a time, observe, repeat. The only reliable way to tune a PID in the real world.

Motor backlash

Play in the gearbox. A small angular range where the motor turns without the wheel turning. Kills precise balance.

Homework 5 min

1. Get the robot balancing for at least 10 seconds. Video it.
2. Save the final gains in your notebook.
3. Read ahead to ARD-L04-31 (What Is Edge AI?). Bring a Nano 33 BLE Sense if you have one — Edge AI works best on that board.