Robot Software Architecture

Programming for Robotics

Course 1 · Chapter 1 · Robot Software Architecture

Robotics Fundamentals drew the sense-think-act loop as a picture and wrote it as pseudocode (Robotics Fundamentals 1 section 2, and the top-level loop of Robotics Fundamentals 10 section 9.7). This course turns the pictures into programs that really run. This first chapter builds the skeleton that every later chapter hangs its code on: a loop that runs at a steady rate, code that never freezes while it waits, a clear split into sensors, estimator, controller and actuators, a hardware layer that can be swapped for a fake or a simulator, constants with their units in their names, a folder layout, logging, and error handling that leaves the robot safe when something goes wrong. None of it needs a robot: everything here runs on an ordinary PC with Python 3, and the same structure carries over to a board.

What was run, and what was not
Every Python block in this chapter that has an output below it was run with CPython 3, and the output shown is what it printed. The blocks use fake clocks and fake hardware, so the numbers are exact and repeatable; on a real machine, timing numbers will differ. The MicroPython boxes (and the Raspberry Pi Pico sketch in section 7) are written from memory of the MicroPython documentation and have not been run on hardware: check each name against the documentation for your port and firmware version. The two small blocks that show bad code are labelled as illustrations and are not run.

The blocks build on each other, like one growing file: later blocks use names that earlier blocks define. If you type them in, put them in one file (the layout of section 9 shows how the file splits into modules), or run them in one interactive session.

1. The sense-think-act loop in real code

A robot is a loop closed through the world: the program reads sensors, decides, drives the actuators, the world changes, and the next reading shows the change (Robotics Fundamentals 1 section 2). Here is the smallest program that really is that loop. The “world” is one line: a wall 1.00 m ahead and a robot that moves by speed × dt every pass. The robot slows as it nears a stop line 0.25 m from the wall, which is a proportional controller (Robotics Fundamentals 7 section 3) with a speed limit.

wall_m = 1.0 # the wall is 1.00 m from where the robot starts x_m = 0.0 # how far the robot has driven dt_s = 0.02 # 50 Hz: one pass every 20 ms for step in range(151): range_m = wall_m - x_m # SENSE: how far away is the wall? speed_mps = 1.5 * (range_m - 0.25) # THINK: slow down as the 0.25 m stop line nears speed_mps = max(0.0, min(0.3, speed_mps)) # never faster than 0.3 m/s, never backwards x_m += speed_mps * dt_s # ACT: the wheels turn, and the world changes if step % 25 == 0: print("step %3d t=%.2f s range=%.3f m speed=%.3f m/s" % (step, step * dt_s, range_m, speed_mps))
step 0 t=0.00 s range=1.000 m speed=0.300 m/s step 25 t=0.50 s range=0.850 m speed=0.300 m/s step 50 t=1.00 s range=0.700 m speed=0.300 m/s step 75 t=1.50 s range=0.550 m speed=0.300 m/s step 100 t=2.00 s range=0.405 m speed=0.233 m/s step 125 t=2.50 s range=0.322 m speed=0.109 m/s step 150 t=3.00 s range=0.284 m speed=0.051 m/s

It works, and it is also the kind of program that stops working as soon as it grows: the loop has no rate of its own (it runs as fast as the PC allows, and the dt_s is a hope, not a measurement), the sensor, the decision and the motors are tangled in four lines, and there is no way to test it without the real world. The rest of the chapter fixes each of those, one at a time.

2. Timing: period, dt and jitter

A control loop (Robotics Fundamentals 7 section 6 wrote the discrete PID) assumes that the passes are evenly spaced: the integral adds Ki e dt, the derivative divides by dt, the odometry adds v dt. If the real spacing wanders, every one of those is wrong by the same wander. Three words describe the timing:

  • Period T, the time you want between the starts of two passes, and the rate f = 1 / T.
  • dt, the time that actually passed since the previous pass began. The code should measure it, not assume it equals T.
  • Jitter, how much dt varies from pass to pass, summarised as the largest deviation max |dt − T| and as the standard deviation of the dt values.
Rate and period: f = 1 / T T = 1 / f 50 Hz -> T = 20 ms 100 Hz -> T = 10 ms Sample of n measured values dt(1) ... dt(n): mean dt(mean) = ( dt(1) + ... + dt(n) ) / n std dev s = sqrt( sum( (dt(i) - dt(mean))^2 ) / (n - 1) ) (the sample form, dividing by n - 1) jitter J(max) = max( |dt(i) - T| ) the effective rate is 1 / dt(mean), not 1 / T Worked example: a loop asked for T = 20 ms measured ten passes (ms): 20.1 19.9 20.3 20.0 33.4 19.8 20.2 19.9 20.4 20.0 sum = 214.0 mean = 21.40 ms effective rate = 1000 / 21.40 = 46.7 Hz (not 50 Hz) sum of squared deviations from the mean = 160.32 s = sqrt( 160.32 / 9 ) = 4.22 ms largest dt = 33.4 ms J(max) = 33.4 - 20.0 = 13.4 ms (one pass took 1.67 times the period) One slow pass (33.4) dominates all three numbers: the mean and the standard deviation hide it only a little, and the maximum shows it. For a robot the MAXIMUM is the number that matters: the worst pass is the one that arrives late at the obstacle.

Where does jitter come from? From anything that makes one pass take longer or start later than another: the operating system running something else (on a PC or a Raspberry Pi running Linux), a garbage-collection pause (Python on any board), a slow sensor read, a print that fills a buffer, a file write that has to wait for the flash. A board running one program with no operating system has far less jitter than a PC, which is one reason both are used in robots.

