From Bench to Car: The System That Finally Drives Itself

Image 1 : RC Car with Components

The previous post documented bench testing in full. The LiDAR was tuned with a persistence filter and a narrow forward FOV. The radar pipeline had MTI clutter suppression, CFAR detection, and a nearest-neighbour tracker. YOLO was running on CUDA with bounding-box distance estimation. The sensor health monitoring layer was built. Everything was sitting on a desk pointing at a wall.

The next step was getting it on the car. That sentence sounds simple. It was not.

Neither the ESC reverse bug nor the LiDAR false reading caused by a ribbon cable would have been visible on the bench. Both required real driving to find. This post covers what changed between "everything works on the bench" and "the car drives autonomously and avoids obstacles." The gap between those two things turned out to involve a complete motor controller state machine, an Arduino UDP command protocol, a distributed autostart system, a CSV data logging pipeline, and six logic bugs that only appeared when the system was under real load.

Three Platforms, One Hotspot

The physical system distributes work across three boards connected over a shared WiFi hotspot. Each has a distinct, non-overlapping role.

The Raspberry Pi 5 handles radar. It runs the BGT60TR13C signal processing pipeline and streams processed tracks to the Jetson over UDP on port 9576. The Jetson Orin Nano runs everything else LiDAR parsing, YOLO inference, sensor fusion, motor control decisions, and data logging. The Arduino Uno R4 WiFi receives single-byte UDP commands from the Jetson and translates them directly into ESC and servo PWM signals.

One important addition this session was the laptop override path. The Jetson listens on a separate UDP port for pause and resume commands. When paused, the motor controller immediately stops the car and hands control to the laptop. This made testing significantly safer a single keypress halts everything regardless of what the autonomous logic is doing.

Image 2 : Distributed system architecture Raspberry Pi 5, Jetson Orin Nano, and Arduino Uno R4 WiFi connected over WiFi UDP

Building the Avoidance State Machine

The motor controller is the layer between the fusion output and the physical car. It runs at 20Hz in a dedicated Python thread, reads the latest LiDAR zone distances from a shared lock, and maintains a finite state machine that decides what throttle and steering commands to send to the Arduino over WiFi UDP.

The state machine has six states. Each is time-bounded but sensor data is checked continuously within each state, allowing early exits if conditions change.

NORMAL  Full speed forward with reactive side steering. If either side has more than 1 metre of clearance when the front is blocked, the car steers around the obstacle without stopping at all. This is the bypass mode.

STOPPING (0.5s)  Full brake. The system picks an escape direction based on which side has more clearance at that moment.

REVERSING (2.5s)  Straight reverse with centre steering. The ESC is armed once and holds reverse continuously for the full 2.5 seconds.

TURNING (1.2s)  Slow forward while steering toward the escape direction. This is a moving turn rather than a stationary point turn. If the front is still blocked during this phase, the car aborts immediately and goes back to REVERSING for another 2.5 seconds. It does not drive into the obstacle.

ESCAPING (2.0s) Slow forward on the new heading while maintaining the escape steering angle. After a 0.5 second grace period, if a new obstacle appears ahead the car stops and replans. If a side wall gets within 0.25 metres on the escape side, steering straightens temporarily to avoid scraping.

STRAIGHTENING (0.8s)  Slow forward while recentring steering. All retry counters reset here on a successful escape.

Total time for one full manoeuvre from detection to normal driving resumed: 7 seconds.

Image 3 : Finite state machine six states, timings, transition conditions, and abort paths

The Anti-Stuck Retry System

If the escape direction chosen is also blocked, the system counts attempts and flips direction after three failures. If right is tried three times and always blocked, the system locks the direction to left and tries three times on that side. If left also fails, it flips back. This alternation continues until a path opens.

The direction flip is locked using a flag so that when the car returns to STOPPING it does not silently recalculate and overwrite the flip. Without this lock the direction flip was completely ineffective the car always tried the same side regardless of how many retries had occurred.

In a physically impossible situation where all directions are blocked, the car will keep alternating indefinitely. This is correct behaviour. There is no software fix for a physical impossibility.

The Reverse That Wouldn't Reverse

The most confusing hardware issue of this phase was that the car appeared to reverse in the software logs but did not physically move. The motor controller showed REVERSING state, the Arduino Serial Monitor printed REV, and yet the car sat still.

