Obstacle Avoiding Robot with HC-SR04 Ultrasonic Sensor and Servo Scan

An obstacle-avoiding robot drives around a room on its own, stops before it hits a wall, looks around and chooses a new direction. It is the natural next project after a line follower because the robot now has to deal with an unstructured environment instead of a fixed track.

This build adds an HC-SR04 ultrasonic sensor mounted on an SG90 servo to the 2WD chassis. You will learn how ultrasonic ranging works, how to read the sensor without freezing your program for too long, and how to write a small state machine that drives, scans and turns.

How the HC-SR04 Measures Distance

The HC-SR04 has two transducers: one transmits a burst of ultrasound at 40 kHz and the other listens for the echo. You start a measurement by holding the TRIG pin high for at least 10 microseconds. The module then sends its burst and raises the ECHO pin; ECHO stays high until the echo returns. The width of the ECHO pulse is the round-trip travel time of the sound.

distance = (echo time x speed of sound) / 2

Speed of sound at about 20 deg C = 343 m/s = 0.0343 cm/us
distance_cm = echo_us x 0.0343 / 2  =  echo_us / 58.3 (approx.)

Example: echo pulse of 1166 us -> 1166 x 0.0343 / 2 = 20.0 cm

The commonly quoted working range is about 2 cm to 4 m, but in practice soft, angled or very narrow objects reflect poorly and may not be detected at all. Sound travels slightly faster in warm air, so on a hot day readings shift a little; for obstacle avoidance this does not matter.

Hardware Connections

Device	Pin	Arduino Uno
HC-SR04	VCC / GND	5 V / GND
HC-SR04	TRIG	A1
HC-SR04	ECHO	A0
SG90 servo	Signal (orange)	11
SG90 servo	Power (red) / GND (brown)	Separate 5 V supply / common GND
TB6612FNG	PWMA, AIN1, AIN2	5, 7, 8
TB6612FNG	PWMB, BIN1, BIN2, STBY	6, 12, 13, 4

The Servo library uses Timer1 on the Uno, which disables PWM on pins 9 and 10. That is why the right motor direction pins have moved to 12 and 13 in this build, while the two motor PWM pins stay on 5 and 6, which use Timer0. Mount the servo at the front of the chassis with the sensor horn centred at 90 degrees.

Arduino Code

The robot cruises forward while the path is clear. When the distance drops below a threshold it stops, reverses briefly, scans right and left, and turns towards the side with more room. If both sides are blocked it turns around.

Obstacle avoider with servo scanning and a simple decision rule

#include <Servo.h>

const int TRIG = A1, ECHO = A0;
const int SERVO_PIN = 11;
const int PWMA = 5, AIN1 = 7,  AIN2 = 8;    // left motor
const int PWMB = 6, BIN1 = 12, BIN2 = 13;   // right motor
const int STBY = 4;

const int SAFE_CM  = 25;    // stop when an obstacle is closer than this
const int SPEED    = 160;
const int TURN_SPD = 150;

Servo scanner;

void motor(int pwmPin, int in1, int in2, int s) {
  s = constrain(s, -255, 255);
  digitalWrite(in1, s > 0 ? HIGH : LOW);
  digitalWrite(in2, s < 0 ? HIGH : LOW);
  analogWrite(pwmPin, abs(s));
}
void drive(int l, int r) { motor(PWMA, AIN1, AIN2, l); motor(PWMB, BIN1, BIN2, r); }

// Returns distance in cm, or 400 if no echo (treat as clear)
int distanceCm() {
  digitalWrite(TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  unsigned long us = pulseIn(ECHO, HIGH, 25000UL);  // ~4 m max
  if (us == 0) return 400;
  return (int)(us * 0.0343 / 2.0);
}

// Median of three readings rejects single bad echoes
int distanceFiltered() {
  int a = distanceCm(); delay(30);
  int b = distanceCm(); delay(30);
  int c = distanceCm();
  if ((a <= b && b <= c) || (c <= b && b <= a)) return b;
  if ((b <= a && a <= c) || (c <= a && a <= b)) return a;
  return c;
}

int lookAt(int angle) {
  scanner.write(angle);
  delay(350);                // let the servo settle before measuring
  return distanceFiltered();
}

void setup() {
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  int pins[] = {PWMA, AIN1, AIN2, PWMB, BIN1, BIN2, STBY};
  for (int p : pins) pinMode(p, OUTPUT);
  digitalWrite(STBY, HIGH);
  scanner.attach(SERVO_PIN);
  scanner.write(90);         // look straight ahead
  Serial.begin(9600);
  delay(1000);
}

void loop() {
  int front = distanceCm();

  if (front > SAFE_CM) {     // path clear: keep driving
    drive(SPEED, SPEED);
    delay(40);
    return;
  }

  // Obstacle ahead: stop, back off, then look both ways
  drive(0, 0);              delay(150);
  drive(-SPEED, -SPEED);    delay(300);
  drive(0, 0);

  int right = lookAt(20);
  int left  = lookAt(160);
  scanner.write(90);
  delay(250);

  Serial.print("L="); Serial.print(left);
  Serial.print(" R="); Serial.println(right);

  if (left < SAFE_CM && right < SAFE_CM) {
    drive(TURN_SPD, -TURN_SPD); delay(800);   // both blocked: turn around
  } else if (left > right) {
    drive(-TURN_SPD, TURN_SPD); delay(400);   // more room on the left
  } else {
    drive(TURN_SPD, -TURN_SPD); delay(400);   // more room on the right
  }
  drive(0, 0);
  delay(100);
}

Design Notes

The pulseIn timeout of 25 ms limits how long the program can wait for an echo, which caps the blocking time. A missing echo returns 400 cm, meaning the path is treated as clear, because the most common cause of no echo is open space. The median-of-three filter in the scan stage removes the occasional wild reading caused by a stray reflection.

The turn durations of 400 ms and 800 ms are only starting points. The angle actually turned depends on your motors, battery voltage and floor, so time a full spin on your robot and adjust. With wheel encoders, which come later in this series, you can turn by a measured angle instead of a time.

To extend the project, scan at five angles instead of two and turn towards the direction with the largest distance, or keep the servo sweeping slowly while driving so the robot sees obstacles approaching from the side. Another useful improvement is to slow down in proportion to the front distance, for example full speed above 60 cm and gradually less down to the stop distance, which reduces overshoot and makes the robot look far more controlled.

Troubleshooting

Always reads 0 or 400: check TRIG and ECHO are not swapped and the module has a solid 5 V supply.
Robot hits chair legs: thin objects reflect little sound and the beam can miss them. Increase SAFE_CM or add IR proximity sensors at the corners.
Robot jitters or resets during scans: the servo is drawing current from the Arduino 5 V pin. Give it its own supply.
Readings are erratic near walls at an angle: sound reflects away from the sensor. Scanning at several angles helps.
Motor on pins 9 or 10 stops responding to speed after adding the servo: that is the Timer1 conflict described above.

Summary

The HC-SR04 converts echo time into distance using the speed of sound, and a servo lets one sensor look in several directions. A drive-stop-scan-turn state machine with timeouts and a median filter gives a robot that wanders a room reliably. Remember the Timer1 conflict between the Servo library and PWM on pins 9 and 10 when planning your pins.