Examples
The repo ships runnable example programs in
examples/.
Push any of them to a hub with openbricks run -n <name> <script> or
try them in the simulator with openbricks sim run <script>.
Two representative ones are reproduced below.
Drive a square (ST-3032 drivebase)
# SPDX-License-Identifier: MIT
"""
Drive a square with the ST-3032 drivebase.
Four sides of ``SIDE_MM`` straight + ``+90°`` turn each. Demonstrates
``DriveBase.straight`` / ``turn`` composing into a closed loop — if
the chassis geometry (``WHEEL_DIAMETER_MM`` / ``AXLE_TRACK_MM``) is
calibrated correctly, the robot returns to within a few cm of its
starting pose after one lap.
Uses the new ``then=Stop.COAST`` default end-state (1.6.7), so the
wheels free-wheel briefly between segments. If your bench has
significant momentum carryover and you'd rather pin the wheels
between sides, pass ``then=Stop.BRAKE`` to the ``straight`` / ``turn``
calls (or ``then=Stop.HOLD`` for active position lock — ST-3032
supports it).
Hardware: identical to ``examples/st3032_drivebase_test.py``.
Run with:
openbricks run -n ls examples/st3032_drivebase_square.py
"""
from openbricks.drivers.st3032 import ST3032Motor
from openbricks.robotics import DriveBase
from openbricks.parameters import Stop
LEFT_ID, RIGHT_ID = 2, 1
UART_ID, TX, RX = 1, 14, 41
WHEEL_DIAMETER_MM = 88
AXLE_TRACK_MM = 136
SIDE_MM = 200
NUM_LAPS = 1
STRAIGHT_SPEED_MM = 150
TURN_RATE_DPS = 200
def main():
print("--- ST-3032 drivebase square (%d mm sides × %d laps) ---" %
(SIDE_MM, NUM_LAPS))
left = ST3032Motor(servo_id=LEFT_ID, uart_id=UART_ID, tx=TX, rx=RX, invert=True)
right = ST3032Motor(servo_id=RIGHT_ID, uart_id=UART_ID, tx=TX, rx=RX)
db = DriveBase(left, right,
wheel_diameter_mm=WHEEL_DIAMETER_MM,
axle_track_mm=AXLE_TRACK_MM)
db.settings(straight_speed=STRAIGHT_SPEED_MM, turn_rate=TURN_RATE_DPS)
for lap in range(NUM_LAPS):
for side in range(4):
print(" lap %d side %d: straight(%d) → turn(+90)" %
(lap + 1, side + 1, SIDE_MM))
db.straight(SIDE_MM)
db.turn(90)
print("--- done ---")
main()
Gyro-corrected square (ICM-45686)
With an ICM-45686 attached, use_gyro(True) moves heading control
off the encoders and onto measured body rotation — corrected every
millisecond inside the firmware’s 1 kHz control tick, with no Python
in the loop. This script drives the same square twice, encoders-only
and gyro-corrected, and prints each pass’s heading drift so the
difference is a number, not an impression. On the reference bench the
gyro pass returns within ~0.6° over all four turns; wheel slip that
would bend the encoder pass simply gets steered back out.
# SPDX-License-Identifier: MIT
"""
Gyro-corrected square — ICM-45686 on the hard tick.
The ICM-45686 is read INSIDE the 1 kHz control tick over SPI, so
with ``use_gyro(True)`` the DriveBase corrects heading every
millisecond in C — no Python in the loop. This script drives a
square twice, encoder-only and then gyro-corrected, and prints how
far each pass drifts from its starting heading. Bench 2026-08-10:
the gyro pass returned within +0.6 degrees over all four turns.
The learned gyro bias is persisted to NVS after the stillness
lock, so later boots start corrected without the ~0.5 s wait.
Wiring: breakout on 3V3/GND plus the four SPI pins below; INT
stays unwired (the tick polls). Needs ~0.5 m of clear floor.
Run:
openbricks run -n <hub> examples/icm45686_square.py
"""
import time
from openbricks.drivers.icm45686 import ICM45686
from openbricks.drivers.st3032 import ST3032Motor
from openbricks.robotics import DriveBase
from openbricks.parameters import Stop
LEFT_ID, RIGHT_ID = 2, 1
UART_ID, TX, RX = 1, 14, 41
SCK, MOSI, MISO, CS = 12, 13, 11, 17
WHEEL_DIAMETER_MM = 88
AXLE_TRACK_MM = 136
STRAIGHT_SPEED = 300
TURN_RATE = 200
SIDE_MM = 300
def square_drift(db, imu):
start = imu.heading()
for side in range(4):
db.straight(SIDE_MM)
db.turn(90)
print("corner %d: heading %.1f" % (side + 1, imu.heading() - start))
return imu.heading() - start - 360
print("hold still for gyro bias lock ...")
imu = ICM45686(sck=SCK, mosi=MOSI, miso=MISO, cs=CS)
while not imu.calibrated():
time.sleep_ms(100)
imu.save_calibration()
left = ST3032Motor(servo_id=LEFT_ID, uart_id=UART_ID, tx=TX, rx=RX,
invert=True)
right = ST3032Motor(servo_id=RIGHT_ID, uart_id=UART_ID, tx=TX, rx=RX)
db = DriveBase(left, right, wheel_diameter_mm=WHEEL_DIAMETER_MM,
axle_track_mm=AXLE_TRACK_MM, imu=imu)
db.settings(straight_speed=STRAIGHT_SPEED, turn_rate=TURN_RATE)
print("pass 1: encoders only")
print("drift: %+.1f deg" % square_drift(db, imu))
db.use_gyro(True)
print("pass 2: gyro-corrected at 1 kHz")
print("drift: %+.1f deg" % square_drift(db, imu))
db.stop(then=Stop.BRAKE)
Wiring for the IMU is four SPI pins plus power — see
Hardware guide. The first still half-second learns the gyro bias;
save_calibration() persists it so later boots skip the wait.
Rounded square (DriveBase.curve)
curve(radius, angle) follows the Pybricks contract — positional
order, parameter names, and sign semantics: positive angle arcs
right (clockwise), a negative radius drives the arc backward, and
curve(0, angle) degrades to a turn in place. The forward and turn
profiles run with proportionally scaled speed and acceleration, so
the path is a true circle even through the ramps, and the outer
wheel is automatically capped at the straight_speed setting.
(One deviation: then defaults to Stop.COAST like every openbricks
move; pass then=Stop.HOLD for the Pybricks end state.)
# SPDX-License-Identifier: MIT
"""Drive a rounded square with DriveBase.curve().
Four straights joined by four quarter-circle arcs — the robot never
stops to pivot, so the lap is faster and smoother than the
straight+turn square. curve(radius, angle) follows Pybricks: positive
angle arcs right, the radius sign picks forward/backward, and the
outer wheel is automatically capped at the straight_speed setting.
Run with:
openbricks run -n ls examples/st3032_drivebase_curve.py
"""
from openbricks.drivers.st3032 import ST3032Motor
from openbricks.robotics import DriveBase
LEFT_ID, RIGHT_ID = 2, 1
UART_ID, TX, RX = 1, 14, 41
WHEEL_DIAMETER_MM = 88
AXLE_TRACK_MM = 136
SIDE_MM = 150
RADIUS_MM = 60
NUM_LAPS = 1
left = ST3032Motor(servo_id=LEFT_ID, uart_id=UART_ID, tx=TX, rx=RX,
invert=True)
right = ST3032Motor(servo_id=RIGHT_ID, uart_id=UART_ID, tx=TX, rx=RX)
db = DriveBase(left, right,
wheel_diameter_mm=WHEEL_DIAMETER_MM,
axle_track_mm=AXLE_TRACK_MM)
db.settings(straight_speed=150, turn_rate=200)
print("rounded square: %d mm sides, %d mm corner radius" %
(SIDE_MM, RADIUS_MM))
for lap in range(NUM_LAPS):
for side in range(4):
db.straight(SIDE_MM)
db.curve(radius=RADIUS_MM, angle=90)
print("done")
Colour sensor, direct to the ESP32 (one TCS34725)
Mode 1 from the hardware guide: a single TCS34725 on GPIO 15/16, no multiplexer. The driver is handed the bus itself.
# SPDX-License-Identifier: MIT
"""
Example: read RGB from a TCS34725 over I2C and classify the color.
Hardware:
* ESP32-S3 (or classic ESP32)
* TCS34725 breakout on I2C bus 0 (3.3V, GND)
SDA=15, SCL=16 on ESP32-S3; SDA=21, SCL=22 on classic ESP32
"""
from machine import I2C, Pin
from openbricks.drivers.tcs34725 import TCS34725
from openbricks.tools import wait
i2c = I2C(0, sda=Pin(15), scl=Pin(16), freq=400_000)
color = TCS34725(i2c, integration_ms=50, gain=4)
def classify(r, g, b):
"""Very rough color classifier. Good enough to tell red from green."""
if r > g and r > b:
return "red"
if g > r and g > b:
return "green"
if b > r and b > g:
return "blue"
if r > 200 and g > 200 and b > 200:
return "white"
if r < 40 and g < 40 and b < 40:
return "black"
return "unknown"
while True:
r, g, b = color.rgb()
print("rgb=({:3d},{:3d},{:3d}) ambient={:3d} -> {}".format(
r, g, b, color.ambient(), classify(r, g, b)
))
wait(200)
Colour sensor array via a TCA9548A (two TCS34725s)
Mode 2: the TCS34725’s address is fixed at 0x29, so two or more go
through a TCA9548A multiplexer, one per channel. mux[n] behaves
like an I2C bus, so the driver call is the same as above — only the
bus argument changes. Each loop combines ambient() and rgb() to
name the colour under every sensor.
# SPDX-License-Identifier: MIT
"""
Demo: name the colour under each of 2 TCS34725s through a TCA9548A mux.
The TCS34725's I2C address is fixed at 0x29, so two of them can't
share a bare bus — this is exactly the job the TCA9548A multiplexer
exists for. Each sensor hangs off its own mux channel, and ``mux[n]``
behaves like a normal I2C bus, so the driver is constructed unchanged.
Each loop reads **both signals** from every sensor and combines them:
* ``ambient()`` (0..100) — brightness. Separates black (too dark to
trust hue at all) and, together with the channel spread, white.
* ``rgb()`` (0..255 each, clear-normalised) — hue. Splits the
chromatic colours: red, yellow, green, blue.
``classify()`` reduces one sensor's (ambient, rgb) pair to one of
``red / blue / green / yellow / white / black``.
Hardware:
* ESP32-S3 (or classic ESP32)
* TCA9548A breakout on I2C bus 0 (3.3V, GND)
SDA=15, SCL=16 on ESP32-S3; SDA=21, SCL=22 on classic ESP32
* 2x TCS34725, one per mux channel 0..1, facing the surface
Calibration matters: TCS34725 readings shift with illumination LED,
distance, and surface gloss. The thresholds below are starting points
— print ``sensor.ambient()`` and ``sensor.rgb()`` over each of your
own surfaces and adjust.
"""
BLACK_AMBIENT = 12
NEUTRAL_SPREAD = 45
WHITE_AMBIENT = 35
YELLOW_G_OVER_R = 0.6
def classify(ambient, rgb):
"""Reduce one sensor's (ambient 0..100, (r, g, b) 0..255) reading
to ``"red" / "blue" / "green" / "yellow" / "white" / "black"``.
Decision order: black by darkness first (hue is noise down
there), then white/black by neutral spread + brightness, then the
chromatic colours by channel dominance.
"""
r, g, b = rgb
if ambient < BLACK_AMBIENT:
return "black"
spread = max(r, g, b) - min(r, g, b)
if spread < NEUTRAL_SPREAD:
return "white" if ambient >= WHITE_AMBIENT else "black"
if b >= r and b >= g:
return "blue"
if g >= r:
return "green"
if g >= r * YELLOW_G_OVER_R:
return "yellow"
return "red"
def main():
from machine import I2C, Pin
from openbricks.drivers.tca9548a import TCA9548A
from openbricks.drivers.tcs34725 import TCS34725
from openbricks.tools import wait
i2c = I2C(0, sda=Pin(15), scl=Pin(16), freq=400_000)
mux = TCA9548A(i2c)
sensors = [TCS34725(mux[ch]) for ch in range(2)]
print("Hold surfaces under both sensors (Ctrl-C to stop)...")
while True:
parts = []
for i, s in enumerate(sensors):
ambient = s.ambient()
rgb = s.rgb()
parts.append("s%d: ambient=%3d rgb=(%3d,%3d,%3d) -> %s" % (
i, ambient, rgb[0], rgb[1], rgb[2],
classify(ambient, rgb)))
print(" | ".join(parts))
wait(200)
if __name__ == "__main__":
main()
Line following (QTR sensor bar, center mode)
The QTRLineSensor’s LineMode.CENTER mode steers on the weighted centroid
of all ten elements, so edge_error() is proportional across the
whole 56 mm window. The same control law ships pinned to
LineMode.LEFT and LineMode.RIGHT in examples/qtr_line_follow_left.py /
_right.py — see Hardware guide for what each mode holds.
# SPDX-License-Identifier: MIT
"""Line following on the QTRLineSensor window — center mode.
Run ``examples/qtr_calibrate.py`` once first. Holds the line's
CENTRE under the middle of the window, steering on the weighted
centroid of all ten elements, so the error stays proportional
across the whole 56 mm span instead of railing a pitch away from
one setpoint element. Switch to ``examples/qtr_line_follow_left.py``
/ ``_right.py`` for the edge disciplines — or call
``qtr.set_mode(...)`` mid-run. Branches show on either outer band.
The whole window going dark ends the run.
"""
import time
from openbricks.drivers.qtr import QTRLineSensor
from openbricks.drivers.st3032 import ST3032Motor
from openbricks.robotics import DriveBase
# --- control law (pure logic, unit-tested in tests/test_qtr_line_follow.py) ---
from openbricks.parameters import LineMode
MODE = LineMode.CENTER
CRUISE_DPS = 200
KP = 5.0
MAX_DPS = 400
FLAG_COUNT = 3
def clamp(dps):
return max(0, min(MAX_DPS, int(dps)))
def get_wheel_speeds(reading):
if all(e.ambient() < 50 for e in reading.elements):
return None
steer = KP * reading.edge_error()
return (clamp(CRUISE_DPS + steer),
clamp(CRUISE_DPS - steer))
def branch_seen(reading, mode):
if mode == LineMode.LEFT:
flags = reading.elements[-FLAG_COUNT:]
elif mode == LineMode.RIGHT:
flags = reading.elements[:FLAG_COUNT]
else:
flags = reading.elements[:FLAG_COUNT] + reading.elements[-FLAG_COUNT:]
for e in flags:
if e.ambient() < 50:
return True
return False
# --- end control law ---
qtr = QTRLineSensor()
qtr.set_mode(MODE)
qtr.load_calibration("/qtr.cal")
left_motor = ST3032Motor(servo_id=2, uart_id=1, tx=14, rx=41,
invert=True)
right_motor = ST3032Motor(servo_id=1, uart_id=1, tx=14, rx=41)
db = DriveBase(left_motor, right_motor,
wheel_diameter_mm=88, axle_track_mm=136)
print("following (%s mode). Full-window dark stops the run." % MODE)
while True:
reading = qtr.read()
speeds = get_wheel_speeds(reading)
if speeds is None:
db.stop()
print("intersection - stopped")
break
if branch_seen(reading, MODE):
print("branch marker")
db.move_wheels(speeds[0], speeds[1])
time.sleep_ms(5)
Square up on a line (QTR sensor bar)
The classic align move on the QTRLineSensor window: each half of
the ten-element bar acts as one virtual corner sensor, in two
passes. Seek: drive slowly toward the line — the wheel whose half
reaches it first stops while the other keeps rolling, pivoting the
chassis square. Edge: servo each wheel proportionally — the
follower’s KP discipline — until its half reads ambient of about
50, the elements straddling the black/white boundary, parked
right ON the line’s edge.
Calibrate once with examples/qtr_calibrate.py first; mount the
bar ahead of the wheels.
# SPDX-License-Identifier: MIT
"""Square up on the edge of a perpendicular line, QTRLineSensor.
The classic FLL/WRO align move, on one sensor bar instead of two
corner sensors: each half of the window is one virtual corner
sensor, and each wheel servos proportionally until its half's mean
ambient reads 50 — the elements straddling the black/white
boundary. The wheel whose half arrives first holds while the other
pivots the chassis on; overshoot backs up. Both halves end ON the
edge, so the bar — and the chassis — is square right at it.
Run ``examples/qtr_calibrate.py`` once first. The bar must be
mounted ahead of the wheels; the farther ahead, the finer the final
heading.
"""
import time
from openbricks.drivers.qtr import QTRLineSensor
from openbricks.drivers.st3032 import ST3032Motor
from openbricks.robotics import DriveBase
from openbricks.parameters import Stop
# --- control law (pure logic, unit-tested in tests/test_qtr_align.py) ---
KP = 1.3
EDGE_TOLERANCE = 8
SIDE_COUNT = 5
def side_ambient(elements):
total = 0
for e in elements:
total += e.ambient()
return total // len(elements)
def edge_dps(elements, target):
error = side_ambient(elements) - target
if abs(error) <= EDGE_TOLERANCE:
return 0
return int(KP * error)
def edge_wheel_speeds(reading):
left = edge_dps(reading.elements[:SIDE_COUNT], 50)
right = edge_dps(reading.elements[-SIDE_COUNT:], 50)
if left == 0 and right == 0:
return None
return (left, right)
# --- end control law ---
qtr = QTRLineSensor()
qtr.load_calibration("/qtr.cal")
left_motor = ST3032Motor(servo_id=2, uart_id=1, tx=14, rx=41,
invert=True)
right_motor = ST3032Motor(servo_id=1, uart_id=1, tx=14, rx=41)
db = DriveBase(left_motor, right_motor,
wheel_diameter_mm=88, axle_track_mm=136)
print("aligning on the line ...")
while True:
speeds = edge_wheel_speeds(qtr.read())
if speeds is None:
break
db.move_wheels(speeds[0], speeds[1])
time.sleep_ms(5)
db.stop(then=Stop.BRAKE)
print("aligned - square on the edge")
Square up on a line (two color sensors)
The same maneuver with a corner-mounted color sensor per side, for rigs without the QTR bar.
# SPDX-License-Identifier: MIT
"""
Demo: square the robot up on a dark line using two colour sensors.
The classic FLL/WRO "align on a line" move: drive slowly toward a
black line with one colour sensor mounted near each front corner,
ahead of the wheels. The moment a sensor crosses onto the line its
wheel brakes — the other wheel keeps rolling, pivoting the chassis
until *its* sensor reaches the line too. When both have stopped, the
sensor pair (and therefore the chassis) is parallel to the line, no
matter how crooked the approach was.
Geometry matters: the sensors must sit ahead of the axle and be
mounted symmetrically. The farther apart they are, the more accurate
the final heading.
Line detection reuses the idea from ``color_array.py``: a black line
on a light mat is simply "too dark to be the mat" — ``ambient()``
below a threshold. Print ``sensor.ambient()`` over your own mat and
line and put ``LINE_AMBIENT`` between the two readings.
Hardware (same bus layout as ``color_array.py`` / ``full_robot.py``):
* ESP32-S3 (I2C on 15/16; serial bus UART on 14/6)
* 2x ST-3032 wheel servos, IDs 1 (left) / 2 (right)
* TCA9548A mux, one TCS34725 per channel: 0 = left, 1 = right,
both facing the mat at the front of the chassis
"""
from machine import I2C, Pin
from openbricks.tools import wait
from openbricks.drivers.st3032 import ST3032Motor
from openbricks.drivers.tca9548a import TCA9548A
from openbricks.drivers.tcs34725 import TCS34725
from openbricks.robotics import DriveBase
from openbricks.parameters import Stop
left_motor = ST3032Motor(servo_id=2, uart_id=1, tx=14, rx=41)
right_motor = ST3032Motor(servo_id=1, uart_id=1, tx=14, rx=41, invert=True)
db = DriveBase(left_motor, right_motor,
wheel_diameter_mm=88, axle_track_mm=136)
i2c = I2C(0, sda=Pin(15), scl=Pin(16), freq=400_000)
mux = TCA9548A(i2c)
left_sensor = TCS34725(mux[1])
right_sensor = TCS34725(mux[0])
LINE_AMBIENT = 20
def align_on_line():
approach_dps = 100
poll_ms = 10
timeout_ms = 8000
left_done = False
right_done = False
left_ambient = None
right_ambient = None
speeds = [approach_dps, approach_dps]
db.move_wheels(speeds[0], speeds[1])
try:
for _ in range(max(1, timeout_ms // poll_ms)):
if not left_done:
left_ambient = left_sensor.ambient()
if left_ambient < LINE_AMBIENT:
speeds[0] = 0
db.move_wheels(speeds[0], speeds[1])
left_done = True
print('left wheel stopped (ambient=%d)' % left_ambient)
if not right_done:
right_ambient = right_sensor.ambient()
if right_ambient < LINE_AMBIENT:
speeds[1] = 0
db.move_wheels(speeds[0], speeds[1])
right_done = True
print('right wheel stopped (ambient=%d)' % right_ambient)
if left_done and right_done:
return
wait(poll_ms)
finally:
db.stop(then=Stop.BRAKE)
raise RuntimeError(
"no line found within %d ms (last ambient: left=%r right=%r) — "
"is the line in reach, and is LINE_AMBIENT calibrated for "
"your mat?" % (timeout_ms, left_ambient, right_ambient))
def main():
print("aligning on the line ...")
align_on_line()
print("aligned — square to the line.")
wait(500)
left_motor.coast()
right_motor.coast()
if __name__ == "__main__":
main()
Full robot (ST-3032 drivebase + IMU + colour sensor + arm)
Everything from the reference build on one bus map — the wheels and the optional ST-3215 arm share the serial bus (IDs 1, 2 and 3), the IMU is on SPI, the colour sensor on I2C, and the QTR bank (GPIO 1-10) is left untouched.
# SPDX-License-Identifier: MIT
"""
Example: a small robot that rolls forward until its colour sensor sees red,
then demos a servo wave.
Hardware — the reference build from docs/hardware.md, wired to its
GPIO map so nothing collides with the QTR bank (GPIO 1-10):
* ESP32-S3 DevKitC-1
* 2x ST-3032 serial bus servos (wheel mode, IDs 1 and 2) on one
URT-2: GPIO 14 -> URT-2 RX, GPIO 41 -> URT-2 TX
* 1x ICM-45686 IMU on SPI: SCK 12, MOSI 13, MISO 11, CS 17
* 1x TCS34725 RGB colour sensor on I2C: SDA 15, SCL 16
* 1x ST-3215 serial bus servo (ID 3) daisy-chained on the SAME
bus as the wheels (optional — the servo demo at the bottom
just prints a message if it isn't attached)
"""
import time
from machine import I2C, Pin
from openbricks.drivers.icm45686 import ICM45686
from openbricks.drivers.st3032 import ST3032Motor
from openbricks.drivers.st3215 import ST3215
from openbricks.drivers.tcs34725 import TCS34725
from openbricks.robotics import DriveBase
I2C_SDA, I2C_SCL = 15, 16
SCK, MOSI, MISO, CS = 12, 13, 11, 17
UART_ID, TX, RX = 1, 14, 41
LEFT_ID, RIGHT_ID, ARM_ID = 1, 2, 3
WHEEL_DIAMETER_MM = 88
AXLE_TRACK_MM = 138
i2c = I2C(0, sda=Pin(I2C_SDA), scl=Pin(I2C_SCL), freq=400_000)
imu = ICM45686(sck=SCK, mosi=MOSI, miso=MISO, cs=CS)
color = TCS34725(i2c)
left = ST3032Motor(servo_id=LEFT_ID, uart_id=UART_ID, tx=TX, rx=RX,
invert=True)
right = ST3032Motor(servo_id=RIGHT_ID, uart_id=UART_ID, tx=TX, rx=RX)
drivebase = DriveBase(left, right,
wheel_diameter_mm=WHEEL_DIAMETER_MM,
axle_track_mm=AXLE_TRACK_MM)
try:
arm = ST3215(servo_id=ARM_ID, uart_id=UART_ID, tx=TX, rx=RX)
if not arm.ping():
raise OSError("servo %d did not answer ping" % ARM_ID)
except Exception as e:
print("no arm servo attached:", e)
arm = None
while True:
r, g, b = color.rgb()
heading = imu.heading()
print("rgb=({:3d},{:3d},{:3d}) heading={:6.1f}".format(r, g, b, heading))
if r > g and r > b and r > 120:
print("Red detected — stopping.")
drivebase.stop()
break
drivebase.drive(speed_mm_s=150, turn_rate_dps=0)
time.sleep_ms(50)
if arm is not None:
arm.move_to(180, speed=500)
time.sleep_ms(500)
arm.move_to(0, speed=500)