1D Ball Balancing (Servo + TOF Sensor)
Time estimate: ~90 minutes Prerequisites: SSH Login, Enable I2C
Learning Objectives
By the end of this tutorial you will be able to:
- Control a servo motor via Linux hardware PWM (sysfs interface)
- Read distance measurements from a VL53L0X TOF (Time-of-Flight) sensor
- Implement a PID controller to balance a ball on a tilting beam
- Measure control loop timing and compare standard vs PREEMPT_RT kernel jitter
Real-Time Control Loops on Linux
A closed-loop control system reads a sensor, computes a correction, and drives an actuator -- all within a fixed time budget called the control period. For ball balancing at 50 Hz, each iteration must complete within 20 ms. If the loop takes longer, the controller misses its deadline and the ball falls.
Jitter -- the variation in loop timing -- is the enemy of control quality. A PID controller assumes uniform time steps; when dt varies unpredictably, the derivative term over- or under-corrects, causing oscillation. Standard Linux is not a real-time OS: kernel preemption, garbage collection pauses, and I/O scheduling can introduce multi-millisecond jitter spikes.
PREEMPT_RT is a kernel patch set that makes nearly all kernel code preemptible, reducing worst-case scheduling latency from tens of milliseconds to tens of microseconds. For control loops above ~100 Hz or safety-critical systems, PREEMPT_RT (or a dedicated microcontroller) is essential.
The latency budget for each loop iteration must account for: sensor read time (I2C transaction ~1-2 ms), PID computation (~0.01 ms), actuator write (sysfs write ~0.1 ms), and OS scheduling overhead (variable). If the sum exceeds the period, reduce the loop rate or optimize the bottleneck.
For the full theory on real-time scheduling, jitter measurement, and PREEMPT_RT, see Real-Time Systems.
Introduction
A ball rolling on a beam, balanced by a servo motor, is one of the simplest real-time control problems. The sensor measures ball position, a PID controller calculates the correction, and the servo tilts the beam. The loop must run fast enough (>50 Hz) to prevent the ball from rolling off, and with consistent timing to avoid oscillation.
This makes it an ideal test bed for embedded Linux real-time behavior. You will measure the control loop jitter on a standard kernel and compare it with PREEMPT_RT — the same question asked in the Jitter Measurement tutorial, but now with physical consequences: if the kernel stalls, the ball falls.
Hardware
- MG996R servo — high-torque standard servo, 50 Hz PWM control, ~4.8-7.2V
- VL53L0X — laser Time-of-Flight sensor, I2C, 0-2m range, up to ~50 Hz measurement rate
- Beam — a rigid strip or channel (30-50 cm) with the ball rolling freely
- Ball — a small ball (ping pong or marble)
Warning
The MG996R draws significant current (up to 2.5A stall). Do not power it from the Pi's 5V GPIO pin — use a separate 5-6V power supply with common ground to the Pi. Powering high-torque servos from the Pi can cause brownouts and SD card corruption.
1. Servo Control via Hardware PWM
Concept: Standard servos expect a 50 Hz PWM signal where the pulse width (1.0-2.0 ms) determines the angle. The Pi's hardware PWM is accessible via sysfs at /sys/class/pwm/.
Enable Hardware PWM
Add to /boot/firmware/config.txt:
Inside the PWM Overlay
This overlay configures a hardware PWM channel on the Pi's BCM2835/BCM2711 SoC. Decompile it to see:
The parameters pin=18,func=2 map to:
pin=18— GPIO18 (physical pin 12), one of the two GPIOs that can output hardware PWM on the Pifunc=2— GPIO alternate function 5 (ALT5), which connects GPIO18 to the PWM0 hardware peripheral
The Pi's GPIOs are multiplexed — each pin can serve multiple functions depending on which alternate function register is active. The BCM2711 datasheet (Section 5.3, Alternative Function Assignments) shows GPIO18 has:
| ALT0 | ALT1 | ALT2 | ALT3 | ALT4 | ALT5 |
|---|---|---|---|---|---|
| PCM_CLK | SD10 | — | — | SPI6_CE0 | PWM0_0 |
The overlay sets ALT5, connecting this pin to the hardware PWM timer. Without the overlay, GPIO18 is a general-purpose I/O with no PWM capability.
The device tree node added by the overlay:
fragment@0 {
target = <&gpio>;
__overlay__ {
pwm_pins: pwm_pins {
brcm,pins = <18>;
brcm,function = <2>; /* ALT5 */
};
};
};
fragment@1 {
target = <&pwm>;
__overlay__ {
pinctrl-names = "default";
pinctrl-0 = <&pwm_pins>;
status = "okay";
};
};
For custom images: Include CONFIG_PWM_BCM2835=y in your kernel config and add the PWM + pinctrl nodes to your device tree. The brcm,function value selects the alt function.
Reboot. Then export the PWM channel:
The sysfs PWM Interface
The Linux PWM subsystem (drivers/pwm/) exposes hardware PWM channels through sysfs. The interface mirrors how the kernel driver manages the hardware:
/sys/class/pwm/pwmchip0/ ← PWM controller (one per hardware block)
├── npwm ← number of channels (2 on Pi: PWM0, PWM1)
├── export ← write channel number to create pwmN directory
├── unexport ← write channel number to remove pwmN directory
└── pwm0/ ← exported channel 0
├── period ← total cycle time in nanoseconds
├── duty_cycle ← high-time in nanoseconds (must be ≤ period)
├── polarity ← "normal" or "inversed"
└── enable ← 1 = output active, 0 = output idle
Writing to export calls the driver's pwm_request() function, which claims the hardware channel and creates the sysfs directory. Writing to period and duty_cycle programs the hardware timer registers. The hardware generates the PWM waveform independently — the CPU is not involved once configured.
Timing precision: The BCM2835 PWM peripheral runs from a 19.2 MHz oscillator. The minimum period resolution is ~52 ns. For servo control at 50 Hz (20 ms period), this gives ~384,000 discrete duty cycle steps — far more than any servo can resolve.
Configure for Servo
Standard hobby servos expect a 50 Hz PWM signal (20 ms period) where the pulse width encodes the target angle:
| Pulse Width | Typical Angle | Duty Cycle (ns) |
|---|---|---|
| 1.0 ms | ~0° | 1,000,000 |
| 1.5 ms | ~90° (center) | 1,500,000 |
| 2.0 ms | ~180° | 2,000,000 |
PWM=/sys/class/pwm/pwmchip0/pwm0
# Period = 20ms = 20,000,000 ns (50 Hz)
echo 20000000 | sudo tee $PWM/period
# Duty cycle = 1.5ms = 1,500,000 ns (center position)
echo 1500000 | sudo tee $PWM/duty_cycle
# Enable
echo 1 | sudo tee $PWM/enable
The servo should move to center position.
Test Range
# Minimum angle (~0°): 1.0ms pulse
echo 1000000 | sudo tee $PWM/duty_cycle
# Center (~90°): 1.5ms pulse
echo 1500000 | sudo tee $PWM/duty_cycle
# Maximum angle (~180°): 2.0ms pulse
echo 2000000 | sudo tee $PWM/duty_cycle
Python Servo Helper
python3 - <<'PY'
import time
PWM_PATH = "/sys/class/pwm/pwmchip0/pwm0/duty_cycle"
def set_angle(degrees):
"""Map 0-180 degrees to 1.0-2.0ms pulse width."""
degrees = max(0, min(180, degrees))
pulse_us = 1000 + (degrees / 180.0) * 1000 # 1000-2000 us
pulse_ns = int(pulse_us * 1000)
with open(PWM_PATH, "w") as f:
f.write(str(pulse_ns))
# Sweep test
for angle in range(60, 121, 5):
set_angle(angle)
time.sleep(0.3)
set_angle(90) # return to center
PY
Checkpoint
The servo sweeps from 60° to 120° and returns to center. The movement is smooth and repeatable.
Stuck?
- "No such file: pwm0" — the PWM overlay is not loaded. Check
/boot/firmware/config.txtand reboot. - Servo jitters at rest — electrical noise on the PWM pin. Ensure the servo has its own power supply, not GPIO 5V.
- Servo doesn't move — verify the signal wire is connected to GPIO18 (pin 12 on the header).
2. VL53L0X Distance Sensor
Concept: The VL53L0X is a laser Time-of-Flight sensor that measures distance by timing how long a photon takes to bounce back. It connects via I2C and can measure at up to ~50 Hz.
Verify I2C
The VL53L0X should appear at address 0x29:
Install Python Library
What the Library Hides: VL53L0X I2C Protocol
The Adafruit library abstracts a complex I2C register protocol. Understanding it matters when you need to optimize measurement speed, debug communication failures, or port to a custom image without Python.
The VL53L0X communicates over I2C at address 0x29. Key register operations:
| Operation | Registers | What It Does |
|---|---|---|
| Init sequence | ~40 register writes | Loads calibration data, sets measurement mode |
| Start measurement | Write 0x01 to reg 0x00 |
Triggers a single-shot TOF measurement |
| Poll for completion | Read reg 0x13, check bit 0 |
Returns 1 when measurement is ready |
| Read distance | Read 2 bytes from reg 0x14+0x15 |
16-bit distance in millimeters |
| Clear interrupt | Write 0x01 to reg 0x0B |
Resets the "measurement ready" flag |
The measurement timing budget controls how long the sensor averages the reflected laser signal. A 20 ms budget gives ~50 Hz with ±25 mm accuracy at short range. A 200 ms budget gives ~5 Hz with ±5 mm accuracy. The trade-off between speed and accuracy is critical for control loop design.
Direct I2C access (without the library):
import smbus2, time
bus = smbus2.SMBus(1)
VL53L0X_ADDR = 0x29
# Read model ID to verify communication
model_id = bus.read_byte_data(VL53L0X_ADDR, 0xC0)
print(f"Model ID: 0x{model_id:02X}") # Should be 0xEE
Kernel driver alternative: Linux has a VL53L0X IIO driver (CONFIG_VL53L0X). If enabled, the sensor appears at /sys/bus/iio/devices/iio:deviceN/in_distance_raw, following the same pattern as the BMI160 IMU. For a custom Buildroot image, the kernel driver is cleaner than shipping a Python library.
Test Reading
python3 - <<'PY'
import board, busio, adafruit_vl53l0x, time
i2c = busio.I2C(board.SCL, board.SDA)
tof = adafruit_vl53l0x.VL53L0X(i2c)
# Set measurement timing budget (lower = faster, less accurate)
# 20000 us = 20 ms → ~50 Hz measurement rate
# Trade-off: faster = noisier, slower = smoother but laggier control
tof.measurement_timing_budget = 20000
for _ in range(20):
dist = tof.range
print(f"Distance: {dist} mm")
time.sleep(0.05)
PY
Move your hand or the ball closer and farther from the sensor. The distance should track your movement.
Checkpoint
VL53L0X reports distance in millimeters that changes when you move an object in front of it.
3. Mechanical Assembly
Concept: The servo tilts one end of a beam. The VL53L0X is mounted at one end, looking along the beam to measure the ball's position.
Layout
VL53L0X (looking along beam)
│
▼
┌─────────────────────────────────┐
│ ● │ ← beam (30-50 cm)
│ ball rolls freely │
└───────────┬─────────────────────┘
│
┌────┴────┐
│ Servo │ ← pivot point (center of beam)
│ MG996R │
└─────────┘
- Mount the servo at the beam's center (or near one end as a fulcrum)
- Attach the beam to the servo horn
- Mount the VL53L0X at one end, pointing along the beam
- The ball rests on the beam and rolls based on tilt angle
Tip
Start with a flat channel (aluminum angle, cardboard V-fold, or 3D-printed track) so the ball can only roll in one dimension.
4. PID Controller
Concept: A PID controller calculates the servo angle correction based on the error (distance between the ball's current position and the target position). The three terms — Proportional, Integral, Derivative — each address a different aspect of control quality.
Tip
For the full mathematical derivation of PID control — including transfer functions, discrete-time implementation, Ziegler-Nichols tuning, and anti-windup — see Real-Time Systems § Control Theory Foundations.
PID Theory (Brief)
error(t) = setpoint - measured_position
P term = Kp × error → push toward target (strength)
I term = Ki × ∫error dt → eliminate steady-state error (memory)
D term = Kd × d(error)/dt → dampen oscillation (prediction)
output = P + I + D → servo angle correction
Implementation
cat > ~/ball-balance/balance_1d.py << 'EOF'
#!/usr/bin/env python3
"""1D ball balancing: PID control with servo + VL53L0X."""
import time, math, csv, os
# ── Hardware setup ──────────────────────────────────
import board, busio, adafruit_vl53l0x
i2c = busio.I2C(board.SCL, board.SDA)
tof = adafruit_vl53l0x.VL53L0X(i2c)
tof.measurement_timing_budget = 20000 # ~50 Hz
PWM_PATH = "/sys/class/pwm/pwmchip0/pwm0/duty_cycle"
def set_servo_angle(degrees):
degrees = max(60, min(120, degrees)) # limit tilt range
pulse_ns = int((1000 + degrees / 180.0 * 1000) * 1000)
with open(PWM_PATH, "w") as f:
f.write(str(pulse_ns))
# ── PID controller ──────────────────────────────────
SETPOINT = 150 # target distance in mm (center of beam)
Kp = 0.08
Ki = 0.001
Kd = 0.05
integral = 0.0
prev_error = 0.0
BASE_ANGLE = 90.0 # level position
# ── Data logging ────────────────────────────────────
LOG_FILE = "/tmp/balance_log.csv"
log = open(LOG_FILE, "w", newline="")
writer = csv.writer(log)
writer.writerow(["time_ms", "distance_mm", "error", "p_term", "i_term",
"d_term", "output", "angle", "loop_dt_ms"])
# ── Control loop ────────────────────────────────────
LOOP_HZ = 50
LOOP_PERIOD = 1.0 / LOOP_HZ
print(f"Balancing at setpoint={SETPOINT}mm, PID=({Kp}, {Ki}, {Kd})")
print(f"Target loop rate: {LOOP_HZ} Hz ({LOOP_PERIOD*1000:.0f} ms)")
print("Press Ctrl+C to stop.")
t_start = time.monotonic()
t_prev = t_start
iteration = 0
try:
while True:
t_loop_start = time.monotonic()
# Read sensor
distance = tof.range
if distance > 500:
distance = 500 # clamp outliers
# PID calculation
dt = t_loop_start - t_prev
t_prev = t_loop_start
if dt < 0.001:
dt = 0.001 # safety
error = SETPOINT - distance
integral += error * dt
integral = max(-500, min(500, integral)) # anti-windup
derivative = (error - prev_error) / dt
prev_error = error
p_term = Kp * error
i_term = Ki * integral
d_term = Kd * derivative
output = p_term + i_term + d_term
angle = BASE_ANGLE + output
set_servo_angle(angle)
# Log
elapsed_ms = (t_loop_start - t_start) * 1000
loop_dt_ms = dt * 1000
writer.writerow([f"{elapsed_ms:.1f}", distance, f"{error:.1f}",
f"{p_term:.3f}", f"{i_term:.3f}", f"{d_term:.3f}",
f"{output:.3f}", f"{angle:.1f}", f"{loop_dt_ms:.1f}"])
iteration += 1
if iteration % 50 == 0:
print(f" dist={distance:4d}mm err={error:+6.1f} "
f"angle={angle:5.1f}° dt={loop_dt_ms:5.1f}ms")
# Wait for next loop period
elapsed = time.monotonic() - t_loop_start
sleep_time = LOOP_PERIOD - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
except KeyboardInterrupt:
set_servo_angle(90)
log.close()
print(f"\nStopped. Log saved to {LOG_FILE}")
print(f"Total iterations: {iteration}")
EOF
mkdir -p ~/ball-balance
mv ~/ball-balance/balance_1d.py ~/ball-balance/balance_1d.py 2>/dev/null
Checkpoint
The script runs, reads the sensor, and moves the servo. The ball may not balance yet — that requires PID tuning (next section).
5. PID Tuning
Concept: PID gains determine the control quality. Start with P-only control, then add D to dampen oscillation, then add I to eliminate steady-state error.
Tuning Procedure
-
Set Ki=0, Kd=0. Increase Kp until the ball oscillates around the setpoint but doesn't fly off. This is your starting Kp.
-
Add Kd. Increase Kd until the oscillation dampens and the ball settles. The derivative term acts as a brake.
-
Add Ki (optional). If the ball settles at a consistent offset from the setpoint, a small Ki corrects it. Too much Ki causes slow oscillation.
Step Response Test
Place the ball at the sensor end and release it. The controller should bring it to the setpoint. Record:
| Metric | Kp-only | Kp+Kd | Kp+Ki+Kd |
|---|---|---|---|
| Settling time (s) | _ | _ | _ |
| Overshoot (mm) | _ | _ | _ |
| Steady-state error (mm) | _ | _ | _ |
| Oscillation | _ | _ | _ |
Analyze the Log
# Plot the logged data (on your host machine):
python3 -c "
import csv, matplotlib.pyplot as plt
data = list(csv.DictReader(open('/tmp/balance_log.csv')))
t = [float(r['time_ms'])/1000 for r in data]
d = [float(r['distance_mm']) for r in data]
a = [float(r['angle']) for r in data]
fig, (ax1, ax2) = plt.subplots(2, 1, sharex=True)
ax1.plot(t, d); ax1.axhline(150, color='r', ls='--'); ax1.set_ylabel('Distance (mm)')
ax2.plot(t, a); ax2.set_ylabel('Servo angle (°)'); ax2.set_xlabel('Time (s)')
plt.savefig('/tmp/balance_plot.png', dpi=100); print('Saved to /tmp/balance_plot.png')
"
Checkpoint
The ball balances near the setpoint with minimal oscillation. You have tuning parameters that produce a stable system.
6. Control Loop Timing Measurement
Concept: The control loop must run at a consistent rate. Jitter (variation in loop period) causes the derivative term to over- or under-correct, reducing control quality.
Analyze Loop Timing from the Log
python3 - <<'PY'
import csv
data = list(csv.DictReader(open("/tmp/balance_log.csv")))
dts = [float(r["loop_dt_ms"]) for r in data if float(r["loop_dt_ms"]) > 0]
avg = sum(dts) / len(dts)
mx = max(dts)
mn = min(dts)
jitter = mx - mn
# Count outliers (> 2× target period)
target = 20.0 # 50 Hz = 20 ms
outliers = sum(1 for d in dts if d > target * 2)
print(f"Loop timing ({len(dts)} iterations):")
print(f" Target: {target:.1f} ms")
print(f" Average: {avg:.2f} ms")
print(f" Min: {mn:.2f} ms")
print(f" Max: {mx:.2f} ms")
print(f" Jitter: {jitter:.2f} ms")
print(f" Outliers (>{target*2:.0f}ms): {outliers} ({outliers/len(dts)*100:.1f}%)")
PY
Standard vs PREEMPT_RT Comparison
Run the balancing controller on both kernels and compare:
| Metric | Standard Kernel | PREEMPT_RT Kernel |
|---|---|---|
| Average loop time (ms) | _ | _ |
| Maximum loop time (ms) | _ | _ |
| Jitter (max - min, ms) | _ | _ |
| Outliers (>40 ms) | _ | _ |
| Ball stability (subjective) | _ | _ |
To stress-test the kernel during balancing:
Does the ball drop under load? How much worse is jitter?
Checkpoint
You have loop timing data for at least one kernel configuration and can quantify the jitter.
What Just Happened?
You built a closed-loop real-time control system on Linux:
VL53L0X reads distance (I2C, ~50 Hz)
→ PID calculates correction
→ Servo adjusts beam angle (PWM, 50 Hz)
→ Ball position changes
→ Sensor reads new position
→ Loop repeats every 20 ms
The ball physically falls if the control loop doesn't run on time — making jitter visible and consequential. This is a concrete demonstration of why real-time guarantees matter in embedded systems.
Challenges
Challenge 1: Disturbance Rejection
While the ball is balanced, gently push it with a pencil. Record how long the controller takes to re-balance. Measure the recovery time for different push strengths. This tests the controller's disturbance rejection.
Challenge 2: C Implementation
Rewrite the control loop in C with direct I2C reads (/dev/i2c-1) and sysfs PWM writes. Compare the loop timing jitter between Python and C. How much does the language runtime affect control quality?
Challenge 3: Live Visualization
Add an SDL2 display showing real-time ball position, setpoint, and PID terms as a strip chart. This combines the SDL2 tutorials with the control loop — a live dashboard for the balancing system.
Deliverable
- [ ] Ball balances on the beam at the target setpoint
- [ ] PID gains tuned with step response table filled in
- [ ] Control loop timing table filled in (at least one kernel config)
- [ ] CSV log file with timing data for offline analysis
Course Overview | Previous: ← IMU Controller | Next: 2D Plate Balancing →