Real-Time Self-Balancing Robot

The robot needed to do several things at once: read the IMU, estimate pitch, run PID, generate motor step rates, accept live tuning commands, stream telemetry, watch for faults, and keep OTA/debug services available. I treated that as a scheduling problem first, then separated the firmware into a high-priority control task and a lower-priority supervisor task.

In the repo, `main/src/main.c` pins `control_core1` to Core 1 at higher priority and `supervisor_core0` to Core 0 at lower priority. The control task owns time-sensitive balance behavior, while the supervisor task polls commands, publishes telemetry, checks watchdog state, and logs slower driver diagnostics.

The balance loop is driven by a 10 ms `esp_timer` callback. Instead of doing control work inside the timer callback, the callback sets a balance-tick flag and wakes the control task with a task notification, keeping the interrupt-style path short while still giving the control loop a deterministic 100 Hz cadence.

On each balance tick, the control task polls the BNO08x, updates the state estimator, integrates linear acceleration into a small dynamic pitch target, runs the PID controller against pitch and tilt rate, and maps the output into mirrored left/right wheel step frequencies. Keeping that entire sense-control-actuate path inside one high-priority task avoids queue latency between IMU data and motor commands.

Stepper pulse generation has tighter timing needs than the 100 Hz balance loop, so the motor HAL owns a separate 10 us `esp_timer` service for step/dir pulses. The control loop updates target step frequencies, but the motor module handles the actual pulse toggling and clamps output against the live maximum step-rate cap. That separation lets PID output change at the control rate while pulse generation stays smooth enough for the TMC2240 drivers.

The robot exposes UDP commands for `start`, `stop`, motor tests, PID gains, pitch target, velocity gain, and max step-rate cap. Commands flow through a length-1 FreeRTOS queue using overwrite semantics, so the control task reacts to the newest command instead of backing up behind stale inputs.

Telemetry is intentionally slower than the control loop. The supervisor publishes a 50 Hz CSV stream with yaw, pitch, roll, tilt rate, linear acceleration, target pitch, dynamic target, velocity gain, PID output, saturation, wheel step rates, and active-control state. That gave me enough visibility to tune the robot live without putting WiFi or logging work in the timing-critical path.

I built a LabVIEW interface for operating the robot during tests and viewing the live telemetry. The panel shows connection status, the local telemetry port, live pitch on a color-coded gauge, left/right motor RPM, loop timing, robot state, and a pitch/pitch-rate plot for watching the control response over recent samples. The directional controls and large stop button made it easier to command the robot and shut it down quickly while the graph gave insight to motor control response while tuning balance behavior on the bench.

The mechanical design centered on packaging a tall, narrow two-wheel platform so the electronics, battery, IMU, motor drivers, and stepper motors could fit without making the robot impossible to balance. I used the CAD assembly to think through center-of-mass placement, wheel spacing, chassis stiffness, and service access for wiring and debugging. Because the controller reacts directly to pitch and motor response, the mechanical layout mattered as much as the firmware: loose mounting, poor mass distribution, or flex in the frame would show up immediately as oscillation, drift, or falls.

Hardware bring-up needed immediate, boring failure behavior. A software e-stop sets a global request and wakes the control task immediately, which commands both wheels to 0 Hz, disables the motor outputs, resets PID state, clears velocity integration, and drops back to STOP mode. The IMU timeout watchdog uses the same shutdown path after 5 missed balance ticks, so bad sensor data cannot leave the robot driving on stale state.

This clip shows the robot during a balance test, where the firmware is continuously reading pitch, updating the controller, and commanding the stepper motors to keep the chassis upright. It is the clearest end-to-end view of the real-time control stack working against the physical instability of the robot.

I would add clearer fault-state reporting around excessive pitch angle, sustained saturation, and motor-driver diagnostics before extended free-balancing tests. Additionally, a big point of failure seemed to be IMU drift. After running the balance task for a few seconds, the robot would drift off until eventually falling over. To address this, I would like to integrate encoders on the stepper motors to gauge true position and angular velocity, then fuse that with the IMU data.