Swarm Robotics and Advanced Engineering Patterns

Welcome back, engineer — this is the big one!

Sparky waving We paired two robots over BLE in Chapter 12. Now we turn that pair into a real swarm — robots that avoid obstacles together, follow in a convoy, and even dance in sync. Along the way we'll pick up the software patterns real robotics engineers use every day: state machines, multithreading, and PID control. Then, in the second half of this chapter, we'll build a second kind of swarm — one where robots don't send each other messages at all. Instead, they read their own 9-DOF motion sensor, agree on a shared heading over WiFi, and steer to match it. Let's activate our superpower one more time!

Summary

This capstone chapter extends the BLE leader/follower pair from Chapter 12 into full swarm robotics: collective obstacle avoidance, convoy following, and synchronized dance routines, all organized with a state machine. Along the way, students learn the software patterns professional robotics teams rely on — project planning, multithreading, asynchronous programming, PID control, encoder feedback, and data logging. The chapter then introduces a second, independent path to swarm coordination: a 9-DOF inertial measurement unit (the L3GD20 gyroscope and LSM303DLHC accelerometer/magnetometer) that each robot calibrates and fuses into a stable heading estimate, broadcast over a WiFi access point using UDP so every follower in the swarm can steer to match it — no pairing, no per-robot connection, and no limit on how many robots can listen in.

Concepts Covered

This chapter covers the following 30 concepts from the learning graph:

Swarm Robotics
Emergent Behavior
Leader-Follower Pattern
BLE Leader Robot Code
BLE Follower Robot Code
Collective Obstacle Avoid
Swarm Algorithm Design
Distributed Systems
Convoy Following
Synchronized Swarm Dance
State Machine Pattern
Project Planning
Team Collaboration
Multithreading Basics
Asynchronous Programming
PID Control Overview
Encoder Motor Feedback
Data Logging
9-DOF IMU Overview
L3GD20 Gyroscope
LSM303DLHC Accelerometer Magnetometer
Gyroscope Calibration
Magnetometer Hard Iron Calibration
Complementary Filter Sensor Fusion
Heading Estimation
WiFi Access Point Host Mode
UDP Broadcast Networking
Heading Synchronization Swarm Pattern
UDP Master Broadcast Code
UDP Follower Steering Code

Prerequisites

This chapter builds on concepts from:

From Pairs to Swarms

In Chapter 12, one robot connected to another and sent it commands. That is communication between two robots. Swarm robotics is something different: it is the study of how many robots, each following simple local rules, produce useful behavior as a group — without any single robot knowing the whole plan.

Watch a flock of birds turn together, or a school of fish swerve around a predator. No bird is in charge. Each one just reacts to its nearest neighbors, and the group-level pattern — the turn, the swerve — appears on its own. Computer scientists call this emergent behavior: a group-level pattern that isn't written down anywhere in any single robot's code, but appears anyway from many robots following the same simple rules at the same time.

That's the engineering promise of swarm robotics. You don't program "form a convoy." You program "keep a fixed distance from the robot in front of you," give that rule to every robot, and the convoy emerges.

No robot sees the whole picture

Sparky thinking Here's the mind-bending part: in a real swarm, no single robot — and often no single human — has to know what the "big picture" behavior looks like. It just has to write one robot's local rule correctly. That's a very different kind of program than anything we've written so far in this course.

Extending Leader-Follower into Collective Behaviors

Chapter 12's BLE pairing already used the leader-follower pattern: one robot (the central) makes decisions and sends commands; the other (the peripheral) just executes them. That pattern doesn't stop at two robots. The same BLE leader robot code you wrote in Chapter 12 — scanning, connecting, writing to a characteristic — can address more than one peripheral, and the same BLE follower robot code — advertising, accepting a connection, executing received commands — runs unchanged on each follower. Each follower just needs a unique BLE name so the leader can tell them apart.

With more than two robots connected, several new group behaviors become possible:

Collective obstacle avoidance — every robot in the swarm shares what its own time-of-flight sensor sees. If one robot detects a wall, it broadcasts that to the others, and the whole group adjusts — not just the robot that "saw" the obstacle.
Swarm algorithm design — the general engineering discipline of writing the one local rule that, repeated across every robot, produces the group behavior you want. This is decomposition and pattern recognition (Chapter 1) applied to a multi-robot system.
Distributed systems — a broader computer science idea: a system made of multiple independent devices, none of which has the full picture, that still cooperates to get something done. A swarm of robots is a small, physical, easy-to-see example of the same idea that powers large computing systems like content delivery networks.
Convoy following — each follower keeps a target distance from the robot ahead of it, using its own time-of-flight sensor from Chapter 8. The lead robot drives; the convoy shape emerges from every follower running the same distance-keeping rule.
Synchronized swarm dance — building on the robot dance sequence from Chapter 10, the leader broadcasts a shared timing beat, and every robot performs its part of a choreographed routine on that beat.

The following table summarizes the four collective behaviors, all built from the same BLE leader/follower plumbing you already have:

Behavior	What the leader sends	What each follower does
Collective obstacle avoid	Distance reading from any robot that sees a wall	Adjusts path even if its own sensor sees nothing
Convoy following	Nothing extra — followers watch the robot ahead	Matches speed to hold a fixed following distance
Synchronized dance	A shared timing beat	Runs its part of a choreographed sequence on-beat
Heading synchronization (this chapter, Part 2)	A shared compass heading	Steers to match the broadcast heading

Diagram: Swarm Collective Behaviors

This simulation shows a group of small robots on a field. Each robot follows one simple local rule, and you can switch between rules to see convoy following, collective obstacle avoidance, and a leader-follower group appear on their own.

Run Swarm Collective Behaviors Fullscreen

Change one local rule for every robot and watch a convoy or swarm pattern emerge

Type: microsim sim-id: swarm-collective-behaviors
Library: p5.js
Status: Specified
Reuse: None — new design. It is inspired by boids-style flocking, but limited to the behaviors in this chapter.