The code below keeps these statistics online, one value at a time, without storing the whole list (Welford's method). That matters on a board with a few hundred kilobytes of memory, where a list of a million timings would not fit, and it also avoids the rounding loss of subtracting two large sums.

class LoopStats: """Running statistics of the measured dt values of a loop (Welford's method).""" def __init__(self, period_s): self.period_s = period_s self.n = 0 self.mean_s = 0.0 self._m2 = 0.0 # running sum of squared deviations from the mean self.max_s = 0.0 self.max_dev_s = 0.0 # largest |dt - period| def add(self, dt_s): self.n += 1 delta = dt_s - self.mean_s self.mean_s += delta / self.n self._m2 += delta * (dt_s - self.mean_s) if dt_s > self.max_s: self.max_s = dt_s deviation = abs(dt_s - self.period_s) if deviation > self.max_dev_s: self.max_dev_s = deviation def std_s(self): if self.n < 2: return 0.0 return (self._m2 / (self.n - 1)) ** 0.5 sample_ms = [20.1, 19.9, 20.3, 20.0, 33.4, 19.8, 20.2, 19.9, 20.4, 20.0] stats = LoopStats(0.020) for value in sample_ms: stats.add(value / 1000) print("n = %d" % stats.n) print("mean = %.2f ms (effective rate %.1f Hz)" % (stats.mean_s * 1000, 1 / stats.mean_s)) print("std dev = %.2f ms" % (stats.std_s() * 1000)) print("max dt = %.1f ms max |dt - T| = %.1f ms" % (stats.max_s * 1000, stats.max_dev_s * 1000))
n = 10 mean = 21.40 ms (effective rate 46.7 Hz) std dev = 4.22 ms max dt = 33.4 ms max |dt - T| = 13.4 ms

3. Measuring a loop: time.monotonic and ticks_ms

To measure dt you need a clock that only moves forward. The wall-clock time (time.time()) can jump when the system clock is corrected, so it must not be used for control. In CPython the right clock is time.monotonic() (seconds, a float, never goes backwards); time.perf_counter() is the finest-grained one, best for timing short pieces of code. The MicroPython clock is different, and the difference matters:

MicroPython differs (from memory; unchecked on hardware)
The standard MicroPython time module has ticks_ms(), ticks_us() and ticks_diff(a, b), and sleep_ms() and sleep_us(); it does not have time.monotonic(). The tick counters wrap around (on many ports after 230 ms, which is 12.4 days: the number was recomputed, the port detail is from memory), so you must not subtract two ticks yourself: use ticks_diff(later, earlier), which gives the right answer across the wrap. The tick values are plain integers, not floats, which also avoids float rounding and float memory allocation.

The pair of small functions below hides the difference. The rest of the chapter uses now_ms() and diff_ms() for its schedulers, so the same code runs in both worlds. (Only the CPython branch has been run here.)

import time if hasattr(time, "ticks_ms"): # MicroPython (from memory: not run here) def now_ms(): return time.ticks_ms() def diff_ms(later, earlier): return time.ticks_diff(later, earlier) else: # CPython def now_ms(): return int(time.monotonic() * 1000) def diff_ms(later, earlier): return later - earlier

Now measure a real loop. This block asks time.sleep for 5 ms, forty times, and records how long each pass really took. Run it on your own machine: the output is different every time and different on every computer (Windows and Linux differ, and a busy PC differs from an idle one), which is the whole point. Expect the mean to be a little above 5 ms and the maximum to be noticeably above the mean.

def measure_sleep_loop(period_s, n_passes): dts = [] last = time.monotonic() for _ in range(n_passes): time.sleep(period_s) now = time.monotonic() dts.append(now - last) last = now return dts dts = measure_sleep_loop(0.005, 40) print("asked for 5.0 ms: mean %.2f ms, max %.2f ms" % (1000 * sum(dts) / len(dts), 1000 * max(dts)))

4. Why sleep(dt) drifts, and how to schedule against a deadline

The obvious fixed-rate loop does the work, then sleeps for the period:

# illustration only: the obvious loop, which is NOT run because it does not keep its rate while True: do_one_pass() # takes w seconds of work time.sleep(0.020) # then waits 20 ms: so each pass takes 20 ms PLUS w

Each pass takes the work time w plus the sleep, so the real period is T + w, not T. There is more overhead too (the call itself, the wake-up delay of the system), and it is added on every pass, so the loop falls steadily further behind the schedule it was meant to keep:

Real period with sleep(T): T(real) = T + w real rate f' = 1 / (T + w) Accumulated drift after n passes: drift(n) = n x w (w = the work plus any overhead per pass) Worked example: T = 20 ms, w = 3 ms real period 23 ms, real rate 1 / 0.023 = 43.5 Hz (not 50 Hz) after n = 1000 passes: drift = 1000 x 3 ms = 3.0 s behind the schedule (the pass number 1000 begins at about 23.0 s instead of 20.0 s) What the drift does to the maths: odometry adds v x dt every pass. If the code uses the nominal dt = 0.020 s but the real dt is 0.023 s, 1000 passes at v = 0.3 m/s: the program believes 0.3 x 0.020 x 1000 = 6.0 m, the robot really drove 0.3 x 0.023 x 1000 = 6.9 m. An error of 0.9 m (13 %) from timing alone. The fix: measure dt (never assume it), AND schedule each pass against an ABSOLUTE deadline: deadline(k) = start + k x T so a pass that runs late is followed by a shorter wait, and the errors do not add up. Timing diagram, one character = 1 ms (### = 3 ms of work, . = waiting): ms 0 10 20 30 40 50 60 |.........|.........|.........|.........|.........|.........|......... want S S S S (1) ###....................###....................###....................# (2) ###.................###.................###.................###....... (1) sleep(T) after the work: passes start at 0, 23, 46, 69 ms ... the schedule slips 3 ms more on every pass. (2) wait until the next deadline: passes start at 0, 20, 40, 60 ms ... the work time is absorbed, because only the REST of the period is slept.

Below, the two loops run against a fake clock: an object with a now_s() that returns the time, a sleep() that moves the time forward, and a work() that pretends the program computed for a while. A fake clock makes timing code testable: nothing really waits, and the results are exact. (Passing the clock into the code as an argument is the idea of section 7 in a small case.)

class FakeClock: """A clock you move by hand. Time is kept as whole microseconds, so the results are exact.""" def __init__(self): self.us = 0 def now_s(self): # seconds, like time.monotonic() return self.us / 1000000 def sleep(self, seconds): # moves the time forward, like time.sleep() self.us += int(round(seconds * 1000000)) def work(self, seconds): # pretend the program computed for this long self.us += int(round(seconds * 1000000)) def run_sleep_loop(clock, period_s, work_s, steps): starts = [] for _ in range(steps): starts.append(clock.now_s()) clock.work(work_s) clock.sleep(period_s) # sleeps a full period AFTER the work return starts def run_deadline_loop(clock, period_s, work_s, steps): starts = [] deadline = clock.now_s() for _ in range(steps): starts.append(clock.now_s()) clock.work(work_s) deadline += period_s # the next absolute deadline wait = deadline - clock.now_s() if wait > 0: clock.sleep(wait) # sleeps only the REST of the period return starts for name, runner in (("sleep(T)", run_sleep_loop), ("deadline", run_deadline_loop)): clock = FakeClock() starts = runner(clock, 0.020, 0.003, 1000) ideal = 999 * 0.020 behind = max(0.0, starts[-1] - ideal) print("%-9s pass 1000 began at %.3f s (ideal %.3f s): behind by %.3f s" % (name, starts[-1], ideal, behind))
sleep(T) pass 1000 began at 22.977 s (ideal 19.980 s): behind by 2.997 s deadline pass 1000 began at 19.980 s (ideal 19.980 s): behind by 0.000 s

The first loop is 3 s behind after a thousand passes, n × w = 1000 × 3 ms exactly as the formula says; the second is on time. Now turn the deadline loop into something you can reuse. It must also decide what to do when a pass takes longer than the whole period (an overrun): one choice is to run the next passes back to back until the schedule is caught up, but a burst of passes with tiny dt is as bad for a derivative term as one long pass. The choice made here is to count the overrun, restart the schedule from now, and carry on. The loop also measures the real dt and hands it to the step function, as section 2 asked.

class FixedRateLoop: """Calls step(dt_s) at a fixed rate against absolute deadlines, and measures the real dt.""" def __init__(self, period_s, clock_s, sleep_s): self.period_s = period_s self.clock_s = clock_s # a function returning seconds (time.monotonic, or a fake) self.sleep_s = sleep_s # a function that waits (time.sleep, or a fake) self.stats = LoopStats(period_s) self.overruns = 0 def run(self, step, n_steps): deadline = self.clock_s() previous_start = None for _ in range(n_steps): start = self.clock_s() if previous_start is None: dt_s = self.period_s # first pass: no history, so use the nominal period else: dt_s = start - previous_start # measured time since the last pass began self.stats.add(dt_s) previous_start = start step(dt_s) deadline += self.period_s now = self.clock_s() if deadline > now: self.sleep_s(deadline - now) else: self.overruns += 1 deadline = now # late: restart the schedule, no burst of catch-up passes clock = FakeClock() seen_dt = [] def step(dt_s): seen_dt.append(dt_s) if len(seen_dt) == 11: clock.work(0.055) # one pass hits a slow I2C read and takes 55 ms else: clock.work(0.003) loop = FixedRateLoop(0.020, clock.now_s, clock.sleep) loop.run(step, 20) print("dt of passes 9 to 14 (ms):", [round(d * 1000, 1) for d in seen_dt[8:14]]) print("overruns: %d mean dt %.2f ms max dt %.1f ms" % (loop.overruns, loop.stats.mean_s * 1000, loop.stats.max_s * 1000))
dt of passes 9 to 14 (ms): [20.0, 20.0, 20.0, 55.0, 20.0, 20.0] overruns: 1 mean dt 21.84 ms max dt 55.0 ms

The slow pass (the 55 ms) is counted as one overrun, the passes before and after it keep their 20 ms, and the schedule is not left with a debt to repay. For a real robot you would also log the overrun (section 10) and treat several in a row as a fault (section 11), because a loop that cannot keep its rate is a control problem, not an accounting one.

MicroPython and garbage collection (from memory; unchecked on hardware)
MicroPython collects garbage automatically, and a collection pause can lengthen one pass by a few milliseconds. Building new lists, strings and objects inside the loop makes that more frequent. The usual remedies are to create objects once outside the loop (the RingBuffer of section 10 is built that way), to reuse buffers, and to call gc.collect() at a moment you choose, for example just after the deadline wait begins. On the Raspberry Pi Pico the measured effect depends on the firmware and the heap size: measure it with the LoopStats above.

5. Non-blocking code: never wait inside the loop

A blocking call stops the whole program until it finishes: time.sleep(0.5), a while that waits for a pin to change, a sensor read that waits for an answer. While the program is stuck, nothing else happens: the other sensors are not read, the controller does not run, the motors keep the last command, and the e-stop logic (Robotics Fundamentals 10 section 7) cannot act. The robot is blind and uncontrolled for exactly as long as the wait lasts.

Blind distance: d(blind) = v x t(block) the distance covered while the program is stuck and the motors keep the last command Worked example, the build of Robotics Fundamentals 10 section 9 at v = 0.3 m/s: A programmer adds time.sleep(0.5) to "let the sensor settle": d(blind) = 0.3 x 0.5 = 0.15 m = 150 mm. The stopping distance of that robot is 57.0 mm (Robotics Fundamentals 10 section 7: reaction 34.5 mm from a 115 ms latency, plus 22.5 mm of braking) so the obstacle has to be detected beyond 150 + 57 = 207 mm. The obstacle threshold is 250 mm, so the margin has shrunk from 250 / 57 = 4.4 to 250 / 207 = 1.2. Waiting for a sensor that may never answer is worse: a loop "while not echo_pin.value(): pass" has no limit at all. If a wire is loose, d(blind) is infinite. Even a bounded wait costs rate. An ultrasonic read that waits for the longest echo (4 m range) blocks for 2 x 4 / 343 = 23.3 ms (c = 343 m/s) which is longer than the whole 20 ms period: the loop cannot run at 50 Hz at all (the demonstration below).

The demonstration runs the FixedRateLoop with a step that blocks for 23.3 ms (the longest echo) plus 1 ms of other work:

clock = FakeClock() def blocking_step(dt_s): clock.work(0.0233 + 0.001) # wait for an echo that never comes, plus 1 ms of other work loop = FixedRateLoop(0.020, clock.now_s, clock.sleep) loop.run(blocking_step, 101) print("overruns: %d of 101 mean dt %.1f ms = %.1f Hz instead of 50 Hz" % ( loop.overruns, loop.stats.mean_s * 1000, 1 / loop.stats.mean_s))
overruns: 101 of 101 mean dt 24.3 ms = 41.2 Hz instead of 50 Hz

The cure is to never wait: start the slow thing, go and do other work, and come back to collect the answer when it is ready. The simplest tool is an “every N milliseconds” check. Each task owns a deadline; each pass of a fast main loop asks every task whether its deadline has passed, and runs the ones that are due. The class below does the checking, and advances its deadline by the period (an absolute deadline again, for the same reason as in section 4), so that it does not drift. It uses diff_ms, so it is safe across the tick wrap-around on MicroPython.

class Every: """True once per period_ms when asked; never blocks. Uses absolute deadlines, so it does not drift.""" def __init__(self, period_ms, start_ms=0): self.period_ms = period_ms self.next_ms = start_ms + period_ms def due(self, now_ms): if diff_ms(now_ms, self.next_ms) >= 0: # the deadline has passed self.next_ms += self.period_ms if diff_ms(now_ms, self.next_ms) >= 0: # more than a whole period late: self.next_ms = now_ms + self.period_ms # skip the missed ticks, do not run a burst return True return False class Scheduler: """A list of (Every, function) pairs. Call poll() as often as you like, from the main loop.""" def __init__(self): self.tasks = [] def add(self, period_ms, func, start_ms=0): self.tasks.append((Every(period_ms, start_ms), func)) def poll(self, now_ms): for every, func in self.tasks: if every.due(now_ms): func(now_ms) counts = {"control": 0, "sonar": 0, "log": 0} sonar_times = [] def control_task(now_ms): counts["control"] += 1 def sonar_task(now_ms): counts["sonar"] += 1 sonar_times.append(now_ms) def log_task(now_ms): counts["log"] += 1 scheduler = Scheduler() scheduler.add(20, control_task) # control loop: every 20 ms (50 Hz) scheduler.add(60, sonar_task) # ultrasonic ping: every 60 ms scheduler.add(500, log_task) # log line: every 500 ms for t_ms in range(0, 1001, 5): # a main loop that polls every 5 ms for one second scheduler.poll(t_ms) print(counts) print("sonar ran at", sonar_times[:4], "...") every = Every(20) print([every.due(t) for t in (10, 20, 100, 110, 119, 120)])
{'control': 50, 'sonar': 16, 'log': 2} sonar ran at [60, 120, 180, 240] ... [False, True, True, False, False, True]

In the one second, the 20 ms task ran 50 times, the 60 ms task 16 times (at 60, 120, ... 960 ms) and the 500 ms task twice, and nothing waited. The last line shows the late-task rule: after a gap, the task at 100 ms runs once (not four times), and its next deadline is 120 ms. A scheduler polled every 5 ms starts a task up to 5 ms after its deadline, so the polling period is a jitter floor: poll faster than your fastest task needs. Note what the design gives the ultrasonic sensor: the sensor task can send the trigger pulse and return at once; a later pass collects the echo time when the pin has changed (the interrupt and timer techniques for this are the subject of Programming for Robotics 2 and Programming for Robotics 8, both planned and not yet written).

The non-blocking rule
Inside the control loop, no call may wait for the outside world without a limit. Every wait is either replaced by a state (“I asked at time T, I will look again at T + 60 ms”) or bounded by a timeout that is shorter than the loop period, and a timeout is reported as a missing value (section 11), not hidden. The chapter on state machines (Programming for Robotics 7, planned) uses the same “remember the time you started, check later” idea to build timeouts.

6. Structure: sensors, estimator, controller, actuators

A robot program is easier to write, test and change when each stage of the loop is its own small piece with a clear input and a clear output. The data flows one way, down the page:

hardware: range_m() and ticks() are read ... | v +--------------------------------+ | read_sensors() | the ONLY place that touches the sensors; | returns Readings | turns failures into None +--------------------------------+ | Readings (raw: may contain None) v +--------------------------------+ | update_estimate() | filters, converts units, counts missed readings; | writes RobotState | the best guess of "what is true now" +--------------------------------+ | RobotState (the one place that holds what the robot remembers) v +--------------------------------+ | decide() | pure logic: state in, wheel speeds out; | returns (left, right) m/s | no hardware, no clock +--------------------------------+ | wheel speeds in m/s v +--------------------------------+ | drive() | the ONLY place that touches the motors; | clamps, converts to duty | limits everything +--------------------------------+ | duty cycles, -1 to +1 v ... hardware: set_duty() Robot.update(dt_s) = read_sensors -> update_estimate -> decide -> drive (one pass of the loop)

The rules that make this useful:

  • Each stage has one job. read_sensors reads and validates; update_estimate turns readings into the quantities the logic needs (a filtered range, a speed in m/s); decide turns those into commands; drive turns commands into hardware outputs, with limits. The names follow Robotics Fundamentals 1 section 7 (sensors, controller, actuators), with an estimator in the middle (Robotics Fundamentals 6 section 5 on sensor fusion).
  • State lives in one place. Everything the robot remembers between passes (the filtered range, how stale it is, the last tick counts, the last commands, the current fault) is a field of one RobotState object. Not in globals scattered over the files, not hidden inside five different objects. One object can be logged in one line, printed, copied for a test and reset in one place.
  • decide is a pure function of the state. It reads no sensor and no clock, so a test can give it any state it likes and check the answer, with no robot at all.
  • Units belong in the data. Distances in metres, speeds in m/s, time in seconds, and the names say so (range_m, speed_mps, dt_s), the convention that section 8 extends to every constant.

The code for the stages is below. The choices are deliberately small: the “sensors” are one ultrasonic range sensor and two wheel encoders, the “estimator” is an exponential filter on the range (Robotics Fundamentals 5 section 9) plus a wheel-speed calculation (Robotics Fundamentals 4 section 5), the “controller” is the slow-down rule of section 1, and the “actuators” are two motor duty cycles. The constants come from a Config class that section 8 explains; its values are the build robot of Robotics Fundamentals 10 section 9.

First the configuration, so that nothing below needs a bare number:

import math class Config: """Every constant of the robot, in one place. The units are in the names.""" # timing LOOP_PERIOD_S = 0.020 # 50 Hz control loop # geometry: the build robot of Robotics Fundamentals 10 section 9 WHEEL_RADIUS_M = 0.0325 # 65 mm wheels TRACK_WIDTH_M = 0.150 # distance between the wheels TICKS_PER_WHEEL_REV = 600 # 12 counts per motor turn x a 50:1 gearbox METRES_PER_TICK = 2 * math.pi * WHEEL_RADIUS_M / TICKS_PER_WHEEL_REV # derived, never typed # behaviour MAX_SPEED_MPS = 0.40 # the hardware limit: duty 1.0 means this speed CRUISE_SPEED_MPS = 0.30 # the speed we drive at when the way is clear SLOW_RANGE_M = 0.60 # start slowing down inside this range STOP_RANGE_M = 0.25 # stop at this range # sensor limits and filtering RANGE_MIN_M = 0.02 # an HC-SR04 style sensor reads roughly 0.02 to 4 m RANGE_MAX_M = 4.00 RANGE_FILTER_ALPHA = 0.5 # exponential filter: 1.0 = no filtering RANGE_TIMEOUT_S = 0.11 # a range older than this is stale: stop print("metres per tick = %.3f mm" % (Config.METRES_PER_TICK * 1000))
metres per tick = 0.340 mm

Then the state and the four stages. Read them top to bottom in the order of the diagram; each is short enough to read at a glance.

def clamp(value, low, high): return max(low, min(high, value)) class Readings: """What the sensors reported on one pass. None means 'no valid reading'.""" def __init__(self, range_m, left_ticks, right_ticks): self.range_m = range_m self.left_ticks = left_ticks self.right_ticks = right_ticks class RobotState: """Everything the robot remembers between passes, in one object.""" def __init__(self): self.t_s = 0.0 # robot time: the sum of the measured dt values self.range_m = None # filtered range; None until the first valid reading self.range_age_s = 0.0 # time since the last valid reading self.speed_mps = 0.0 # forward speed from the wheel encoders self.left_ticks = 0 # tick counts at the previous pass self.right_ticks = 0 self.cmd_left_mps = 0.0 # the last commanded wheel speeds self.cmd_right_mps = 0.0 self.fault = None # None, or a short text saying why the robot is stopped def read_sensors(hw, cfg): """SENSE: the only place that touches the sensors. A failure becomes None, never an exception.""" try: range_m = hw.range_m() except OSError: # a bus error or a timeout inside the driver range_m = None if range_m is not None and not (cfg.RANGE_MIN_M <= range_m <= cfg.RANGE_MAX_M): range_m = None # outside the sensor's range, or NaN: not a measurement left_ticks, right_ticks = hw.ticks() return Readings(range_m, left_ticks, right_ticks) def update_estimate(state, readings, dt_s, cfg): """ESTIMATE: turn readings into the quantities the logic needs. Writes the state.""" state.t_s += dt_s if readings.range_m is None: state.range_age_s += dt_s # no news: the old value is one pass older else: if state.range_m is None: state.range_m = readings.range_m else: state.range_m += cfg.RANGE_FILTER_ALPHA * (readings.range_m - state.range_m) state.range_age_s = 0.0 d_left = readings.left_ticks - state.left_ticks d_right = readings.right_ticks - state.right_ticks state.speed_mps = 0.5 * (d_left + d_right) * cfg.METRES_PER_TICK / dt_s state.left_ticks = readings.left_ticks state.right_ticks = readings.right_ticks def decide(state, cfg): """THINK: a pure function of the state. No hardware, no clock. Returns wheel speeds in m/s.""" if state.range_m is None or state.range_age_s > cfg.RANGE_TIMEOUT_S: state.fault = "range stale" # no trustworthy range: the safe default is to stop return 0.0, 0.0 state.fault = None fraction = (state.range_m - cfg.STOP_RANGE_M) / (cfg.SLOW_RANGE_M - cfg.STOP_RANGE_M) speed_mps = cfg.CRUISE_SPEED_MPS * clamp(fraction, 0.0, 1.0) return speed_mps, speed_mps def drive(hw, left_mps, right_mps, cfg): """ACT: the only place that touches the motors. Converts speeds to duty cycles and limits them.""" left_duty = clamp(left_mps / cfg.MAX_SPEED_MPS, -1.0, 1.0) right_duty = clamp(right_mps / cfg.MAX_SPEED_MPS, -1.0, 1.0) hw.set_duty(left_duty, right_duty) class Robot: """The skeleton: hardware in, one update() per pass, state in one place.""" def __init__(self, hw, cfg=Config): self.hw = hw self.cfg = cfg self.state = RobotState() self.state.left_ticks, self.state.right_ticks = hw.ticks() # start counting from now def update(self, dt_s): readings = read_sensors(self.hw, self.cfg) update_estimate(self.state, readings, dt_s, self.cfg) left_mps, right_mps = decide(self.state, self.cfg) self.state.cmd_left_mps = left_mps self.state.cmd_right_mps = right_mps drive(self.hw, left_mps, right_mps, self.cfg) def stop(self): self.hw.set_duty(0.0, 0.0) # the safe state: motors off

Notice what Robot does not contain: no pin numbers, no time.sleep, no print, no sensor driver. It receives a hardware object (hw) and a dt_s. That is what lets the same Robot run on a fake, on a simulator and on real pins.

7. A hardware abstraction: the same logic on fakes, a simulator and real pins

The robot logic calls only three hardware methods: hw.range_m() (a distance in metres, or None), hw.ticks() (the two encoder counts) and hw.set_duty(left, right) (motor commands from −1 to +1). That small set of methods is the hardware abstraction: the contract between the logic and the world. Anything that provides those three methods can be plugged in. This is dependency passing: the Robot is given its hardware instead of creating it, so the choice is made once, in main.py, and nothing else changes.

Hardware objectWhat it isUsed for
ScriptedHardwareReturns a list of canned range readings (including None) and records every commandUnit tests of the logic: give it the exact situation you want to check, such as a sensor that goes silent
WallWorldA one-line world: a wall ahead, and encoders that count what the motors were told to doClosed-loop tests of the whole loop on a PC, with no hardware (this chapter)
A simulated robotThe 2D differential-drive simulator, with noisy range sensors and encodersTesting behaviours and tuning before touching hardware (Programming for Robotics 3, planned and not yet written; it will provide the same three methods)
PicoHardwareThe real pins of the build robot: trigger and echo pins, PWM outputs, encoder inputsThe real robot (Programming for Robotics 2, planned and not yet written; a first sketch is below)

First the fakes, then a harness that runs the robot in them. A fake is ordinary code, a few lines each, and a fake that can inject a failure on demand (a missing reading, here) lets you test the failure handling that is nearly impossible to trigger on demand with a real sensor.

class FakeHardware: """Base for fakes. set_duty() records commands; advance() moves the encoder counts like real wheels.""" def __init__(self, cfg=Config): self.cfg = cfg self.duty = (0.0, 0.0) self.commands = [] self._ticks = [0.0, 0.0] def set_duty(self, left, right): self.duty = (left, right) self.commands.append((left, right)) def ticks(self): return int(self._ticks[0]), int(self._ticks[1]) # whole counts, like a real encoder def advance(self, dt_s): for side in (0, 1): speed_mps = self.duty[side] * self.cfg.MAX_SPEED_MPS self._ticks[side] += speed_mps * dt_s / self.cfg.METRES_PER_TICK def range_m(self): raise NotImplementedError class ScriptedHardware(FakeHardware): """Range readings from a list; None means 'the sensor timed out'. The last value repeats.""" def __init__(self, ranges, cfg=Config): FakeHardware.__init__(self, cfg) self._ranges = list(ranges) self._next = 0 def range_m(self): value = self._ranges[min(self._next, len(self._ranges) - 1)] self._next += 1 return value class WallWorld(FakeHardware): """A wall wall_m ahead of the start. The range is what is left after the wheels have turned.""" def __init__(self, wall_m, cfg=Config): FakeHardware.__init__(self, cfg) self.wall_m = wall_m def range_m(self): left, right = self.ticks() travelled_m = 0.5 * (left + right) * self.cfg.METRES_PER_TICK return self.wall_m - travelled_m def simulate(robot, hw, steps, dt_s): for _ in range(steps): hw.advance(dt_s) # the world moves under the last command ... robot.update(dt_s) # ... then the robot senses, decides and acts again def test_stops_when_sensor_goes_silent(): hw = ScriptedHardware([1.0] * 3 + [None] * 8) # three good readings, then eight timeouts robot = Robot(hw) duties = [] for _ in range(11): robot.update(0.02) duties.append(round(hw.duty[0], 2)) assert duties[2] > 0.0 # driving while the readings are good assert duties[7] > 0.0 # still driving on a reading 0.10 s old (5 missed passes) assert duties[-1] == 0.0 # stopped once the reading is older than 0.11 s assert robot.state.fault == "range stale" return duties def test_stops_before_the_wall(): hw = WallWorld(1.0) robot = Robot(hw) simulate(robot, hw, 500, 0.02) # 10 s of closed-loop driving final_range_m = hw.range_m() assert final_range_m >= Config.STOP_RANGE_M - 0.005 return final_range_m print("duty history:", test_stops_when_sensor_goes_silent()) print("final range: %.3f m" % test_stops_before_the_wall()) print("all tests passed")
duty history: [0.75, 0.75, 0.75, 0.75, 0.75, 0.75, 0.75, 0.75, 0.0, 0.0, 0.0] final range: 0.250 m all tests passed

The first test is the point of the exercise. With real hardware you would have to unplug a wire at the right moment to see what the robot does when the sensor goes silent; with the fake it is one line, it is repeatable, and it runs in a millisecond. The test shows the logic holding its last reading for five missed passes (0.10 s, still under the 0.11 s limit) and then stopping. The second test closes the loop through WallWorld and checks that the robot never goes past the stop line.

The real hardware class has the same three methods. This is a first sketch for a Raspberry Pi Pico running MicroPython, written from memory of the machine module: the pin numbers are only an example, there is no direction control or encoder reading yet, and it has not been run on a board. It is here to show the shape of the seam, not to be copied.

# MicroPython sketch for a Raspberry Pi Pico: from memory, NOT run on hardware, NOT run here from machine import Pin, PWM, time_pulse_us import time class PicoHardware: def __init__(self, cfg=Config): self.cfg = cfg self.trig = Pin(14, Pin.OUT) # example pin numbers: check your wiring self.echo = Pin(15, Pin.IN) self.left_pwm = PWM(Pin(10)) self.right_pwm = PWM(Pin(11)) self.left_pwm.freq(1000) self.right_pwm.freq(1000) def range_m(self): self.trig.value(0) time.sleep_us(2) self.trig.value(1) time.sleep_us(10) # a 10 microsecond trigger pulse self.trig.value(0) echo_us = time_pulse_us(self.echo, 1, 25000) # BLOCKS for up to 25 ms: see section 5 if echo_us < 0: # the call returns a negative number on a timeout return None return echo_us * 1e-6 * 343 / 2 # half the round trip at the speed of sound def ticks(self): return 0, 0 # encoders: Programming for Robotics 2 def set_duty(self, left, right): self.left_pwm.duty_u16(int(abs(left) * 65535)) self.right_pwm.duty_u16(int(abs(right) * 65535)) # direction pins are left out of this sketch

Look at the comment on time_pulse_us: this read blocks for up to 25 ms, longer than the 20 ms period, which is exactly the problem of section 5. Making this read non-blocking (an interrupt on the echo pin and a timer, or a start-then-collect pattern) is the first job of Programming for Robotics 2. The architecture lets that change happen inside PicoHardware, with no change to Robot.

8. Constants, units and magic numbers

A magic number is a bare constant in the middle of code whose meaning you must guess. They are the commonest source of robot bugs that look like physics: the wheel size typed in three places, one of them from the old robot; a speed in mm/s mixed with a distance in m. Two habits remove them: name every constant, and put the unit in the name.

# illustration only: the same line of odometry, not run speed = 0.5 * (l + r) * 0.00034 / 0.02 # what is 0.00034? what is 0.02? which unit is the answer in? # and as written in section 6, with names and units: speed_mps = 0.5 * (d_left + d_right) * cfg.METRES_PER_TICK / dt_s
  • The unit is the last part of the name: _m, _mm, _s, _ms, _mps (metres per second), _rad, _deg, _hz, _v. When two names disagree, wheel_radius_m next to distance_mm, the bug is visible before the code runs.
  • Derived values are computed, not typed. METRES_PER_TICK is calculated from the wheel radius and the ticks per revolution, so when you calibrate the wheel radius (Programming for Robotics 5 will calibrate it on the real robot) the tick size follows. For the build: 2 π r / N = 2 π × 0.0325 / 600 = 0.340 mm per tick, the 0.340 mm that Robotics Fundamentals 10 section 9.3 gives.
  • Keep SI units inside, convert at the edges. Inside the program everything is metres, seconds and radians. If a datasheet gives millimetres or a user wants degrees, the conversion happens in the one function that reads the sensor or prints the value.
  • One place for the constants. Config (a config.py module in the layout below) holds every constant, so that tuning a robot means editing one file, and a second robot is a second config. In MicroPython a class of plain attributes is cheap; a dict works too. The logic receives the config as an argument (cfg) instead of importing it, so a test can pass a different one.
  • Check the config at start-up. A one-line assert Config.STOP_RANGE_M < Config.SLOW_RANGE_M catches a typo at the desk, not in the middle of the floor.

9. Project layout

The blocks of this chapter, grouped by what they do, make the layout below. Keeping the hardware, the logic, the timing tools and the tests in separate places is what makes the swapping of section 7 an edit of one line. The names are a suggestion; the idea is what to copy.

robot/ ├── main.py builds the hardware, the Robot and the loop; runs the mission ├── config.py Config: every constant, with units in the names ├── robot.py Readings, RobotState, read_sensors, update_estimate, decide, drive, Robot ├── timing.py now_ms, diff_ms, LoopStats, FixedRateLoop, Every, Scheduler ├── logtools.py RateLimitedPrinter, CsvLog, RingBuffer ├── hardware/ │ ├── __init__.py │ ├── pico.py PicoHardware: the real pins (Programming for Robotics 2) │ ├── sim.py the simulator's robot (Programming for Robotics 3) │ └── fake.py FakeHardware, ScriptedHardware, WallWorld ├── tests/ │ └── test_robot.py the tests of section 7, run on a PC └── logs/ CSV files written by the robot (kept out of version control)

Two small points about it. First, logtools.py is not called logging.py: a file with the name of a standard module hides the real one, and the error that follows is confusing (this is a classic Python mistake that costs an afternoon). Second, only main.py and hardware/pico.py need to change to run on the robot; everything else is the same code that ran on the PC. On a MicroPython board you copy the files to the board's flash; the tests/ folder stays on the PC.

main.py is short: it chooses the hardware, builds the Robot and a FixedRateLoop (passing time.monotonic and time.sleep on a PC, or fakes in a test), and runs the mission inside the try/finally of section 11. Swapping a fake for the real pins is the one line that creates the hardware object.

10. Logging: rate-limited prints, CSV and a ring buffer

A robot cannot be debugged by staring at it, so it must be able to say what it did. Three tools cover most needs, and each fits a different question.

  • Rate-limited prints, for a human watching. A print in a 50 Hz loop produces 50 lines per second, which nobody can read, and on a board a print over a serial link is slow (a blocking call, in fact: section 5 applies). Print at 1 to 4 Hz. The class below reuses Every.
  • CSV logging, for plotting later. One row per pass (or per few passes) of plain numbers, with a header row, opens straight into a spreadsheet or into Python for a graph (a PID response plot, a range-against-time plot). Write times and values in SI units; leave a cell empty for a missing value (None).
  • A ring buffer, for what happened just before the fault. It keeps the last N rows in a fixed block of memory, overwriting the oldest, so it can run all the time at no growing cost, and on a fault you dump it. It is the robot's black box.
Memory budget of a ring buffer of 'capacity' rows of 'k' fields, each stored as a 4-byte float (array type "f"): bytes = capacity x k x 4 Worked example: the last 10 s of a 50 Hz loop with 5 fields per row: capacity = 10 s x 50 Hz = 500 rows bytes = 500 x 5 x 4 = 10 000 bytes = 9.8 KiB The same data as a Python list of lists of floats would take several times more, and would grow without limit if you forgot to trim it. Preallocating the array once also means the loop never allocates, which keeps the garbage collector quiet (section 4). (A 4-byte float keeps about 7 significant digits: fine for a log, not for odometry.)
import io from array import array class RateLimitedPrinter: """Prints at most once per min_interval_ms, however often say() is called.""" def __init__(self, min_interval_ms, out=print): self._every = Every(min_interval_ms) self._out = out def say(self, now_ms, text): if self._every.due(now_ms): self._out(text) return True return False class CsvLog: """Writes a header, then one row per call. None becomes an empty cell.""" def __init__(self, stream, columns): self.stream = stream stream.write(",".join(columns) + "\n") def row(self, *values): cells = [] for value in values: if value is None: cells.append("") elif isinstance(value, float): cells.append("%.4f" % value) else: cells.append(str(value)) self.stream.write(",".join(cells) + "\n") class RingBuffer: """The last 'capacity' rows of n_fields floats, in one preallocated array. A missing value is NaN.""" def __init__(self, capacity, n_fields): self.capacity = capacity self.n_fields = n_fields self._data = array("f", [0.0] * (capacity * n_fields)) self._next = 0 self.count = 0 def push(self, *values): base = self._next * self.n_fields for i in range(self.n_fields): self._data[base + i] = values[i] self._next = (self._next + 1) % self.capacity if self.count < self.capacity: self.count += 1 def rows(self): """The stored rows, oldest first.""" start = (self._next - self.count) % self.capacity for k in range(self.count): base = ((start + k) % self.capacity) * self.n_fields yield tuple(self._data[base + i] for i in range(self.n_fields)) hw = WallWorld(1.0) robot = Robot(hw) csv_stream = io.StringIO() csv = CsvLog(csv_stream, ["t_s", "range_m", "speed_mps", "cmd_mps", "fault"]) ring = RingBuffer(50, 3) printer = RateLimitedPrinter(500) for step in range(1, 101): # two seconds at 50 Hz hw.advance(0.02) robot.update(0.02) s = robot.state csv.row(s.t_s, s.range_m, s.speed_mps, s.cmd_left_mps, s.fault) nan = float("nan") ring.push(s.t_s, s.range_m if s.range_m is not None else nan, s.cmd_left_mps) printer.say(step * 20, "t=%.2f s range=%.3f m cmd=%.3f m/s" % (s.t_s, s.range_m, s.cmd_left_mps)) print("--- the first CSV lines ---") print("\n".join(csv_stream.getvalue().splitlines()[:4])) print("--- the ring buffer: %d rows kept of 100 pushed; oldest and newest ---" % ring.count) kept = list(ring.rows()) print("%.2f %.3f %.3f" % kept[0]) print("%.2f %.3f %.3f" % kept[-1])
t=0.50 s range=0.862 m cmd=0.300 m/s t=1.00 s range=0.712 m cmd=0.300 m/s t=1.50 s range=0.563 m cmd=0.269 m/s t=2.00 s range=0.452 m cmd=0.173 m/s --- the first CSV lines --- t_s,range_m,speed_mps,cmd_mps,fault 0.0200,1.0000,0.0000,0.3000, 0.0400,0.9971,0.2893,0.3000, 0.0600,0.9926,0.3063,0.3000, --- the ring buffer: 50 rows kept of 100 pushed; oldest and newest --- 1.02 0.706 0.300 2.00 0.452 0.173

The four lines of the rate-limited print come in two seconds of 50 Hz passes, instead of a hundred. The CSV lines show the header and the columns; the speed estimate reads 0.000 on the first pass (the robot has not moved yet), then 0.289 and 0.306 m/s while the commanded speed is a steady 0.300, because the encoder counts are whole numbers and one count in a 20 ms pass is 0.017 m/s (the quantisation of Robotics Fundamentals 10 section 9.3, which advised averaging over about five passes). That is useful to see in a log rather than to find out later on a graph. The ring buffer kept the last fifty of the hundred rows: the oldest at 1.02 s, the newest at 2.00 s.

11. Error handling: timeouts, None and a safe stop

Sensors fail, drivers raise exceptions, wires work loose, and programs have bugs. Robot code does not get to crash and apologise: a crash with the motors running leaves the motors running. Plan the failure in four layers (Robotics Fundamentals 8 section 7 and Robotics Fundamentals 10 section 7 give the reasoning):

  1. Bad readings become None. read_sensors catches the bus error, rejects values outside the sensor's range (and NaN) and returns None, so that one bad reading is an ordinary value to the rest of the program, never an exception in the middle of the controller.
  2. Missing data has a timeout. The estimator counts the age of the last good reading. The logic may use an old reading for a short, stated time (here 0.11 s, a little over five passes) and then it treats the data as gone.
  3. Safe default outputs. When data is missing or stale, decide returns zero speed. Stop is the safe state for a small wheeled robot; other robots need a different answer (an arm holding a load should hold, not release: Robotics Fundamentals 9 section 10), and the answer is decided in advance, not at the moment of the failure.
  4. A top-level try/finally that stops the motors. If a bug raises an exception anywhere in the loop, the finally clause runs on the way out, so the program never leaves the robot driving. It also runs on Ctrl-C and at the normal end.
def run_mission(robot, loop, steps): try: loop.run(robot.update, steps) finally: robot.stop() # runs at the end, on an exception, and on Ctrl-C class CrashingRobot(Robot): """A robot with a bug that appears after 0.1 s of driving, to test the safe stop.""" def update(self, dt_s): Robot.update(self, dt_s) if self.state.t_s > 0.1: raise ZeroDivisionError("a bug in the logic") hw = ScriptedHardware([1.0]) # the way ahead is clear: the robot drives robot = CrashingRobot(hw) clock = FakeClock() loop = FixedRateLoop(0.020, clock.now_s, clock.sleep) try: run_mission(robot, loop, 100) except ZeroDivisionError as err: print("crashed:", err) print("duty after the crash:", hw.duty) print("commands sent: %d, the last one %s" % (len(hw.commands), hw.commands[-1]))
crashed: a bug in the logic duty after the crash: (0.0, 0.0) commands sent: 7, the last one (0.0, 0.0)

The robot crashed after six passes, and the last command it sent was a stop. Without the finally, the duty would still be 0.75 (the cruise speed) and a real robot would have driven on, with nothing in charge. Note the limits of this safety net, which are the same as in Robotics Fundamentals 10 section 7: a finally clause cannot run if the whole program is frozen (an infinite loop, a hung driver), if the board loses power or resets, or if the interpreter itself dies. Those cases need the hardware watchdog and the hardware e-stop; a first treatment of the watchdog timer is in Programming for Robotics 8 (planned, not yet written), and the safety logic as a whole in Programming for Robotics 12 (planned). The software stop is the first layer of safety, not the last.

12. Budgets: the time per pass, and the latency chain

A loop at 50 Hz has 20 ms for everything: that is the time budget. Each stage costs some of it, and the sum must fit with room to spare, because the worst pass (a log write, a garbage collection, a retry) is longer than the average.

Time budget = the period: T = 1 / f 50 Hz -> T = 20 ms Cost of one pass: C = c(sense) + c(estimate) + c(decide) + c(act) + c(log) + ... CPU utilisation: U = C / T headroom = T - C Fastest possible rate: f(max) = 1 / C (U = 100 %: no room for anything else) A rule of thumb: keep U under about 50 %, measured on the WORST pass, not the average Worked example (the numbers below are the fake costs of the demonstration that follows): sense 1.20 ms + estimate 0.30 ms + decide 0.20 ms + act 0.80 ms + log 1.50 ms = 4.00 ms U = 4.00 / 20 = 0.20 = 20 % headroom = 20 - 4.00 = 16.00 ms f(max) = 1 / 0.004 = 250 Hz At U = 50 % the period could shrink to C / 0.5 = 8 ms = 125 Hz at the most. The latency chain (Robotics Fundamentals 1 section 2): the time from "something happens" to "the motors respond" is the SUM of the delays, and the loop contributes one period T (the reading may arrive just after the pass began, so it waits up to a whole period): t(lat) = t(sensor) + T + t(compute) + t(actuator) d(react) = v x t(lat) The build of Robotics Fundamentals 10 section 7: 60 + 20 + 5 + 30 = 115 ms -> at 0.3 m/s, d(react) = 0.3 x 0.115 = 34.5 mm. Halving the period to 10 ms saves 10 ms: t(lat) = 105 ms, d(react) = 31.5 mm (3 mm: a faster loop helps less than you expect). Raising the period to 50 ms (a 20 Hz loop): t(lat) = 145 ms, d(react) = 43.5 mm. A blocking wait of t(block) adds to the chain directly (section 5): every millisecond blocked is v x 1 ms of travel without control.

The budget needs measurements, not guesses. The helper below times each stage of a pass and reports the mean cost per stage; the demonstration feeds it the fake costs of the worked example, so the printed numbers can be checked against the arithmetic above. On a real robot you would run the same helper with the real clock and look at the largest value as well as the mean (extend time() to keep the maximum).

class StageTimer: """Adds up the time spent in each named stage of a pass.""" def __init__(self, clock_s): self.clock_s = clock_s self.total_s = {} def time(self, name, func, *args): start = self.clock_s() result = func(*args) self.total_s[name] = self.total_s.get(name, 0.0) + (self.clock_s() - start) return result clock = FakeClock() timer = StageTimer(clock.now_s) costs_s = [("sense", 0.0012), ("estimate", 0.0003), ("decide", 0.0002), ("act", 0.0008), ("log", 0.0015)] def make_stage(seconds): def stage(): clock.work(seconds) # a stand-in that "takes" this long return stage stages = [(name, make_stage(seconds)) for name, seconds in costs_s] passes = 100 for _ in range(passes): for name, stage in stages: timer.time(name, stage) total_ms = 0.0 for name, _ in stages: mean_ms = 1000 * timer.total_s[name] / passes total_ms += mean_ms print("%-9s %5.2f ms" % (name, mean_ms)) print("%-9s %5.2f ms = %.0f %% of the 20 ms period; headroom %.2f ms" % ("total", total_ms, 100 * total_ms / 20, 20 - total_ms))
sense 1.20 ms estimate 0.30 ms decide 0.20 ms act 0.80 ms log 1.50 ms total 4.00 ms = 20 % of the 20 ms period; headroom 16.00 ms

13. Challenges

Work these with a pencil and with Python. The solutions are text files with the working, and the code in them was run.

Challenge 1: Jitter statistics, drift and the budget
(a) A 100 Hz loop (T = 10 ms) was measured for eight passes, in milliseconds: 10.1, 9.9, 10.0, 10.3, 14.2, 9.8, 10.1, 9.9. Find the mean, the sample standard deviation, the largest dt, the largest |dt − T|, and the effective rate. (b) A naive loop that calls time.sleep(0.010) after 1.8 ms of work per pass runs for 5 minutes (300 s of real time). How many passes does it make, how much robot time does the program believe has passed if it adds the nominal 10 ms per pass, and how big is the error? (c) A pass costs 1.1 ms (sense) + 0.4 ms (estimate) + 0.3 ms (decide) + 0.7 ms (act) + 1.0 ms (log). Find the cost C, the utilisation at 100 Hz, the headroom and the fastest rate; then the fastest loop rate that keeps U at or below 50 %. (d) Using the chain of Robotics Fundamentals 10 section 7 (sensor 60 ms, compute 5 ms, actuator 30 ms), find the reaction distance at 0.3 m/s for the 100 Hz loop and for a 20 Hz loop.
View solution
Challenge 2: A debounce that never blocks
A low-battery check must not trigger on one dip. Write a class Debounce with a method update(now_ms, condition) that returns True only after condition has been True on every call for at least hold_ms milliseconds in a row, and returns False again as soon as the condition is false. It must never sleep or wait: it is called on every pass, and it uses diff_ms. Test it with hold_ms = 2000 on a battery voltage that is sampled every 500 ms and goes 7.2, 6.5, 6.4, 7.0, 6.5, 6.4, 6.3, 6.2, 6.1 (volts) from 0 ms upwards, with the condition “voltage below 6.6 V”. At which time does it first report True?
View solution
Challenge 3: A stall check through the hardware abstraction
Robotics Fundamentals 10 section 7 gave the stall rule: when the duty is above 30 % and the wheel speed is under 10 % of the speed that duty asks for, for 0.3 s, cut the motors. Add it to the Robot of this chapter without changing the hardware interface: write a StallRobot(Robot) whose update calls the normal update, counts the time that the stall condition has held, and, when it reaches 0.3 s, sets state.fault = "stall", stops the motors, and stays stopped (a latch: later passes do not drive again). Write a fake StuckHardware (a WallWorld whose wheels do not turn) and show that the robot latches the stall after the right number of passes, and that on a free-running WallWorld it never reports a stall.
View solution

Quick reference

  • The loop: sense, estimate, decide, act, once per pass; one Robot.update(dt_s); state in one RobotState; decide a pure function of the state.
  • Timing: f = 1 / T. Measure dt, never assume it. Report the mean, the standard deviation and the maximum of dt, and max |dt − T|. Use time.monotonic (CPython) or ticks_ms with ticks_diff (MicroPython, from memory), never the wall clock.
  • Drift: sleep(T) after the work gives a real period T + w and a lag of n w after n passes. Schedule against absolute deadlines (deadline += T); on an overrun, count it and restart the schedule.
  • Non-blocking: no unbounded wait in the loop. Blind distance = v × t(block). Use Every or a Scheduler: start the slow thing, come back later.
  • Hardware abstraction: the logic sees three methods (range_m, ticks, set_duty); pass the hardware in. Fakes (ScriptedHardware, WallWorld), a simulator and real pins plug in at the same place.
  • Constants: named, in one Config, with units in the names (wheel_radius_m, dt_s, speed_mps); derived values computed (metres per tick = 2 π r / N = 0.340 mm).
  • Logging: rate-limited prints for people, CSV for plots, a preallocated ring buffer (bytes = capacity × fields × 4) for the moments before a fault.
  • Errors: bad reading to None; stale data times out; the safe default output is stop; a top-level try/finally stops the motors. It does not replace a watchdog or an e-stop.
  • Budget: U = C / T; keep U under about 50 % on the worst pass; the latency chain t(lat) = t(sensor) + T + t(compute) + t(actuator), d(react) = v t(lat).

Next: Programming for Robotics 2, Talking to Hardware

This chapter built a robot that is a skeleton: a loop that keeps its rate, stages with clear jobs, and three hardware methods, all tested against fakes. The next chapter, Programming for Robotics 2 (planned; not yet written), fills the real PicoHardware in: digital inputs and outputs, PWM for the motors, the ADC, and the buses (I2C, SPI, UART) that the sensors speak, with the datasheet-reading and wiring of the build robot from Robotics Fundamentals 10, and with the non-blocking ultrasonic read that section 7 left as an open problem. Programming for Robotics 3 then builds the simulator that makes the same three methods available on a PC with no robot at all.