TINYBOT FORGE
Home › Learn › ESP32 obstacle avoiding robot code

ESP32 obstacle avoiding robot code that doesn't get stuck

ESP32HC-SR04L298NArduinoSimulationUpdated October 6, 2026

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%.

Firmware compile-checked on 2026-10-06 with arduino-cli 1.5.2-rc.1 and the esp32 core 3.3.12 (ESP32 Dev Module, all 4 strategies, warnings on: all compiled, 0 warnings). Simulator run from a fresh virtualenv with Python 3.13.16, numpy 2.5.3 and matplotlib 3.11.2; the output below is pasted from that run. Results are from a 2D simulation, not yet from a real car: our Kit 1 sample parts arrive in mid-October and we'll add real-floor results here.
The hard case: an angled wall. The pings bounce away, the car runs into the wall, and once the reading has been frozen for about 1.5 s the stall watchdog backs it out. Our Kit 1 car; animation only, not a real photo.

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:

Diagram: an ultrasonic ping hitting a wall head-on returns to the sensor; at 50 degrees it reflects away and the sensor reads nothing ahead
Specular reflection: the same wall, two angles. Simplified diagram.

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).

The four strategies (all in one sketch, pick with #define STRATEGY):

  1. 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.
  2. Random turn: back up, then spin a random direction for 1× to 2× a quarter turn.
  3. 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.
  4. 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 pinnedRooms pinned >50%Floor covered in 2 minBumps / min
1. Back up, turn right (our old tutorial code)21%3 / 4037%6.5
2. Random turn20%3 / 4041%4.6
3. Look both ways27%6 / 4037%5.2
4. Look both ways + stall watchdog5%0 / 4043%4.9
Bar chart: time pinned against obstacles and floor covered for the four strategies; the stall watchdog strategy is pinned least
Mean of 40 rooms. Every run uses the same rooms and start positions for all four strategies.
Two maps of the same room: the classic strategy leaves many contact points along walls, the watchdog strategy few
Same room, same start. Red circles mark where the car touched something.

The sensor is the problem, not the turning logic. Here is the time pinned for every strategy as the walls get harder to see:

StrategyEcho lost >25°>40°Perfect sensor
1. Back up, turn right (our old tutorial code)35%21%6%
2. Random turn32%20%7%
3. Look both ways56%27%6%
4. Look both ways + stall watchdog8%5%4%

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);
}
Download obstacle_car_v2.ino

Tune it on your car

  1. 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.
  2. 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.
  3. 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.
  4. 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.

Download obstacle_twin.py
$ 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

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.

One email when Kit 1 opens. Unsubscribe anytime.

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

  1. 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).
  2. Brown University CS148, Sensors and Sensing (ultrasonic sensor loses a plaster wall at about 40°).
  3. 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).
  4. Espressif, Arduino-ESP32 migration guide 2.x to 3.0 (LEDC: ledcAttach merges ledcSetup and ledcAttachPin).
  5. 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