Learning objective: Analyze (Bloom L4) — connect a single local rule and its settings to the group pattern it produces, and explain why no robot needs to know the whole plan.

Canvas layout: total width responsive (max 800 px), height 600 px. Top 480 px is the field. Bottom 120 px is the control strip.

Visual elements: - Field: light gray floor with a dark border. Scale 1 cm = 1.5 px, so the field is 480 cm x 320 cm. - Robots: 6 small triangles pointing in their heading. The leader is DarkOrchid (#9932CC) with a crown mark. Followers are OliveDrab (#6B8E23). Each has a faint circle showing its sensor range (default 60 cm). - Wall obstacle: one draggable gray bar. Robots that see it turn Crimson (#DC143C) for a moment (the AVOID state). - Lines: thin gray lines from each follower to the robot it is following (the nearest robot ahead), so the chain is visible. - Trails: 40-frame fading trails behind each robot. - Readouts: "Mode", "Average gap to robot ahead (cm)", "Gap error (cm)", "Robots in AVOID".

Interactive controls: - Dropdown "Behavior": "Convoy following" (default), "Collective obstacle avoidance", "Leader broadcast (all steer to leader)". Changing the dropdown swaps the same single rule for every robot. - Slider "Target gap (cm)": 15 to 80, default 30. Used in convoy mode. - Slider "Follower gain Kp": 0.005 to 0.1, default 0.03. Speed correction per cm of gap error. - Slider "Sensor range (cm)": 20 to 100, default 60. - Slider "Number of robots": 3 to 8, default 6. - Checkbox "Share wall alerts": when on, a robot that sees the wall tells all others, as in collective obstacle avoidance. Default off. - Buttons "Run / Pause", "Scatter robots", "Reset". The leader can be dragged with the mouse to steer it.

Behavior: robot speed limit is 30 cm/s. Convoy mode: each follower finds the nearest robot within its sensor range that is ahead of it. Its speed = 30 + Kp x (gap - target gap), limited to 0 to 30 cm/s. It turns toward that robot. If nothing is in range, it wanders slowly (state SEARCH, gray). The leader drives a slow loop around the field. Collective avoidance mode: every robot drives in a straight line and bounces off walls. A robot that measures distance below 20 cm to the wall turns 90 degrees. With "Share wall alerts" on, all robots within 200 cm of that robot also turn away at once. Leader broadcast mode: every follower steers with proportional control toward the leader's heading, turn = Kp x heading error. Display "no robot has the plan" text in a small note. The gap error readout = average of |gap - target gap|. Follower rules never use the leader's position except in leader broadcast mode.

Default state: Convoy following, 6 robots in a scattered line, gap 30 cm, Kp 0.03, paused.

Assessment/Challenge: In Convoy mode, raise Kp to 0.1 and watch the gap error. What do you see? Then lower it to 0.005. Which value keeps the convoy tight but calm? (Answer: high Kp makes the chain stretch and squeeze like an accordion. Low Kp is slow to catch up. A middle value near 0.03 works best.)

Responsive: redraw on window resize.

The follower rule in convoy mode is the one from the table: keep a fixed distance to the robot in front using the time-of-flight sensor. The sensor range slider matches how far the real sensor can see. Watch how one robot slowing down passes a wave back along the chain, even though no robot sends that message.

Organizing Multi-Behavior Code with a State Machine

A single robot in this swarm might need to switch between avoiding an obstacle, following a convoy leader, and dancing — sometimes within the same run. Writing one giant tangle of if statements to handle every combination gets unreadable fast. This is exactly the problem a state machine pattern solves.

A state machine describes a program as a small set of named states — like SEARCH, FOLLOW, and AVOID — plus the rules for transitioning from one state to another. At any moment, the robot is in exactly one state, and only that state's code runs. This mirrors a pattern you already know: the closed-loop feedback from Chapter 10 (sense, decide, act) is really just a state machine with one state. A swarm robot needs several.

Before the diagram below, here is the plain-language version: the robot starts in SEARCH, looking for the leader's signal. If it hears the leader, it moves to FOLLOW. If its distance sensor ever reports something too close, it moves to AVOID — no matter which state it was just in. Once the obstacle clears, it goes back to whatever it was doing.

Diagram: Swarm Robot State Machine

Run the Swarm Robot State Machine Fullscreen

Interactive state machine diagram for a swarm robot's behavior modes

Type: diagram sim-id: swarm-robot-state-machine
Library: Mermaid
Status: Specified

Diagram Name: Swarm Robot State Machine

Bloom Taxonomy: Understand

Bloom Taxonomy Verb: classify

Learning objective: Explain how a state machine organizes a robot's competing behaviors (search, follow, avoid) into a single clear structure, and how a transition can interrupt any state.

Create a Mermaid flowchart (graph TD) with four rounded-rectangle nodes: SEARCH, FOLLOW, AVOID, and DANCE. Directed edges: SEARCH -> FOLLOW labeled "leader signal found", FOLLOW -> SEARCH labeled "signal lost", FOLLOW -> DANCE labeled "dance beat received", DANCE -> FOLLOW labeled "routine finished". From every one of SEARCH, FOLLOW, and DANCE, draw a dashed edge to AVOID labeled "obstacle too close", and one dashed edge from AVOID back to FOLLOW labeled "path clear".

Every node has a click directive opening an infobox with a plain-language definition: SEARCH — "robot is scanning for the leader's advertising signal, motors idle." FOLLOW — "robot is connected to the leader and executing convoy or command logic." AVOID — "robot's own time-of-flight sensor reported an obstacle — this state can interrupt any other state." DANCE — "robot is executing a timed choreography step synced to the leader's beat." Every edge also has a click directive that shows the transition condition in plain language.

Color scheme: SEARCH gray, FOLLOW OliveDrab (matches the ROBOT taxonomy color), AVOID Crimson (matches MOTOR/warning color), DANCE MediumPurple. Canvas responsive to container width, minimum 700px wide before scaling down.

Notice that every state has a path to AVOID, but nothing else does. That's a deliberate design choice: safety behaviors should be able to interrupt anything. When you design your own state machine, ask which state must always be reachable, no matter what else is happening — that state gets the most incoming arrows.

Planning a Swarm Project as a Team

A four-robot swarm is bigger than any single-robot project in this course, and it is usually built by more than one person. Two engineering-process concepts become essential here, not optional.

Project planning means breaking the swarm build into ordered milestones before writing code — for example: (1) get one robot's state machine working alone, (2) get a two-robot BLE pair working, (3) add a third robot, (4) add the chosen collective behavior. Each milestone is testable on its own, so a bug shows up close to where it was introduced instead of buried in a four-robot tangle.

Team collaboration means dividing that plan across people with clear ownership — one student owns the state machine, another owns the BLE messaging, another owns the distance-keeping math — and agreeing on the interface between those pieces (what functions exist, what they're named, what they return) before anyone writes the internals. This is the same modular programming idea from Chapter 5, applied to people instead of just files.

Doing Two Things at Once: Multithreading and Asynchronous Programming

A swarm robot in the FOLLOW state has to do several things that all feel "at the same time": read its distance sensor, listen for new BLE messages, and update its motors. MicroPython runs your code one line at a time, so "at the same time" needs a real technique, not just optimism. This course covers two of them.

Multithreading runs a second, independent stream of instructions using the _thread module. The main program and the new thread genuinely run concurrently, each with its own call stack. Before the code below: _thread.start_new_thread() takes a function and a tuple of arguments, and starts that function running on its own thread immediately — the main program continues on to its next line without waiting.

import _thread
import time

def watch_distance_sensor():
    while True:
        distance = read_distance()   # from Chapter 8's ToF driver
        if distance < 15:
            avoid_flag.set()          # signal the main loop, don't call motors directly
        time.sleep(0.05)

_thread.start_new_thread(watch_distance_sensor, ())

Asynchronous programming solves the same "do several things at once" problem a different way: instead of a second real thread, uasyncio runs several tasks that take turns on a single thread, each one voluntarily pausing at an await. Before the code: async def marks a function as a task; await asyncio.sleep(...) is the pause point where this task lets another task run.

import uasyncio as asyncio

async def blink_status_led():
    while True:
        led.toggle()
        await asyncio.sleep(0.5)   # pause here — other tasks run during this pause

async def main():
    asyncio.create_task(blink_status_led())
    while True:
        check_ble_messages()
        await asyncio.sleep(0.02)

asyncio.run(main())

Both approaches let a robot appear to do several things "at once." The table below compares them now that both have been explained in prose.

	Multithreading (`_thread`)	Asynchronous (`uasyncio`)
How it shares CPU time	Real concurrent threads	One thread, cooperative task-switching
Where it pauses	Anywhere, unpredictably	Only at an explicit `await`
Risk of two tasks fighting over the same variable	Higher — needs care	Lower — tasks never interrupt mid-line
Typical use in this course	A sensor-reading loop that must never stall	Multiple lightweight tasks (blink, poll, log)

Diagram: Cooperative Multitasking Timeline

This simulation shows a timeline of what a robot's single thread does over time. You can compare a program that blocks with sleep() against one that shares time with await and see which one misses events.

Run Cooperative Multitasking Timeline Fullscreen

Compare blocking sleep() with cooperative await tasks on a shared timeline

Type: microsim sim-id: cooperative-multitasking-timeline
Library: p5.js
Status: Specified
Reuse: dmccreary/learning-micropython blocking-vs-nonblocking — https://github.com/dmccreary/learning-micropython/tree/main/docs/sims/blocking-vs-nonblocking. Keep its lane timeline and blocked/free coloring. Rename the tasks to the robot's three jobs (blink status LED, poll BLE, read distance sensor), and add the event-arrival markers and the missed-event counter.

Learning objective: Compare (Bloom L4) — explain how await asyncio.sleep() lets other tasks run while time.sleep() blocks them, and predict which events a blocking program will miss.

Canvas layout: total width responsive (max 800 px), height 560 px. Two timeline panels stacked, each 220 px tall, and a 100 px control strip. Time axis runs 0 to 4 seconds with a moving playhead.

Visual elements: - Panel A "Blocking (time.sleep)": one lane showing a single thread. Colors: blue for "blink LED work", green for "check BLE messages", orange for "read distance sensor", and a wide red-striped block for sleep() where the whole thread is stuck. - Panel B "Cooperative (uasyncio)": three lanes, one per task, plus a fourth "Event loop" lane. Each task shows short colored work blocks and thin dashed gray lines for the await pauses. Only one task block is active at a time (one thread), and an arrow shows the hand-off to the next task. - Event markers: white triangles under each panel for events that arrive from outside: "BLE message" (green) and "obstacle appears" (red). A marker that arrives while the thread is stuck in a sleep gets a red X with "missed or late". - Counters per panel: "Events handled on time", "Events late (ms)", "Thread idle time (%)". - Code snippet on the right of each panel: the 6 matching lines, with the running line highlighted.

Interactive controls: - Slider "Blink period (s)": 0.1 to 1.0, step 0.1, default 0.5 (the await asyncio.sleep(0.5) in the chapter). - Slider "BLE poll period (s)": 0.01 to 0.2, default 0.02 (the await asyncio.sleep(0.02) in the chapter). - Slider "Sensor read time (ms)": 5 to 100, default 20. - Button "Drop a BLE message" and button "Obstacle appears" (each puts an event at the current time). Button "Random events" (a new event every 0.3 to 1 s). - Button "Play / Pause", button "Reset". - Toggle "Use a long time.sleep(0.5)" (Panel A only). When on, the blink task uses a blocking half-second sleep. Default on.

Behavior: in Panel A the loop runs in order: blink, check BLE, read sensor, then time.sleep(blink period). During the sleep, no other work happens, so an event that arrives waits until the sleep ends. In Panel B, each task runs for its work time (blink 1 ms, BLE poll 1 ms, sensor read as set by the slider) and then awaits. While one task waits, the event loop runs another ready task. An event is handled when the matching task next runs, so the delay is at most the poll period. Lateness in ms is shown next to each X. Total work is the same on both panels, so the difference is only the idle sleeping. Show a note under Panel B: "One thread, but nobody sleeps while others wait." Also show a small warning if the sensor read time exceeds 50 ms: "A long task with no await still blocks everyone."

Default state: both panels paused at time 0, blink 0.5 s, BLE poll 0.02 s, sensor 20 ms, long sleep toggle on.

Assessment/Challenge: Press "Random events" and run for 10 seconds. How many events does Panel A handle late, and how many does Panel B? Then set Sensor read time to 100 ms. What happens to Panel B, and why? (Answer: Panel B is almost always on time, and A is often late by up to 0.5 s. A 100 ms sensor read with no await blocks the other tasks, so Panel B gets late events too.)

Responsive: redraw on window resize.

Panel A is the robot that stops listening every time it calls time.sleep(). Panel B is the main() function from the async example, where each task pauses at an await. This shows why the swarm robot can blink, poll BLE messages, and read its sensor without one job freezing the others.

Don't touch the motors from two places at once

Sparky warning A common bug: one thread reads the sensor and calls a motor function while the main loop is also calling a motor function, and the two calls interleave into garbage PWM values. The fix used above — the sensor thread only sets a flag, and the main loop is the only code that ever touches the motors — is the safest pattern for a first multithreaded program.

Smoother Control: PID and Encoder Feedback

Chapter 10's closed-loop feedback loop compared a sensor reading to a target and reacted —but it only reacted to how far off the robot currently was. PID control overview generalizes that idea into three separate reactions, added together:

[ \text{output} = K_p \cdot e + K_i \cdot \int e \, dt + K_d \cdot \frac{de}{dt} ]

Here ( e ) is the error — target minus current value. In plain language, not calculus:

Term	Reacts to	Plain-language effect
P (proportional)	How far off you are right now	Bigger error → bigger correction
I (integral)	How long you've stayed off	Corrects small, stubborn, lingering errors
D (derivative)	How fast the error is changing	Slows the correction down before it overshoots

Most of the closed-loop code in this course — including the collision-avoidance robot from Chapter 10 — only ever used the P term. That's called proportional-only control, and it's often good enough. You'll use exactly that P-only idea again later in this chapter, when a follower robot steers to match a broadcast heading.

Diagram: PID Feedback Loop Tuner

Run the PID Feedback Loop Tuner Fullscreen

PID Feedback Loop Tuner MicroSim

Type: microsim sim-id: pid-feedback-loop-tuner
Library: p5.js
Status: Specified
Template: https://github.com/dmccreary/control-systems/tree/main/docs/sims/feedback-loop-simulator

Learning objective: Apply (Bloom L3) — adjust Kp, Ki, and Kd independently and observe how each changes a simulated robot's approach to a target heading, including overshoot and settling time.

Canvas layout: - Left 500px: a strip-chart plot, target heading as a flat dashed line, actual heading as a solid animated line updating in real time - Right 200px: control panel

Visual elements: - Time-series plot, x-axis = time (seconds), y-axis = heading error in degrees - A single simulated follower robot's heading value updated each frame using the PID formula against a step-change target

Interactive controls: - Slider: Kp (0 to 1.0, default 0.2) - Slider: Ki (0 to 0.2, default 0.0) - Slider: Kd (0 to 0.5, default 0.0) - Button: "Step Target" — jumps the target heading by 90 degrees - Button: "Reset"

Default parameters: Kp=0.2, Ki=0.0, Kd=0.0, so the sim starts in the same proportional-only mode already used elsewhere in this chapter.

Behavior: raising Kp alone should visibly speed up the approach but eventually cause oscillation around the target; adding Kd should visibly damp that oscillation; adding Ki should visibly eliminate any small steady-state offset left over. Label the current Kp/Ki/Kd values and the live error value numerically next to the sliders so the connection between slider position and plotted behavior stays visible (Data Visibility Requirement).

Instructional Rationale: this is an Apply-level objective, so the sim uses direct parameter manipulation with an immediately visible, labeled numeric readout rather than a passive animation — the learner must be able to connect a specific slider position to a specific change in the plotted curve.

Implementation notes: adapt the referenced template's proportional-gain step-response simulator to this course's heading/degrees framing instead of a generic plant/setpoint framing, and add the Ki/Kd sliders and plot terms the template does not yet include.

Closed-loop control gets more precise when the feedback signal itself is more precise. Encoder motor feedback adds a small sensor — often a slotted wheel and an optical or magnetic sensor — that counts wheel rotations directly, instead of only inferring motion from motor commands. If you add an encoder later, the wiring reuses the interrupt pattern from Chapter 6:

import machine

encoder_ticks = 0

def count_tick(pin):
    global encoder_ticks
    encoder_ticks += 1   # one tick = one fixed distance traveled

encoder_pin = machine.Pin(21, machine.Pin.IN, machine.Pin.PULL_UP)
encoder_pin.irq(trigger=machine.Pin.IRQ_FALLING, handler=count_tick)

Feeding encoder_ticks into a PID loop as the feedback signal — instead of a raw distance sensor reading — is how professional robots achieve precise, repeatable convoy-following distances.

Recording What Happened: Data Logging

Tuning Kp by eye on a moving robot is hard. Data logging — writing sensor readings and decisions to a file as the robot runs — lets you review a run afterward instead of watching it live.

log_file = open("heading_log.csv", "a")

def log_heading(t, target, actual):
    row = f"{t},{target},{actual}\n"
    log_file.write(row)
    log_file.flush()   # write to flash now, don't wait for a possible crash

Later, you can open heading_log.csv on your laptop and plot target vs. actual heading over time — the exact same step-response curve the PID tuner MicroSim above simulates, but from your own robot's real run.

Advanced patterns take practice — that's expected

Sparky encouraging State machines, threads, async tasks, and PID math are genuinely more advanced than anything earlier in this course. Professional robotics engineers spend years getting comfortable with these ideas. You don't need to master them today — you need to recognize the shape of the problem each one solves, so you know which tool to reach for later.

A Second Path to Swarm Coordination

Every swarm behavior so far depends on BLE: each robot pairs with, or listens for messages from, another specific robot. That works well for a handful of robots, but BLE connections are one-to-one — a leader has to connect to each follower separately, and Chapter 12 already noted that reliability drops with distance and interference.

The Swarm Robotics Cluster design report explores a different idea: instead of tracking another robot's position, or waiting for a paired connection, what if every robot just matched a shared heading — the compass direction it's currently facing? A master robot broadcasts its heading over WiFi. Every follower reads that broadcast, compares it to its own heading, and steers to close the gap. No pairing. No per-follower connection. Any robot within WiFi range can listen in, at no extra cost to the master.

Matching a heading needs a robot that can measure its own heading precisely, which is a harder sensing problem than anything earlier in this course.

Meet the 9-DOF IMU: L3GD20 + LSM303DLHC

A 9-DOF IMU overview: "9-DOF" stands for nine degrees of freedom — three axes each from a gyroscope, an accelerometer, and a magnetometer. A gyroscope measures rotation rate (how fast the robot is turning, in degrees per second). An accelerometer measures linear acceleration (including gravity, which is how it senses "which way is down"). A magnetometer measures the local magnetic field — essentially, a built-in compass. Combined, these nine numbers are enough to estimate which way a robot is facing and how it's moving, far more precisely than the single time-of-flight sensor from Chapter 8.

The module used in this course packs these onto two separate chips on one small board: the L3GD20 gyroscope and the LSM303DLHC accelerometer/magnetometer. Both sit on the same I2C bus from Chapter 6. The gyroscope answers at address 0x6B. The LSM303DLHC answers at two addresses — 0x19 for its accelerometer and 0x1E for its magnetometer — because inside, it acts like two separate devices. The board also carries a bonus BMP180 temperature and pressure chip at 0x77 that we don't use. So a scan finds four addresses, and reading the module means writing two small drivers, one per chip, not one.

Diagram: 9-DOF IMU Chip Layout

Run the 9-DOF IMU Chip Layout Diagram Fullscreen

9-DOF IMU chip layout on a shared I2C bus

Type: diagram sim-id: imu-chip-layout-diagram
Library: Mermaid
Status: Specified

Diagram Name: 9-DOF IMU Chip Layout

Bloom Taxonomy: Understand

Bloom Taxonomy Verb: explain

Learning objective: Explain that a "9-DOF IMU module" is really two separate I2C sensor chips sharing one bus, each answering at its own address or addresses, rather than one combined chip.

Create a Mermaid flowchart (graph LR). Node "Pico W GPIO16/17 (I2C0)" connects with labeled edges to two chip nodes: "L3GD20 Gyroscope (addr 0x6B)" and "LSM303DLHC Accel + Mag (0x19 + 0x1E)", plus a dashed edge to a muted "BMP180 bonus chip (not used)" node labeled "addr 0x77". Each sensor chip node has a smaller child node beneath it: L3GD20 connects down to "3-axis rotation rate (deg/s)"; LSM303DLHC connects down to two children, "3-axis acceleration (g)" on an edge labeled "0x19" and "3-axis magnetic field (gauss)" on an edge labeled "0x1E".

Every node has a click directive with an infobox: the I2C bus node explains "one shared SDA/SCL pair, same as the ToF sensor and OLED display from earlier chapters — I2C allows multiple devices as long as addresses differ." The gyroscope node explains what a gyroscope measures and that it drifts slowly over time. The accel/mag node explains that acceleration senses gravity/tilt and magnetic field acts as a compass, and that motors nearby distort the magnetic reading. The three data-type leaf nodes each explain their unit and typical use.

Color scheme: I2C bus node DodgerBlue (SENSOR taxonomy color), chip nodes white with black outline, data leaf nodes light gray. Canvas responsive to container width.

Reading either chip means talking to its registers over I2C — the same readfrom_mem pattern you used for the time-of-flight sensor in Chapter 8, just with different register addresses. Before the code: WHO_AM_I is a fixed register that many ST sensor chips have, including the L3GD20 gyroscope. Reading it back confirms you're actually talking to the chip you think you are, before trusting any of its data.

import machine
import config

# SoftI2C, not machine.I2C: on this IMU board the hardware I2C block
# could scan but failed on real reads (see the 9-DOF IMU Kit)
i2c = machine.SoftI2C(sda=machine.Pin(config.I2C_SDA_PIN),
                      scl=machine.Pin(config.I2C_SCL_PIN), freq=100000)
found = i2c.scan()
print("Devices found:", [hex(d) for d in found])   # expect 0x19, 0x1e, 0x6b, 0x77

gyro_id = i2c.readfrom_mem(0x6B, 0x0F, 1)   # WHO_AM_I register
print("Gyro chip ID:", hex(gyro_id[0]))     # expect 0xD4 or 0xD7

This code scans the bus, prints every address it finds, and then asks the gyroscope for its ID. The full driver code for both chips — with all the register constants — is in the shared l3gd20.py and lsm303dlhc.py drivers, tested on this exact module in the 9-DOF IMU Kit.

Calibrating the Gyroscope and Magnetometer

Raw sensor numbers are rarely usable straight out of the box. Two calibration steps matter most for a heading estimate.

Gyroscope calibration starts simple: with the robot sitting perfectly still, the gyroscope should report exactly 0 degrees per second on every axis. In practice it reports a small, steady non-zero number — its bias. You measure that bias once, at startup, by averaging a few hundred readings while the robot doesn't move, and subtract it from every later reading.

Magnetometer hard-iron calibration is trickier, because the error isn't a simple number — it's an offset in two dimensions. Nearby metal and magnets (including the robot's own DC motors) shift every magnetometer reading by a fixed amount in the X and Y directions, called hard-iron distortion. Left uncorrected, the compass heading it computes will be consistently wrong by some fixed angle, no matter which way the robot actually faces.

Diagram: Magnetometer Calibration Explorer

Run the Magnetometer Calibration Explorer Fullscreen

Magnetometer Hard-Iron Calibration Explorer MicroSim

Type: microsim sim-id: magnetometer-calibration-explorer
Library: p5.js
Status: Specified

Learning objective: Apply (Bloom L3) — rotate a simulated magnetometer through a full turn, watch the raw X/Y readings trace an off-center circle, then compute and apply the hard-iron offset to re-center it.

Canvas layout: - Left 450px: an X/Y scatter plot (magnetometer X on horizontal axis, Y on vertical axis) with grid lines and a marked origin (0,0) - Right 250px: control panel and numeric readouts

Visual elements: - A scatter trail of small dots plotted as the simulated robot "rotates" — dots accumulate to trace a circle - The circle starts deliberately off-center (simulating hard-iron distortion) with a visible offset from the origin - A large crosshair marking the circle's actual center, computed live from (max+min)/2 on each axis - After calibration is applied, a second, overlaid circle in a different color shows the corrected, origin-centered trace

Interactive controls: - Slider: "Rotate robot" (0 to 360 degrees) — dragging it plots one new raw (X, Y) point per few degrees, simulating slow hand-rotation - Button: "Auto-rotate" — animates the slider through a full 360-degree sweep automatically - Button: "Compute Calibration" — enabled once at least one full rotation of points exists; computes offset_x = (max_x + min_x) / 2 and offset_y = (max_y + min_y) / 2, draws the crosshair, and overlays the corrected circle - Button: "Reset" - Numeric readout: live offset_x and offset_y values, updating the moment "Compute Calibration" is pressed

Default parameters: simulated true hard-iron offset of (35, -20) in raw sensor units, simulated circle radius 200, small random sensor noise added to each plotted point.

Behavior: the raw circle never passes through the origin until "Compute Calibration" is pressed; after that, the corrected overlay circle is visibly centered on the origin, making the effect of the offset subtraction immediately visible rather than abstract.

Instructional Rationale: this is an Apply-level objective, so the learner performs the actual calibration procedure — rotate, then compute an offset from concrete min/max values — on a simulated sensor before doing it on real hardware, with the before/after circles making the correction's effect directly observable rather than described only in words.

Implementation notes: use p5.js; store raw points in an array; compute min/max per axis incrementally as points are added; responsive canvas that maintains aspect ratio on window resize.

Recalibrate after you remount the sensor

Sparky warning A hard-iron offset depends on exactly where the IMU sits relative to the robot's motors and battery. Move the sensor, and the offset changes. This is the single most common reason a heading-following demo suddenly stops working after a robot gets reassembled — always recalibrate after remounting.

Fusing Sensors: The Complementary Filter and Heading Estimation

Neither sensor alone gives a good heading. The gyroscope is smooth and fast, but its small bias adds up over time into slow drift — a robot sitting perfectly still will report a heading that slowly creeps away from the truth. The calibrated magnetometer doesn't drift, but on its own it's noisy, reading-to-reading. This is the same tradeoff the general sensor fusion idea from Chapter 8 described for combining time-of-flight, ultrasonic, and infrared readings — just applied to a new pair of sensors.

A complementary filter sensor fusion approach blends the two: mostly trust the gyroscope from one instant to the next (it's smooth), but let the magnetometer slowly correct any accumulated drift (it doesn't drift). One tunable number, ( \alpha ) (alpha), controls the blend:

[ \text{heading} = \alpha \cdot (\text{heading}{prev} + \dot\theta \cdot \Delta t) + (1 - \alpha) \cdot \text{heading} ]

Here ( \dot\theta ) is the gyroscope's rotation rate and ( \Delta t ) is the time since the last update. An ( \alpha ) near 1.0 trusts the gyroscope almost completely moment-to-moment; a lower ( \alpha ) leans more on the (calibrated) compass. The result of running this filter every loop iteration is heading estimation — a single, stable number in degrees that the rest of the swarm code can rely on.

import math

class HeadingFilter:
    def __init__(self, alpha=0.98):
        self.alpha = alpha
        self.heading = 0.0

    def update(self, gyro_z_dps, mag_x, mag_y, dt):
        gyro_estimate = self.heading + gyro_z_dps * dt
        compass_estimate = math.degrees(math.atan2(mag_y, mag_x)) % 360
        # How far the compass is from the gyro, the short way around (-180 to +180)
        diff = ((compass_estimate - gyro_estimate + 180) % 360) - 180
        # Move a small step, (1 - alpha), of the way toward the compass
        self.heading = (gyro_estimate + (1 - self.alpha) * diff) % 360
        return self.heading

Why the diff line? Headings wrap around like a clock face: 359° and 1° are only 2° apart, and both point almost exactly north. If we plugged those two raw numbers into the formula above, we would get about 352° — pointing the wrong way! So the code first finds how far the compass is from the gyro the short way around the circle. That gives a number from −180 to +180 (here, +2). Then it moves a small step toward the compass. It is the same blend as the formula, just measured the short way, so it works at every heading.

Diagram: Complementary Filter Heading Tuner

Run the Complementary Filter Heading Tuner Fullscreen

Complementary Filter Heading Tuner MicroSim

Type: microsim sim-id: complementary-filter-heading-tuner
Library: p5.js
Status: Specified

Learning objective: Analyze (Bloom L4) — compare a gyro-only heading estimate, a magnetometer-only heading estimate, and the fused complementary-filter estimate against a simulated true heading, and connect the alpha slider position to which sensor dominates the fused result.

Canvas layout: - Left 450px: a compass-rose style circular dial showing four needles in different colors (true heading, gyro-only, mag-only, fused) rotating in real time - Right 250px: controls and a small numeric error readout for each of the three estimates versus the true heading

Visual elements: - Circular compass dial with degree tick marks - Four colored needles: true heading (black, the reference), gyro-only estimate (orange, visibly drifting away over time), mag-only estimate (green, visibly jittering/noisy), fused estimate (blue, tracking the true heading closely) - A live numeric table below the dial showing current error in degrees for gyro-only, mag-only, and fused

Interactive controls: - Slider: alpha (0.80 to 0.999, default 0.98) - Slider: simulated gyro bias / drift rate (0 to 2 degrees/second, default 0.5) - Slider: simulated magnetometer noise (0 to 15 degrees, default 5) - Button: "Start Turn" — commands the simulated true heading to rotate 90 degrees over 2 seconds, then hold - Button: "Reset"

Default parameters: alpha=0.98, gyro drift=0.5 deg/s, mag noise=5 degrees.

Behavior: with alpha near 0.999, the blue fused needle should visibly drift away from true heading (like the orange gyro-only needle) — demonstrating alpha too high ignores the compass correction. With alpha near 0.80, the blue needle should visibly jitter (like the green mag-only needle) — demonstrating alpha too low ignores gyro smoothing. Near the default 0.98, the fused needle should track the true heading more closely than either individual estimate, with the numeric error table making "closely" concrete rather than just visual.

Instructional Rationale: this is an Analyze-level objective — the learner must compare three simultaneous estimates against ground truth and attribute the fused result's behavior to the alpha parameter, so all three needles plus a live true-heading reference and numeric error readout must be visible at once, not stepped through one at a time.

Implementation notes: use p5.js; simulate true heading as a controllable state; derive gyro-only estimate by integrating (true rate + configured bias) with no correction; derive mag-only estimate as true heading plus random noise scaled by the noise slider; derive fused estimate using the exact HeadingFilter update formula shown in the surrounding chapter text so the sim matches the code. Responsive canvas.

Hosting the Swarm Network: WiFi Access Point and UDP Broadcast

Chapter 11's web server connected a Pico W to an existing WiFi network as a station. WiFi access point host mode is the opposite: the Pico W creates its own network, becomes the access point other devices join, with no router involved at all. The master robot in a heading-synchronized swarm hosts its own access point, and every follower joins it.

Once robots share a network, UDP broadcast networking is how the master reaches all of them at once without addressing each one individually. Unlike the TCP sockets from Chapter 11's web server — which require a connected, one-to-one link — a UDP broadcast packet is sent once to a special broadcast address, and every device on the network receives a copy. If a packet is dropped, nothing breaks; the next one arrives a fraction of a second later.

Diagram: Heading Broadcast Network Topology

Run the Heading Broadcast Network Topology Fullscreen

One master robot broadcasting UDP heading packets to many followers

Type: diagram sim-id: heading-broadcast-topology
Library: Mermaid
Status: Specified

Diagram Name: Heading Broadcast Network Topology

Bloom Taxonomy: Analyze

Bloom Taxonomy Verb: differentiate

Learning objective: Differentiate a one-to-many UDP broadcast topology (this section) from the one-to-one BLE pairing topology used earlier in the chapter, and from the router-based WiFi topology from Chapter 11.

Create a Mermaid flowchart (graph TD). A single node "Master Robot (hosts WiFi AP + broadcasts UDP)" connects with three identically-labeled edges, each labeled "UDP heading packet", to three follower nodes: "Follower 1", "Follower 2", "Follower 3". Add a dashed box around all four nodes labeled "Same WiFi network, master-hosted — no internet router".

Every node has a click directive with an infobox: the master node explains it both hosts the access point and sends the broadcast, unlike Chapter 11's setup where the Pico W joined someone else's router. Each follower node explains it only listens — it never sends anything back, and a dropped packet is not a failure since the next one is a fraction of a second away. Every edge has a click directive noting that this same edge is physically identical to the other two — this is what makes adding a fourth follower free of any code change on the master.

Color scheme: master node DarkOrchid (NET taxonomy color), follower nodes lighter shade of the same hue, dashed boundary box gray. Canvas responsive.

Compare this to Chapter 12's BLE leader, which had to scan for and connect to each follower by name, one at a time. A UDP broadcast doesn't know or care how many followers are listening — the master's code doesn't change at all when you add a fourth or fifth robot. That's the real engineering payoff of this pattern.

Heading Synchronization: Master and Follower Code

Put together, the IMU heading estimate and the UDP broadcast form a complete second swarm pattern — the heading synchronization swarm pattern: a master robot computes its own fused heading and broadcasts it; every follower computes its own fused heading from its own sensors, and steers to close the gap between the two.

UDP master broadcast code hosts the access point and sends the heading on a timer:

import network, socket, json, time

def start_ap(ssid, password):
    ap = network.WLAN(network.AP_IF)
    ap.config(ssid=ssid, password=password)
    ap.active(True)
    while not ap.active():
        time.sleep(0.5)
    return ap

def broadcast_loop(get_heading, port=8000):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
    while True:
        payload = json.dumps({"heading": get_heading()})
        sock.sendto(payload.encode(), ("192.168.4.255", port))   # AP broadcast address
        time.sleep(0.02)   # 50 Hz

UDP follower steering code joins that network, listens for the heading, and steers using the same proportional-only control introduced earlier in this chapter:

def heading_error(target, current):
    return (target - current + 180) % 360 - 180   # shortest turn direction, -180..+180

def steer(error, base_speed, Kp=0.02):
    turn = Kp * error
    left = max(0, min(1, base_speed + turn))
    right = max(0, min(1, base_speed - turn))
    return left, right

Diagram: Heading Error and Steering Explorer

This simulation shows a follower robot from above with a broadcast target heading. It draws the two possible turns and shows why heading_error() always picks the shorter one, then shows how steer() turns that error into left and right motor speeds.

Run Heading Error and Steering Explorer Fullscreen

Set a current and target heading and see the wrap-around error and the motor speeds from steer()

Type: microsim sim-id: heading-error-steering-explorer
Library: p5.js
Status: Specified
Reuse: None — new design. It does not repeat complementary-filter-heading-tuner, which is about estimating the heading. This sim is about using the heading to steer.

Learning objective: Apply (Bloom L3) — calculate the shortest-turn heading error with wrap-around, and predict the left and right motor speeds that steer() returns for a given error and Kp.

Canvas layout: total width responsive (max 800 px), height 560 px. Left 380 px is a compass dial. Right 420 px has the calculation panel (top 300 px) and the motor bars (bottom 260 px).

Visual elements: - Compass dial: a circle with N (0), E (90), S (180), and W (270) marks. A green arrow shows the follower's current heading. A purple arrow shows the target heading from the master's broadcast. A small robot icon sits in the center, rotated to the current heading. - Turn arcs: a solid arc for the shorter turn (blue) and a dashed gray arc for the long way around. The arc label shows the degrees, for example "+30" and "-330". - Calculation panel: shows each step with live numbers: target - current, + 180, % 360, - 180, and the final error. For example current 350, target 20: 20 - 350 = -330, + 180 = -150, % 360 = 210, - 180 = 30, so error = +30 (turn right). A plain sentence says "Turn right 30 degrees" or "Turn left 30 degrees". - Naive comparison: a gray line "Without wrap-around: target - current = -330, the robot would turn the long way". It is red when it differs from the correct error. - Motor bars: two vertical bars for the left and right motors, each 0 to 1 (0 to 65535 duty on the robot). Bars are green, and clip to gray at the ends 0 and 1. The turn amount Kp x error is shown. - A simple top-down replay: when Play is pressed, the robot turns using the shown motor speeds until the error is under 2 degrees.

Interactive controls: - Slider "Current heading (degrees)": 0 to 359, default 350. The green arrow can also be dragged. - Slider "Target heading (degrees)": 0 to 359, default 20. The purple arrow can also be dragged. - Slider "Base speed": 0 to 1.0, step 0.05, default 0.5. - Slider "Kp": 0.005 to 0.1, step 0.005, default 0.02. - Checkbox "Show naive error (no wrap-around)", default on. - Buttons "Play turn" and "Reset".

Behavior: error = (target - current + 180) % 360 - 180, with the result from -180 to +180 (use a true mathematical modulo so negative values wrap). turn = Kp x error. left = clamp(base + turn, 0, 1). right = clamp(base - turn, 0, 1). A positive error means turn right, so the left motor is faster. Example with defaults: error +30, turn 0.6, left = clamp(1.1) = 1.0, right = clamp(-0.1) = 0.0. The bars show the clipping, and a note explains "Kp is too high: the robot pivots hard". In Play mode, each 50 ms tick changes the heading by (left - right) x 20 degrees and recalculates the error. With Kp above 0.05 and a 20 Hz update, the replay overshoots and swings back and forth, which matches the follower oscillation symptom in the checklist. If the error is exactly 180, show "Either way is the same length".

Default state: current 350, target 20, base speed 0.5, Kp 0.02, naive error shown. The correct error is +30 and the naive error is -330.

Assessment/Challenge: Set current to 10 and target to 350. What is the error, and does the robot turn left or right? (Answer: -20, so it turns left through north, not 340 degrees right.) Then raise Kp until the replay oscillates. About what value does that happen? (Answer: around 0.05 to 0.06.)

Responsive: redraw on window resize.

The compass dial shows why we need the % 360 line: headings wrap from 359 back to 0. The motor bars are the values steer() returns before they go to the PWM pins in config.py. A robot that swings back and forth is the sign that Kp is too high, as in the checklist below.

The full master and follower scripts — WiFi joining, the non-blocking receive loop, and wiring steer()'s output into the motor pins from config.py — are written out step by step, phase by phase, in the Swarm Robot Build Plan, matched to the exact Cytron ROBO-PICO and 9-DOF IMU hardware this course uses.

Before trying it on real robots, this checklist — pulled from that build plan — heads off the most common problems:

Symptom	Likely cause
I2C scan is missing an address (expect `0x19`, `0x1e`, `0x6b`, `0x77`)	One chip's wiring or solder joint failed — check the side that's missing
Heading drifts while the robot sits still	Re-run magnetometer calibration
Heading jumps only while driving	IMU mounted too close to a motor — add a standoff, recalibrate
Follower never turns	Confirm it joined the master's access point, and that both sides use the same UDP port
Follower steering oscillates back and forth	`Kp` is too high — this is exactly the overshoot behavior the PID tuner MicroSim showed earlier in this chapter

Key Takeaways

Swarm robotics studies how simple per-robot rules produce emergent behavior at the group level, with no single robot holding the whole plan
The leader-follower pattern from Chapter 12 extends into collective obstacle avoidance, convoy following, and synchronized dance — all distributed systems running the same swarm algorithm design idea
A state machine pattern organizes a robot's competing behaviors into named states with clear transitions, instead of tangled if statements
Real swarm projects need project planning and team collaboration, just like professional engineering teams
Multithreading and asynchronous programming are two different techniques for doing several things "at once" in MicroPython
PID control generalizes closed-loop feedback into proportional, integral, and derivative terms; encoder motor feedback gives it a more precise input signal; data logging lets you review a tuning run after the fact
A 9-DOF IMU — here, the L3GD20 gyroscope and LSM303DLHC accelerometer/ magnetometer — needs gyroscope calibration and magnetometer hard-iron calibration before its readings mean anything
A complementary filter fuses gyro and compass into a stable heading estimate
WiFi access point host mode plus UDP broadcast networking let one master reach any number of followers with no per-robot connection — the network layer underneath the heading synchronization swarm pattern

You just built two different kinds of swarm — that's real engineering!

Sparky celebrating Look at everything you can do now: pair robots over BLE, organize their behavior with a state machine, tune a PID controller, and build a completely independent WiFi-based swarm from a 9-DOF sensor you calibrated yourself. That's not "following a robotics tutorial" anymore — that's engineering judgment. Computational thinking is YOUR superpower, and you've just proven it on hardware. Congratulations, engineer!