The cause was in how the ESC arms reverse. The MSC-25RC requires a specific three-step sequence: a brake pulse at 1300 microseconds held for 200 milliseconds, then a neutral pulse at 1500 microseconds held for 300 milliseconds, then the reverse pulse at 1300 microseconds. This whole sequence takes roughly 500 milliseconds to complete.

The motor controller was sending the reverse command every 50 milliseconds every loop tick at 20Hz. Each incoming command restarted the arming sequence from the beginning. The ESC never got past the brake phase.

The fix was a flag on the Arduino called reverseArmed. Once the three-step sequence completes and reverse is armed, all subsequent incoming reverse commands are silently ignored. The ESC holds the reverse PWM signal continuously from within the main Arduino loop. Only a forward, slow, or stop command cancels reverse and resets the state machine back to idle.

Image 4: ESC reverse arming sequence IDLE → BRAKE → NEUTRAL WAIT → REVERSING, with reverseArmed flag preventing restarts

What the CSV Log Files Revealed

A data logger was integrated into the fusion pipeline. It only writes rows when an obstacle is detected or the motor state changes, keeping file sizes manageable during normal driving. Each row captures the full sensor state LiDAR zone distances, radar range and velocity, camera class, motor state, and action taken.

Five CSV log files from real test runs were analysed. Two significant findings came out of the data that were not visible during bench testing.

Finding 1 Indoor tests. LiDAR distances across all three zones were stuck at 0.44 to 0.48 metres throughout the entire log despite the motor controller showing REVERSING state. The car was physically not moving. The escape abort threshold of 1.0 metre meant every ESCAPING attempt immediately re-triggered STOPPING before the car had moved anywhere. Changing the abort threshold to 0.20 metres resolved this.

Finding 2 Open field tests. The LiDAR left zone was permanently reading 0.33 to 0.40 metres in open space with nothing nearby. This triggered IMMINENT state continuously and prevented any normal driving. The cause was the IMX477 camera ribbon cable hanging loose on the front-left side of the chassis and entering the LiDAR scan plane. The fix is to route the ribbon cable behind the LiDAR and raise the sensor on standoffs above the cable clutter.

Finding 3 Timing. Timestamps across all files are consistently 60 to 65 milliseconds apart, confirming the fusion loop is running at approximately 15Hz with no UDP lag or processing bottlenecks. The occasional row showing IMMINENT fusion level alongside NORMAL motor state is a threading race condition between two independent threads expected behaviour in a multithreaded system, not a logic error.

Six Logic Bugs Found Through Code Analysis

Beyond the hardware issues, a systematic trace of every possible state transition scenario found six software logic bugs.

Bug 1. The retry counter was being incremented every loop tick inside the TURNING state 20 times per second. The direction flip was triggering after 150 milliseconds instead of after three genuine turning attempts. Fixed with a flag that ensures the counter increments only once per state entry.

Bug 2. Every time the car returned to STOPPING it recalculated the escape direction from live sensor data and overwrote the direction flip chosen in TURNING. The flip logic was completely ineffective. Fixed with a lock flag that STOPPING checks before recalculating.

Bug 3. The grace period in ESCAPING used a timestamp that had already been modified, making the calculation inaccurate. Fixed by capturing a precise start timestamp when ESCAPING begins.

Bug 4. There was no side wall monitoring during ESCAPING. The car could drive into a side wall while following the escape heading. Fixed by monitoring the side distance in the escape direction and straightening temporarily if it falls below 0.25 metres.

Bug 5. The Arduino was restarting the ESC arming sequence on every incoming reverse command. The car never actually reversed. Fixed with the reverseArmed flag.

Bug 6. The TURNING state drove slow forward even if the front was still blocked. The car drove into the original obstacle during the turn phase. Fixed by checking front clearance during TURNING and aborting back to REVERSING if still blocked.

Infrastructure Changes

The radar streamer on the Pi now launches automatically on boot as a systemd service. It restarts on crash and logs all output. The Jetson logs into the desktop automatically and a desktop autostart file launches the fusion GUI in a terminal window. The entire system is self-starting from power-on with no laptop required for field testing.

Github: https://github.com/rh960/Sensor_Driven_Digital_Twin_For_Collison_Prevention_in_Autonomous_Systems