Firmware architecture

ESP32 (dual-core Xtensa LX6, 240 MHz), Arduino core on FreeRTOS, built with PlatformIO. The firmware is split so that everything that can be tested without hardware is tested without hardware.

flowchart TB
  subgraph pure["Pure C++, no Arduino headers: built and unit-tested on the PC"]
    MC["mazecore<br/>flood fill, search decisions,<br/>pivot planner, smooth planner,<br/>speed plan, turn table"]
    MO["motioncore<br/>plan runner, path-following law,<br/>wall observer"]
  end
  subgraph app["ESP32 application"]
    MAIN["main.cpp<br/>modes, search loop, console,<br/>calibration routines, NVS"]
    MOT["motion.cpp<br/>1 kHz control task,<br/>odometry, flight log"]
    HAL["hal.cpp<br/>TB6612 PWM, PCNT encoders,<br/>VL53L0X, MPU6050, ADC"]
  end
  MAIN --> MC
  MAIN --> MOT
  MOT --> MO
  MOT --> HAL
  MAIN --> HAL

Tasks and timing

Task	Core	Rate	Priority	Does
ctrl	1	1 kHz, vTaskDelayUntil	max − 2	encoders, gyro, odometry, plan runner or profiles, path following, wall observer, PD + feedforward, PWM, crash detector, flight log
tof	0	200 Hz poll	3	reads each VL53L0X only when it flags a new sample (non-blocking)
loop()	1	event driven	1	mode menu, search decisions, planning, console, NVS

The control task never waits on I2C to the ToF sensors (they live on their own bus and task); the gyro has a separate bus (I2C1), read once per tick. Wi-Fi and Bluetooth are off; if they were on, their stack lives on core 0 and cannot preempt the control loop.

One control tick

sequenceDiagram
  participant T as ctrl task (1 ms)
  participant H as hal
  participant R as PlanRunner
  participant O as observer
  T->>H: encodersDelta(), imuYawRate()
  T->>T: gyro re-zero window (start of run), subtract bias estimate
  T->>T: odometry (midpoint rule)
  T->>R: step(dt) -> v_ref, w_ref, segment mode
  alt orthogonal straight with fresh ToF sample
    T->>O: side walls -> offset, heading, bias; front wall -> along-track
  end
  T->>T: path errors e, psi -> steering rate
  T->>T: position errors, filtered D, PD + kV v + kA a
  T->>H: motorsSetVolts(u_l, u_r, v_batt)

Shared state is guarded by a spinlock (portENTER_CRITICAL); the loop task only starts motions and reads status.

Memory

Item	RAM
Smooth planner workspace: costs, predecessors, indexed heap, chain (4,354 states)	about 70 kB, static
Maze map (walls, known, distances)	1 kB
Flight log: 1,500 samples × 36 B (15 s at 100 Hz)	54 kB, heap, allocated once at boot
Total static (linker report)	108 kB of 320 kB (33 %)
Flash	352 kB of 1.3 MB (27 %)

Nothing is allocated after boot, so planning time and memory are fixed and known.

Modes, presets and calibration

Modes, the console and the bring-up procedure are on Getting started and in the bring-up plan. Calibration values live in NVS (Preferences, namespace cal): ToF offsets, wheel radius, track width, gyro scale and the selected speed preset. The maze is saved in namespace maze after every search leg, so a crash or battery swap loses nothing.

The speed presets are defined once in mazecore (preset()), identical to the Python PRESETS:

Preset	v straight	v diagonal	accel	lateral accel	angular accel
safe	0.8 m/s	0.6 m/s	2.5 m/s²	2.0 m/s²	60 rad/s²
normal	1.2 m/s	1.0 m/s	3.0 m/s²	3.0 m/s²	60 rad/s²
fast	1.6 m/s	1.2 m/s	3.5 m/s²	3.5 m/s²	80 rad/s²

Flight log

During every run the control task records 100 samples per second into a ring buffer (the last 15 s). Serial l prints it as CSV:

t_ms,mode,seg,v_ref,v,w_ref,w,cross_mm,psi_deg,u_l,u_r,tof_l,tof_fl,tof_fr,tof_r,x,y,heading_deg

The simulator writes the same columns (python -m sim.run drive --log sim.csv), and tools/plot_log.py overlays a real run on the simulated one. That comparison is how the model gets corrected once the robot drives: if the real cross-track error or motor voltages disagree with the simulation, the model, not the controller, is fixed first.

Building and testing

cd firmware
pio test -e native     # 6 test cases: flood fill, map symmetry, turn curves, speed plan,
                       # tracking-law signs, and 4 mazes x 2 modes against the simulator
pio run -e esp32       # -Wall, zero warnings

CI runs both on every push, plus the Python suite, lint, and a check that the generated files (test vectors, turn table) are up to date. Releases attach a merged flash image built from the tag.