You can watch the robot in action here.
Recently I was gifted the LEGO Off-Road Buggy (42124), a LEGO Technic buggy that is remote controllable through an app. Once I had put the set together and started to drive it, I noticed something unusual: the app displayed pitch and roll angles. If you braked really hard or steered very sharply these angles would change a bit. Naturally, that made me wonder if it would be possible to turn the set into a self balancing robot (a robot that can balance itself with just one wheel, like a unicycle).
I started by building the robot strictly from pieces included in the set. I took care to get the center of gravity as high as possible to allow for the slowest possible reaction time. Also I actually added two wheels, one on each side, with each wheel being powered by a different motor. This allowed the robot to turn.
Once I had a physical prototype I started to write the software. The motors are controlled through a Bluetooth controller, so my first approach was to use Bluetooth to read out the sensors and send commands. I found the official LEGO Protocol that explains how to do this, as well as existing implementations in Python. With this I was able to read out the sensor and power the motor. However when experimenting I found out that the latency after detecting a change of angle was too large: powering the motor once that happened even at full power wasn’t able to stop the robot from falling over. I found some settings in Linux that reduced Bluetooth latency, but it wasn’t enough.
I then found PyBricks, a great project that includes custom firmware that you can flash on the Bluetooth controller. The firmware runs a Python interpreter, so, it is possible to run small Python programs directly on the robot itself. This made it possible to get rid of the Bluetooth latency.
One difficulty with this approach was that the “angle sensor” was a bit of a myth. The “angle sensor” is actually an accelerometer. This device measures the physics force that applies to it. When put at rest, it is not zero: the gravity force applies, so you would get a downward vector of about 9.8m/s². There are two ways you can convert this force to an angle: getting angular velocity from the change of force over time, or through comparison with the gravity vector. The first approach is quick but drifts over time. The second approach remains stable but is more noisy and slower. The best solution is to use a weighted average of the two.
Once the angle was computed I started with some very simple if statement programs: if the angle is larger than 0, drive forward, else backward. This turned out to be wholly insufficient, only stopping the robot from falling over for a few wobbles. After making my code more sophisticated with things like increasing the power based on how big the angle was I got somewhat better results.
The difficulty so far made me curious to how other self balancing robots did it, and I discovered that it was through the Proportional–integral–derivative (PID) controller. A PID controller is an algorithm specifically designed for these kind of problems. It has one number as input (the angle, positive or negative) and one number as output (the power, positive or negative). The goal is to minimize some error, in our case: the angle (which ideally should be 0). There are three parameters to tune it (P, I and D), roughly:
- how to respond to the angle (P),
- how to increase this response over time if it’s not enough (I) and
- how to react to trends (D), for example, if the angle quickly starts reducing then we should decrease the power.
Using the PID controller and a significant amount of tuning I was able to get the robot to a state where it would basically keep balancing until it ran out of power, achieving my goal.
That wraps it up, though there are many improvements possible. For example, it should be possible be make it user drivable. Stability might also be improved. But for now I have pasted the entire PyBricks program below. Be aware, it might require some adjusting based on surface, tire wear or engine power (make sure the batteries are full enough).
from pybricks import version
from pybricks.tools import wait, StopWatch
from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port
print("version:", version)
motor_a = Motor(Port.A)
motor_b = Motor(Port.B)
watch = StopWatch()
time = watch.time()
hub = TechnicHub()
tick_counter = 0
filtered_angle = hub.imu.tilt()[0]
target_angle = 0
prev_error = 0
integral = 0
distance = 0
# Below this value, the engine will stall
# (when the full weight is on it).
# Note: you can calibrate this by slowly increasing
# motor.dc() until motor.speed() becomes positive.
minimum_dc = 14
# Note: motor power (force) depends on battery level,
# that seems to be hub.battery.voltage()
# Almost empty: voltage ~5000.
# Completely full: voltage ~9000.
while True:
# Read sensors.
angle = hub.imu.tilt()[0]
velocity = -hub.imu.angular_velocity()[1]
# Motor speeds should be identical,
# but use the average to reduce potential errors.
motor_velocity = (motor_a.speed() - motor_b.speed()) / 2
new_time = watch.time()
time_delta = max(1, (new_time - time)) / 1000 # sec
time = new_time
# Compute total movement to help us stay in one place.
distance += motor_velocity * time_delta
# Combine the two accelerometer angle infer approaches
# to get the fastest and most stable angles.
velocity_delta = velocity * time_delta
# Quite high weight, causes multiple degrees of error on
# 90 degrees. However the speed is worth it around
# degrees 0 (at 90 it's way too late anyway).
weight = 0.999
filtered_angle = (
weight * (filtered_angle + velocity_delta) +
(1-weight) * angle)
# Try to adapt to the balance point / further reduce
# drift. By updating the target angle more/less when
# we are below/above it, we can make the bot go forward
# or backward. We use this mechanism to keep it in one
# place. This principle could also be used for a remote
# controller.
if distance < 0:
lower_target = 15
higher_target = 20
else:
lower_target = 20
higher_target = 15
if filtered_angle < target_angle:
target_angle += lower_target * time_delta
else:
target_angle -= higher_target * time_delta
# Scale linearly with error:
# higher angle error -> faster motor.
k_proportional = 0.15
# This controls how much extra force is added over time.
# The force will increase over time as we stay
# below / above the target angle.
k_integral = 0.5
# How much to react to error increases (positive) /
# decreases (negative).
# Higher values make it stutter more.
k_derivative = 0.0015
error = target_angle - filtered_angle
integral += error * time_delta
derivative = (error - prev_error) / time_delta
prev_error = error
speed = (
k_proportional * error +
k_integral * integral +
k_derivative * derivative)
speed = max(-1, min(speed, 1))
# Work around the stalling range in our DC output.
if speed >= 0:
speed = 100 * speed + minimum_dc * (1 - speed)
else:
speed = 100 * speed + -minimum_dc * (1 + speed)
# Control the engines.
motor_a.dc(speed)
motor_b.dc(-speed)
# Debug output
if tick_counter % 100 == 0:
print(speed, distance)
tick_counter += 1