An embedded stability-monitoring and fault-detection prototype for potential autonomous mobile robot (AMR) applications—verified in Wokwi on the Pico W and demonstrated on physical Pico 2 W hardware.
Explore Wokwi Simulation • Physical Hardware & Photos • Watch Hardware Demo • Pico 2 W Hardware Firmware • Live Wokwi Simulation • Test Evidence • Architecture Guide
Autonomous mobile robots (AMRs), automated guided vehicles (AGVs), and rough-terrain robotic platforms operate in dynamic industrial and agricultural environments where terrain gradient shifts, payload imbalances, and floor irregularities pose severe rollover hazards.
In safety-critical embedded systems, relying on raw accelerometer measurements or naive threshold checks introduces three core failure modes:
- Mechanical Vibration & Transient Acceleration Spikes: Wheel impacts and motor vibrations corrupt instantaneous gravity vector calculations, triggering nuisance alarms or emergency e-stops.
- Sensor Bus Interruptions & Fault Propagation: Intermittent I²C bus stalls, clock-stretching lockups, or register corruption freeze control loops unless isolated by deterministic fault-recovery state machines.
- Rollover After Recovery: Resetting from a sensor fault by blindly assuming a
SAFEstate can lead to catastrophic machinery failure if the vehicle is still resting at a critical angle.
StabilityGuard solves these challenges by combining integer-based acceleration vector validation, 64-bit integer square root math, tangent table interpolation, exponential moving average (EMA) jitter attenuation, asymmetric hysteresis state transitions, and real-time fault recovery.
StabilityGuard separates platform-independent core monitoring logic from hardware-dependent peripheral drivers. The following diagram describes the Pico W simulation reference; the physical Pico 2 W configuration is documented in hardware validation:
+-----------------------------------------------------------------------------------+
| Raspberry Pi Pico W Firmware |
| (sketch.ino) |
| |
| +--------------------+ +--------------------+ +-------------------------+ |
| | MPU6050 (0x68) | | SSD1306 (0x3C) | | Actuation & Indicators | |
| | 16,384 LSB/g | | 128x64 OLED | | LEDs: GP10, GP11, GP12 | |
| | Shared I2C0 Bus | | Shared I2C0 Bus | | Buzzer: GP13 PWM | |
| | 10 Hz Sampling | | 4 Hz Telemetry | | Button: GP14 (Active-L) | |
| +---------+----------+ +---------^----------+ +------------^------------+ |
| | Raw Counts (x, y, z) | Display Telemetry | Non-blocking |
+------------|-------------------------|----------------------------|---------------+
| | |
v | |
+--------------------------------------+----------------------------|---------------+
| StabilityGuard.h |
| |
| 1. Vector Sanity Check : 8192^2 <= (x^2 + y^2 + z^2) <= 24576^2 (0.5g to 1.5g) |
| 2. Fixed-Point Math : 64-bit isqrt64(x^2 + y^2) & 46-point tangent table |
| 3. Dynamic Filtering : Exponential Moving Average (alpha = 0.35) |
| 4. Dual Hysteresis : Warning (12°-15° deadband), Danger (26°-30° deadband) |
| 5. Asymmetric Debounce : 3 consecutive samples escalate, 5 samples de-escalate |
| 6. Direct Fault Return : 5-sample recovery with real-time state reclassification|
+-----------------------------------------------------------------------------------+
stateDiagram-v2
[*] --> SENSOR_FAULT : System Boot / Power-On
SENSOR_FAULT --> SAFE : 5 Valid Samples & Angle < 15.0°
SENSOR_FAULT --> WARNING : 5 Valid Samples & 15.0° <= Angle < 30.0°
SENSOR_FAULT --> DANGER : 5 Valid Samples & Angle >= 30.0°
SAFE --> WARNING : Filtered Tilt >= 15.0° (3 consecutive samples)
SAFE --> DANGER : Filtered Tilt >= 30.0° (3 consecutive samples)
WARNING --> DANGER : Filtered Tilt >= 30.0° (3 consecutive samples)
WARNING --> SAFE : Filtered Tilt < 12.0° (5 consecutive samples)
DANGER --> WARNING : Filtered Tilt < 26.0° (5 consecutive samples, >= 15.0°)
DANGER --> SAFE : Filtered Tilt < 26.0° (5 consecutive samples, < 15.0°)
SAFE --> SENSOR_FAULT : I2C Bus Error / Invalid Vector / Button Injected
WARNING --> SENSOR_FAULT : I2C Bus Error / Invalid Vector / Button Injected
DANGER --> SENSOR_FAULT : I2C Bus Error / Invalid Vector / Button Injected
The full interactive circuit simulation runs in the browser on the Wokwi Embedded Simulator:
🔗 Open Verified Wokwi Project 478987065565532161
Figure 1: Verified Wokwi simulation breadboard showing Raspberry Pi Pico W, GY-521 MPU6050, SSD1306 OLED, status LEDs, piezo buzzer, and fault-injection pushbutton.
This table describes the original Pico W simulation. The physical Pico 2 W uses the same GPIO assignments, but its OLED is 128×32 rather than 128×64; see the physical pinout and validation guide.
| Component | Pin / Interface | Pico GPIO | Function & Electrical Parameters |
|---|---|---|---|
| Raspberry Pi Pico W | Core MCU | - | RP2040 Dual-Core ARM Cortex-M0+ @ 133 MHz, 264 KB SRAM |
| MPU6050 IMU |
SDA / SCL
|
GP4 / GP5
|
3-axis accelerometer configured for 0x68
|
| SSD1306 OLED (128x64) |
SDA / SCL
|
GP4 / GP5
|
Live numerical tilt angle, operational state, graphical severity gauge, and fault diagnostics, I²C address 0x3C
|
| Green Status LED | Anode via |
GP12 |
Indicates SAFE operational state (tilt |
| Yellow Status LED | Anode via |
GP11 |
Indicates WARNING state (tilt |
| Red Status LED | Anode via |
GP10 |
Indicates DANGER state (tilt |
| Piezo Buzzer | Positive terminal | GP13 |
Emits distinct non-blocking alarm cadences ( |
| Fault Pushbutton | Active LOW | GP14 |
Injects instantaneous hardware fault / sensor bus disconnect simulation |
The detailed OLED descriptions below refer to the 128×64 Wokwi simulation. The 128×32 physical screen uses a more compact layout.
StabilityGuard classifies operating conditions into four mutually exclusive states, visually rendered on the hardware indicators and SSD1306 OLED display:
| State | Tilt Angle Threshold | Actuators & Indicators | OLED Display Interface |
|---|---|---|---|
| SAFE | Green LED ON Yellow/Red LEDs OFF Buzzer silent |
Displays filtered angle (STATE: SAFE, [OK . . ] gauge |
|
| WARNING | Yellow LED ON Green/Red LEDs OFF Buzzer silent |
Displays live angle, STATE: WARNING, [######. . ] gauge |
|
| DANGER | Red LED ON Green/Yellow LEDs OFF Buzzer |
Displays live angle, STATE: DANGER, [###########!CRIT] gauge |
|
| SENSOR_FAULT | Invalid vector / Bus disconnect | Yellow + Red LEDs flashing ( Buzzer |
Angle masked (--.- deg), STATE: SENSOR_FAULT, FAULT: <REASON>
|
StabilityGuard has been implemented and bench-tested on a Raspberry Pi Pico 2 W, building on the earlier Pico W simulation. The assembled prototype includes an MPU6050-labeled motion module, a 128×32 SSD1306 OLED, three status LEDs, a piezo buzzer, and a fault-injection button.
Physical tests demonstrated live tilt monitoring, SAFE / WARNING / DANGER indication, audible alarms, SENSOR_FAULT injection, and recovery. The following photographs document the actual physical build and each operating state.
| SAFE | WARNING |
|---|---|
![]() |
![]() |
| DANGER | SENSOR_FAULT |
|---|---|
![]() |
![]() |
▶ Watch the real hardware demonstration (MP4) · Physical firmware folder · Hardware validation and wiring
Simulation vs. physical hardware: The Wokwi reference runs on a Pico W with a 128×64 OLED. The physical prototype uses a Pico 2 W with a 128×32 OLED and a separate hardware startup routine. The two firmware versions are preserved independently, sharing the core state-machine implementation. The physical sketch is kept in Hardware_Pico2w/; its StabilityGuard.h dependency must be in that folder alongside the .ino file for an Arduino IDE build.
Before computing angles, the engine validates that the measured static acceleration vector approximates the local gravitational field (INVALID_VECTOR, protecting against free-fall drops, collision impacts, and disconnected sensor floating buses.
To reduce reliance on floating-point math for the magnitude and angle-ratio calculations:
-
Horizontal Magnitude:
$$h = \text{isqrt64}(x^2 + y^2)$$ Calculated using a 64-bit integer Newton-Raphson approximation. -
Angle Calculation:
$$\theta = \arctan2(h, z)$$ Evaluated using a 46-point precomputed lookup table ($\text{round}(16384 \cdot \tan\theta)$ for$\theta \in [0^\circ, 45^\circ]$ ) combined with linear interpolation and cotangent reflection for$[45^\circ, 90^\circ]$ and$[90^\circ, 180^\circ]$ . Angle interpolation uses a bounded lookup table; floating-point arithmetic is still used for interpolation and EMA filtering.
Raw angular measurements are smoothed using a first-order recursive filter at
To prevent oscillation and relay chattering near classification boundaries:
-
Hysteresis Deadbands:
- Warning Entry:
$15.0^\circ$ | Warning Exit:$12.0^\circ$ ($3.0^\circ$ deadband) - Danger Entry:
$30.0^\circ$ | Danger Exit:$26.0^\circ$ ($4.0^\circ$ deadband)
- Warning Entry:
-
Asymmetric Debounce Timing:
-
Escalation (SAFE
$\to$ WARNING, WARNING$\to$ DANGER): Requires 3 consecutive samples ($300\text{ ms}$ ) meeting or exceeding the entry threshold. -
De-escalation (DANGER
$\to$ WARNING, WARNING$\to$ SAFE): Requires 5 consecutive samples ($500\text{ ms}$ ) strictly below the exit threshold.
-
Escalation (SAFE
When recovering from SENSOR_FAULT:
- The system mandates 5 consecutive valid sensor samples before clearing the fault condition.
- The recovered state is evaluated directly by reclassifying the filtered angle (
classify(filtered_tilt_deg)). If the platform is still tilted at$40^\circ$ when the sensor reconnects, it transitions directly toDANGER, preventing hazardous actuation.
The 'c' serial command computes resting bias offsets:
- Queries raw sensor counts directly via
readRawHardwareAccel()rather than already-offset values, ensuring repeated calibrations are idempotent and do not cause offset drift. - Incorporates an
$8.7^\circ$ ($2500$ counts) tilt guard: if an operator attempts calibration on an incline, the routine rejects the operation to prevent masking an active rollover hazard.
The firmware includes an interactive serial command interpreter operating at 115,200 baud:
Figure 6: Live Wokwi Serial Monitor showing the interactive test harness command menu and real-time execution feedback.
| Command Key | Action | Observed System Response |
|---|---|---|
'0' or 'l'
|
Force Level Orientation ( |
Transitions to SAFE; Green LED ON; Buzzer silent |
'1' or 'w'
|
Force Moderate Tilt ( |
Transitions to WARNING after 3 samples; Yellow LED ON |
'2' or 'd'
|
Force Severe Tilt ( |
Transitions to DANGER after 3 samples; Red LED ON; 1000 Hz alarm beeps |
'a' |
Run Automated Demonstration Cycle | Automatically cycles: SAFE |
'r' |
Return to Real MPU6050 Readings | Resumes reading raw simulated physics values from Wokwi canvas |
'c' |
Zero-Reference Calibration | Averages 20 resting samples; stores hardware bias offsets (guarded to |
'?' or 'h'
|
Display Command Menu | Prints interactive menu and status |
During the final verification run in Wokwi, the automated demonstration sequence ('a') was executed. The complete raw serial stream was captured and preserved in evidence/automated_test_log.txt.
| Timestamp | Pre-Transition State | New Transition State | Filtered Angle | Target Angle | Debounce Criteria Satisfied |
|---|---|---|---|---|---|
| 32,800 ms | SENSOR_FAULT |
SAFE |
5th consecutive valid sample on level boot | ||
| 36,900 ms | SAFE |
WARNING |
3 consecutive samples |
||
| 40,700 ms | WARNING |
DANGER |
3 consecutive samples |
||
| 45,000 ms | DANGER |
WARNING |
5 consecutive samples |
||
| 48,900 ms | WARNING |
SAFE |
5 consecutive samples |
The actual recorded dataset was processed through analysis/telemetry_analysis.py to generate this vector plot:
Figure 7: Vector plot of actual Wokwi simulation telemetry showing raw tilt (cyan), filtered tilt (white), and colored background operational state regions.
- Total Samples Logged: 200 samples @ 10.1 Hz (~19.90 seconds)
- State Occupancy: SAFE: 38.0% | WARNING: 38.5% | DANGER: 21.5% | SENSOR_FAULT: 2.0%
-
Filter Convergence: Step to
$40.01^\circ$ reached$39.47^\circ$ by sample 10, exactly matching the theoretical EMA convergence:$$f_{10} = 40.01 \cdot \left(1 - 0.65^{10}\right) = 39.47^\circ$$
StabilityGuard uses a three-tiered testing hierarchy to validate the firmware:
+-------------------------------------------------------------------------------+
| Three-Tiered Verification Hierarchy |
+-------------------------------------------------------------------------------+
| Tier 1: Standalone Native C++ Suite (tests/test_stability_guard.cpp) |
| Validates core safety logic, fixed-point math, and hysteresis dead- |
| bands with zero hardware or Arduino dependencies. |
| |
| Tier 2: Automated Python Regression Suite (tests/test_runner.py) |
| Independent cross-platform test model verifying 20/20 scenarios with |
| closed-form mathematical expectations. |
| |
| Tier 3: Wokwi Interactive End-to-End Simulation (Pico W Target) |
| Full peripheral integration: shared I²C bus arbitration, non-blocking|
| PWM buzzer, discrete LEDs, SSD1306 OLED, and physical fault button. |
+-------------------------------------------------------------------------------+
Additional physical verification: The Pico 2 W assembly was separately bench-tested with live MPU readings, OLED updates, all three LED states, buzzer behavior, and fault-button recovery. Details and photographs: Physical Hardware Verification. This hands-on check is separate from the Python and native C++ tests listed below.
# Run the 20-scenario Python regression test suite:
python tests/test_runner.py
# Or compile and execute the standalone native C++ suite:
g++ -std=c++11 -Wall -Wextra -O2 tests/test_stability_guard.cpp -o test_stability_guard
./test_stability_guard| ID | Test Scenario | Verified Condition | Status |
|---|---|---|---|
| 01 | Startup State | Boots into SENSOR_FAULT, valid=false, stage=STARTUP
|
PASS |
| 02 | 5-Sample Recovery | Requires exactly 5 consecutive valid readings before entering SAFE | PASS |
| 03 | Stable SAFE Operation | Level reading ( |
PASS |
| 04 | WARNING Entry & Convergence | Enters WARNING after 3 qualifying samples; converges within |
PASS |
| 05 | DANGER Entry & Convergence | Enters DANGER after 3 qualifying samples; converges within |
PASS |
| 06 | Direct SAFE |
Direct step to DANGER without entering WARNING
|
PASS |
| 07 | DANGER |
De-escalates cleanly after 5 qualifying samples below |
PASS |
| 08 | WARNING |
Returns to SAFE after 5 qualifying samples below |
PASS |
| 09 | Hysteresis Boundary Immunity | Holds WARNING state at |
PASS |
| 10 | Timing Asymmetry | Confirms 3 samples to escalate vs. 5 samples to de-escalate | PASS |
| 11 | Transient Spike Rejection | Single-sample |
PASS |
| 12 | Zero Vector Rejection | Acceleration vector INVALID_VECTOR fault |
PASS |
| 13 | Excessive Acceleration Rejection |
INVALID_VECTOR fault |
PASS |
| 14 | Sensor Read Failure Handling | I²C communication transaction failure trips READ_FAILURE fault |
PASS |
| 15 | Injected Fault Button Detection | Hardware pushbutton press immediately trips INJECTED fault |
PASS |
| 16 | 5-Sample Fault Recovery to SAFE | Releasing fault button recovers cleanly to SAFE after 5 valid samples |
PASS |
| 17 | Tilted Fault Recovery to DANGER | Sensor recovering while severely tilted (DANGER
|
PASS |
| 18 | Filter State Reset After Fault | Fault clears old tilt history; no ghost bias carries over | PASS |
| 19 | Extreme Angle Precision | Validated numerical precision at |
PASS |
| 20 | Full Cyclic Transition Sequence | SAFE |
PASS |
Structured machine-readable test results are preserved in tests/test_report.json.
| Engineering Challenge | Selected Solution | Evaluated Trade-Off / Rationale |
|---|---|---|
| Shared I²C Bus Stalls | Explicit STOP frames (Wire.endTransmission(true)) |
Using STOP transactions was stable in the verified Pico W simulation; the precise cause of earlier bus errors was not conclusively isolated. |
| Timer Interrupt Corruption | Asynchronous polling (tone(pin, freq) + manual noTone()) |
Timestamp-based buzzer stopping avoided the duration-based alarm path and coincided with stable simulator behavior; an interrupt collision was not independently proven. |
| Rollover After Sensor Loss | Dynamic classification upon recovery | Blindly assuming a vehicle is SAFE upon sensor reconnection is hazardous. Evaluating the real-time tilt angle on the 5th recovery sample ensures direct transition to DANGER if tilted. |
| Repeated Calibration | Query raw registers via readRawHardwareAccel() |
Reading uncorrected counts prevents previously stored offsets from being subtracted a second time during calibration; accuracy remains subject to sensor and measurement conditions. |
- Simulation reference: The Wokwi logs and screenshots verify the Pico W (
board-pi-pico-w) simulation, including automated state transitions. - Physical integration: A separate Pico 2 W build was bench-tested with actual tilt sensing, OLED states, LEDs, buzzer, and button-triggered fault/recovery behavior. See physical validation. Those are functional prototype tests, not formal certification or calibrated accuracy measurements.
- Simulator compatibility: Earlier Wokwi trials using
board-pi-pico-2wproducedANGLE_MATH_INVALID; the simulator-specific cause was not isolated. Successful physical Pico 2 W testing does not establish that Wokwi's Pico 2 W simulator behaves identically. - Sensor identity: The sensor module returned nonstandard
WHO_AM_I = 0x72at I²C address0x68. Hardware-specific initialization handles the observed response. - Tilt estimation: Accelerometer-only gravity estimation can be distorted by dynamic acceleration.
- Open the verified project in your browser:
👉 https://wokwi.com/projects/478987065565532161 - Click Start Simulation.
- Open the Serial Monitor tab in Wokwi (115,200 baud).
- Verify initial self-test output:
=== StabilityGuard v2.1.1 Production Simulation === # [SELFTEST] Level (0,0,16384): PASS stage=OK magSq=268435456 # [MPU] WHO_AM_I verified: 0x68 # [MPU] Initialization OK, sensor ready. - Type
'a'and press Enter to trigger the automated 20-second demonstration cycle. - Click and hold the red HOLD: SENSOR FAULT button on the canvas to observe real-time fault injection and recovery.









