ESP32 obstacle avoiding robot code that doesn't get stuck
Almost every obstacle avoiding car tutorial, including our own, uses the same logic: if the distance is under 20 cm, back up and turn. We ran that code and three alternatives in 40 random simulated rooms. The classic version spent 21% of its time pinned against furniture it never saw. Adding a few-line stall watchdog cut that to 5%.
Why simple obstacle avoiding code fails
The HC-SR04 sends a 40 kHz ping and times the echo. Its datasheet gives a 15° measuring angle, a range of 2 cm to 4 m, and asks for a target of at least 0.5 m² that is "as smooth as possible" [1]. Two things follow:
- Angled walls disappear. Sound bounces off a smooth surface like light off a mirror. Hit a wall head-on and the echo comes straight back; hit it at a shallow angle and the echo flies off sideways. In Brown University's robotics course notes, a plaster wall "disappeared" for their ultrasonic sensor at about 40° [2]. When no echo returns,
pulseIn()times out and the code reads 999 cm: nothing ahead. - Then the car pushes. It keeps both motors on while its corner grinds along the wall or into a cabinet. The sensor keeps reporting open space, so the "if distance < 20 cm" branch never runs. That's the failure people describe as "my robot gets stuck on walls".

The experiment
We wrote a 2D "digital twin" of a small ESP32 car (our Kit 1 numbers: 65 mm wheels, 170 rpm motors, 19 cm track) and ported the Arduino sketch into it line by line, so every delay() and every ping costs the same simulated time as on the board. Then we let each strategy drive for 2 minutes in each of 40 random rooms (3–5 m, 3–6 pieces of furniture at random angles, some chairs with 3 cm legs).
- Sensor model: 15° cone, 2–400 cm, 25 ms timeout, median of 3 pings, 0.5 cm noise, and no echo from a flat surface hit more than 40° from head-on. Because that angle depends on your walls, we also ran 25° and a perfect sensor.
- Motor model: speed proportional to PWM above a dead band, a random ±5% left/right mismatch and a random battery factor, so timed turns are never exactly 90°.
- Contact: the car can slide along a surface or stall against it, and can always back up. "Pinned" = share of time in contact with an obstacle.
The four strategies (all in one sketch, pick with #define STRATEGY):
- Back up, turn right, and turn left instead if the right is blocked too. This is the code from our beginner tutorial and roughly what most tutorials do.
- Random turn: back up, then spin a random direction for 1× to 2× a quarter turn.
- Look both ways: back up, pivot left and measure, pivot right and measure, go towards the more open side; turn around if both are blocked.
- Look both ways + stall watchdog: as 3, plus one rule: if the distance reading hasn't changed by more than 1 cm for 1.5 s, assume the car is pinned and blind, and run the avoid routine anyway.
Results
| Strategy (echo lost beyond 40°) | Time pinned | Rooms pinned >50% | Floor covered in 2 min | Bumps / min |
|---|---|---|---|---|
| 1. Back up, turn right (our old tutorial code) | 21% | 3 / 40 | 37% | 6.5 |
| 2. Random turn | 20% | 3 / 40 | 41% | 4.6 |
| 3. Look both ways | 27% | 6 / 40 | 37% | 5.2 |
| 4. Look both ways + stall watchdog | 5% | 0 / 40 | 43% | 4.9 |


The sensor is the problem, not the turning logic. Here is the time pinned for every strategy as the walls get harder to see:
| Strategy | Echo lost >25° | >40° | Perfect sensor |
|---|---|---|---|
| 1. Back up, turn right (our old tutorial code) | 35% | 21% | 6% |
| 2. Random turn | 32% | 20% | 7% |
| 3. Look both ways | 56% | 27% | 6% |
| 4. Look both ways + stall watchdog | 8% | 5% | 4% |
- With a perfect sensor all four strategies are pinned only 7% or less of the time. Turning smarter barely matters if the car can see every wall.
- Make the walls harder to see and strategies 1–3 fall apart: at 25° the classic code is pinned 35% of the time and "look both ways" 56%. Looking both ways can't help when both looks come back as 999.
- The watchdog is the only change that works without seeing the wall: 8% at 25°, 5% at 40°. A pinned car reads a constant distance (often the 999 timeout), and a constant reading while the motors are on is exactly what the watchdog catches.
- We also tested weaving (steering ±35 PWM in a 1.6 s rhythm so the beam sweeps across more of the wall). On its own it helped a little: look-both-ways went from 27% to 20% pinned. On top of the watchdog it made things slightly worse (5% → 8%), so
WEAVEships as 0.
The code: obstacle_car_v2.ino
Wiring and pins are identical to our ESP32 obstacle-avoiding car tutorial (TRIG 18, ECHO 19 through a divider, L298N on 25/26/27 and 33/32/13). It uses the esp32 core 3.x PWM API, where ledcAttach(pin, freq, bits) replaced ledcSetup + ledcAttachPin [4].
/*
* TinyBot Forge - ESP32 Obstacle-Avoiding Car v2 (four avoidance strategies)
* https://tinybotforge.com/guides/esp32-obstacle-avoiding-robot-code/
*
* Hardware: ESP32 DevKit, HC-SR04 ultrasonic sensor, L298N motor driver, two DC gear motors.
* Wiring: same as https://tinybotforge.com/learn/obstacle-avoiding-car/
* Toolchain: Arduino IDE 2.x or arduino-cli + "esp32 by Espressif Systems" 3.x
* (uses the 3.x ledcAttach / ledcWrite API).
*
* Pick a strategy below. 4 did best in our simulator (40 rooms, see the guide);
* the Python twin obstacle_twin.py runs exactly the same logic.
*
* WARNING: a 5 V HC-SR04 drives ECHO at 5 V. ESP32 pins are 3.3 V only: use a
* 1 kOhm + 2 kOhm divider on ECHO (not needed for the 3.3 V HC-SR04P).
*/
#ifndef STRATEGY
#define STRATEGY 4 // 1 back up + turn right | 2 random turn | 3 look both ways | 4 = 3 + stall watchdog
#endif
// ---------- Pins ----------
const int PIN_TRIG = 18;
const int PIN_ECHO = 19; // through the divider
const int PIN_ENA = 25; // left motor PWM (remove the ENA jumper)
const int PIN_IN1 = 26;
const int PIN_IN2 = 27;
const int PIN_ENB = 33; // right motor PWM (remove the ENB jumper)
const int PIN_IN3 = 32;
const int PIN_IN4 = 13;
// ---------- Tuning (same names in obstacle_twin.py) ----------
const int PWM_FREQ = 1000;
const int PWM_BITS = 8; // speed 0-255
const int SPEED_CRUISE = 170;
const int SPEED_TURN = 160;
const float STOP_CM = 20.0;
const unsigned long BACK_MS = 350;
const unsigned long TURN_MS = 450; // about 90 degrees on our car; tune on yours
const unsigned long LOOK_MS = 300; // one "look" to the side
const int WEAVE = 0; // strategy 4: optional PWM swing while cruising (35 tested: no gain)
const unsigned long WEAVE_MS = 1600; // strategy 4: one left-right weave
const unsigned long STALL_MS = 1500; // strategy 4: reading frozen this long = pinned
// ---------- Motors ----------
void setMotor(int pinEn, int pinA, int pinB, int speed) {
speed = constrain(speed, -255, 255);
digitalWrite(pinA, speed > 0 ? HIGH : LOW);
digitalWrite(pinB, speed < 0 ? HIGH : LOW);
ledcWrite(pinEn, abs(speed));
}
void drive(int left, int right) {
setMotor(PIN_ENA, PIN_IN1, PIN_IN2, left);
setMotor(PIN_ENB, PIN_IN3, PIN_IN4, right);
}
void stopCar() { drive(0, 0); }
// ---------- Ultrasonic ----------
float readOnceCm() {
digitalWrite(PIN_TRIG, LOW);
delayMicroseconds(2);
digitalWrite(PIN_TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(PIN_TRIG, LOW);
unsigned long us = pulseIn(PIN_ECHO, HIGH, 25000UL); // 25 ms timeout, about 4 m
if (us == 0) return 999.0; // no echo came back
return us / 58.0; // datasheet: us / 58 = cm
}
float readDistanceCm() { // median of three pings
float a = readOnceCm(); delay(10);
float b = readOnceCm(); delay(10);
float c = readOnceCm();
if (a > b) { float t = a; a = b; b = t; }
if (b > c) { float t = b; b = c; c = t; }
if (a > b) { float t = a; a = b; b = t; }
return b;
}
// ---------- Avoidance moves ----------
void backUp() {
stopCar(); delay(100);
drive(-SPEED_CRUISE, -SPEED_CRUISE); delay(BACK_MS);
}
void avoidTurnRight() { // strategy 1 (our original tutorial)
backUp();
drive(SPEED_TURN, -SPEED_TURN); delay(TURN_MS);
stopCar(); delay(80);
if (readDistanceCm() > STOP_CM * 1.5) return;
drive(-SPEED_TURN, SPEED_TURN); delay(TURN_MS * 2);
stopCar(); delay(80);
}
void avoidRandom() { // strategy 2
backUp();
int sgn = random(2) ? 1 : -1;
drive(sgn * SPEED_TURN, -sgn * SPEED_TURN);
delay(TURN_MS + random(TURN_MS)); // 1x to 2x a quarter turn
stopCar(); delay(80);
}
void lookBothWays() { // strategies 3 and 4
backUp();
drive(-SPEED_TURN, SPEED_TURN); delay(LOOK_MS); // look left
stopCar(); delay(80);
float left = readDistanceCm();
drive(SPEED_TURN, -SPEED_TURN); delay(2 * LOOK_MS); // look right
stopCar(); delay(80);
float right = readDistanceCm();
if (max(left, right) < STOP_CM * 1.5) { // boxed in: turn around
drive(SPEED_TURN, -SPEED_TURN); delay(2 * TURN_MS);
} else if (left > right) { // left was more open
drive(-SPEED_TURN, SPEED_TURN); delay(2 * LOOK_MS);
} // else: already facing right
stopCar(); delay(80);
}
// ---------- Main ----------
float lastCm = 999.0;
unsigned long lastChange = 0;
void setup() {
Serial.begin(115200);
pinMode(PIN_TRIG, OUTPUT);
pinMode(PIN_ECHO, INPUT);
pinMode(PIN_IN1, OUTPUT);
pinMode(PIN_IN2, OUTPUT);
pinMode(PIN_IN3, OUTPUT);
pinMode(PIN_IN4, OUTPUT);
ledcAttach(PIN_ENA, PWM_FREQ, PWM_BITS);
ledcAttach(PIN_ENB, PWM_FREQ, PWM_BITS);
stopCar();
randomSeed(esp_random());
Serial.printf("TinyBot obstacle car v2, strategy %d: starting in 2 s\n", STRATEGY);
delay(2000);
lastChange = millis();
}
void loop() {
float cm = readDistanceCm();
unsigned long now = millis();
Serial.printf("%lu ms distance %.1f cm\n", now, cm);
#if STRATEGY == 1
if (cm < STOP_CM) avoidTurnRight(); else drive(SPEED_CRUISE, SPEED_CRUISE);
#elif STRATEGY == 2
if (cm < STOP_CM) avoidRandom(); else drive(SPEED_CRUISE, SPEED_CRUISE);
#elif STRATEGY == 3
if (cm < STOP_CM) lookBothWays(); else drive(SPEED_CRUISE, SPEED_CRUISE);
#else
if (fabsf(cm - lastCm) > 1.0) { lastCm = cm; lastChange = now; }
if (cm < STOP_CM || now - lastChange > STALL_MS) { // too close, or pinned and blind
if (cm >= STOP_CM) Serial.println("stall watchdog: reading frozen, backing out");
lookBothWays();
lastCm = 999.0; lastChange = millis();
} else { // WEAVE > 0 sweeps the 15 deg beam
int w = (int)(WEAVE * sinf(2.0f * PI * (float)now / WEAVE_MS));
drive(SPEED_CRUISE + w, SPEED_CRUISE - w);
}
#endif
delay(40);
}
Tune it on your car
- TURN_MS. Put the car on your floor, run strategy 1 and time one turn. 450 ms is roughly 90–100° in our model; a weaker battery or carpet needs more.
- STALL_MS. 1.5 s is long enough that driving across an open room rarely triggers it, short enough to escape quickly. If your car is slow, raise it so the reading has time to change by 1 cm.
- Big empty spaces. In a hall wider than 4 m the sensor can read 999 for more than 1.5 s while the car drives freely, so the watchdog makes a few unnecessary turns. In our 3–5 m rooms the watchdog strategy still covered the most floor. Don't "fix" it by ignoring 999: that is exactly the reading a car pinned against an angled wall returns.
- Have encoders? Kit 1 uses encoder motors. "Motors on, encoder counts flat" is an even better stall signal than a frozen distance, and we'll add it once we've measured it on the real car.
Run the simulator yourself: obstacle_twin.py
About 300 lines, numpy and matplotlib only. Change a constant, rerun, and you'll know whether it helps before you flash anything. --plot paths.png draws one room, --theta 25 makes walls harder to see.
$ pip install numpy matplotlib $ python obstacle_twin.py 20 rooms x 120 s, echo lost beyond 40 deg, seed 1 strategy coverage hits/min time pinned 1 Back up, turn right (our old code) 35% 8.35 22% 2 Random turn 42% 4.08 22% 3 Look both ways 37% 5.08 30% 4 Look both ways + stall watchdog 43% 5.03 6% $ python obstacle_twin.py --plot paths.png saved paths.png
It takes about 3.5 minutes on one CPU core. The default command uses 20 rooms instead of our 40, so its numbers differ slightly from the table; the ranking is the same.
Limits of this test
- It's 2D: no wheel slip in turns, no acceleration, no soft furniture that absorbs sound, no sensor cross-talk.
- The 40° echo limit is a model of a smooth wall. Rough or curved surfaces return echoes from steeper angles; glass and polished cabinets can be worse.
- "Floor covered" counts where the car's body has been in 2 minutes. It's a proxy for "keeps exploring", not a cleaning score.
Build the car this code was written for
Kit 1 is our ESP32 obstacle-avoiding car kit, designed to build without soldering: encoder motors, a step-by-step manual and a simulation twin of the same car. Get one email when the first batch opens.
FAQ
Why does my obstacle avoiding robot still crash into walls?
Usually because the ultrasonic sensor never saw the wall. The HC-SR04 has a narrow 15 degree beam, and a smooth wall hit at a shallow angle reflects the ping away like a mirror, so the sensor times out and the code reads it as open space. The car keeps driving with its wheels pushing against the wall.
What stop distance should I use?
At least the distance the car travels between two readings plus its braking distance and the part of the body ahead of the sensor. With the median-of-three reading in this code, one loop takes up to about 135 ms when nothing echoes back; at about 0.39 m/s that is roughly 5 cm. 20 cm leaves a comfortable margin for a small car.
Do I need a servo to scan left and right?
No. Strategy 3 and 4 pivot the whole car to look left and right, which works with a fixed sensor. A servo makes the look faster because the car doesn't have to turn, but it does not fix the angled-wall problem by itself; the stall watchdog does.
Can I connect the HC-SR04 directly to an ESP32?
TRIG yes, ECHO no. A 5 V HC-SR04 drives ECHO at 5 V and ESP32 pins are 3.3 V only, so put a 1 kOhm + 2 kOhm divider on ECHO, or buy the 3.3 V HC-SR04P version.
Can I test obstacle avoidance code without a robot?
Yes. obstacle_twin.py runs the same four strategies as the Arduino sketch in random 2D rooms with a simple HC-SR04 and motor model. It needs only numpy and matplotlib.
Sources
- ElecFreaks, Ultrasonic Ranging Module HC-SR04 datasheet (15° measuring angle, 2 cm–4 m, 10 µs trigger, µs / 58 = cm, >60 ms cycle, target ≥0.5 m² and smooth).
- Brown University CS148, Sensors and Sensing (ultrasonic sensor loses a plaster wall at about 40°).
- L. Kleeman and R. Kuc, Mobile Robot Sonar for Target Localization and Classification, International Journal of Robotics Research 14(4), 1995 (sonar reflection from planes, corners and edges).
- Espressif, Arduino-ESP32 migration guide 2.x to 3.0 (LEDC:
ledcAttachmergesledcSetupandledcAttachPin). - Simulation, code and measurements: TinyBot Forge, October 2026. The batch script and raw results are available on request.
Need a robot simulation for your product?
We build physics-simulated robot videos and Python robot simulations to order.
See services