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:
- Build the mechanical frame: two motors on the bottom, IMU mid-height, battery on top, all balanced over the wheels.
- Write the PID-driven balance sketch using yesterday's complementary filter as input.
- Tune Kp, Ki, Kd 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
- Read tilt angle (fused from IMU).
- Compute error = 0 − tilt (we want 0° = upright).
- PID output = motor speed.
- If tilting forward, drive wheels forward (catches up under the centre of mass).
- If tilting backward, drive wheels backward.
- 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'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
- Hold the robot upright by hand. Start with Kp = 10, Ki = 0, Kd = 0.
- Release. The robot probably oscillates and falls.
- Increase Kp until the response is more aggressive. Doubles until it overshoots wildly.
- Add Kd = Kp/15 to dampen. Most balancing robots use Kd ~5–15% of Kp.
- If the robot tracks but with a persistent angle (always 2° forward), add Ki = Kp/100.
Typical end values: Kp = 30..60, Ki = 0..0.5, Kd = 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 Kp up. Watch the response. Add Kd. 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).
Basic 5 min
Goal: Add an LED that lights when the robot is balanced (|angle| < 1° for > 2 s). Visual feedback.
1. Think first (on paper, no typing yet):
- Less than 1° for more than 2 s: what two things must be true, and what must the sketch remember?
- When should that memory be reset?
- Your loop must run 100 times a second. Why would delay() here make the robot fall?
2. Code it. Add an LED to your balancing sketch. No delay().
3. Upload and observe. Let the robot balance, watch the LED, then give it a nudge.
It works if:
- The LED comes on about 2 s after the robot settles.
- It goes off as soon as you nudge it.
- The robot balances no worse than before.
Challenge 1 5 min
Goal: "Fall recovery": when the robot has fallen, beep a buzzer 3 times, then resume balancing if it's been lifted back upright (|angle| < 10°).
Here is the buzzer pattern to aim for, each time the robot falls:
1. Think first (on paper, no typing yet):
- The robot is now in one of two modes: balancing or fallen. How will the sketch remember which one it is in?
- What moves it from fallen back to balancing? Why 10° and not 45°?
- How do you make it beep three times once per fall, not for ever?
2. Code it. Start from your balancing sketch. The motors stay off the whole time it is fallen.
3. Upload and observe. Let it fall over. Leave it a moment, then lift it slowly to upright and hold it there.
It works if:
- Exactly three beeps per fall, as in the diagram.
- Motors stay off while it lies there.
- Held within 10° of upright, it starts balancing again by itself, with no big jerk.
Challenge 2 5 min
Goal: Add a second PID for position control (using encoders). The robot balances AND tries to stay at a fixed position. Nudge it forward → it leans back to drive back to position. Foundation for the L04-43 final build.
1. Think first (on paper, no typing yet):
- The balance PID aims for 0°. If the robot has rolled 10 cm forward of its spot, which way must it lean to drive back?
- So what does the position PID's output become for the balance PID?
- Which of the two loops should run slower, and why?
2. Code it. Add the encoder counts from ARD-L04-27. The position PID's output is a small target angle. Limit it to a few degrees.
3. Upload and observe. Let it balance on a taped mark, then nudge it forward.
It works if:
- After a nudge, the robot leans back and returns to within about 10 cm of the mark.
- It does not run away or swing wildly.
- The target angle never goes past your limit.
Challenge 3 · Personal record 10 min
- Start the robot balanced. Time how long it stays upright without falling.
- 10+ seconds = working. 30+ seconds = polished. 5 minutes = exhibition-ready.
- Nudge test: how hard can you push before it falls? Note in degrees of tilt.
- 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. Kp drives correction; Kd 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.
Extra Mission 5 min
Part 1 — Design a self-levelling gadget
Your robot stays upright by reacting to its own tilt. The same idea keeps cameras, trays and telescopes level. On paper, design a gadget that keeps something level while the base tilts.
Your design must include:
- What it keeps level and why that matters to the person using it.
- Where the IMU sits, and what moves to correct the tilt (a servo or motor).
- How far it can correct, in degrees, before it gives up.
- What it should do when the tilt is too large.
Part 2 — Make it
Build a one-servo version. Fix the MPU6050 to the base and a small card platform to the servo arm. Use your fused angle to turn the servo the opposite way to the tilt, so the card stays level. Start with a simple rule: servo angle = 90 − tilt.
Bring back next class: your uploaded sketch and a short video of the platform staying level while you tilt the base. Bring a Nano 33 BLE Sense for ARD-L04-31 if you have one.