Servo-Scan an Ultrasonic Sensor to Find Open Space

A single forward-facing distance sensor can tell a robot that something is ahead, but not which way is clear. So it turns blindly, often straight into another obstacle or a dead end. Mounting the HC-SR04 on an SG90 servo fixes this: the robot stops, looks left and right, and turns toward whichever direction is most open—the same trick a person does at a blind corner.

Why one fixed sensor traps the robot

Picture a robot that only sees straight ahead. It meets a wall, knows it must turn, but has no idea whether left or right is better. Pick wrong and it drives into the corner it just came from. A servo-mounted sensor turns one number (distance ahead) into a small map of the space around the front of the robot, so the turn is an informed choice, not a coin flip.

Mounting the sensor on the servo

Fix the HC-SR04 to the servo horn and mount the servo at the front-centre of the chassis, oriented so the horn sweeps the sensor left-to-right (not up and down). At the servo’s centre position the sensor should face dead ahead. That gives you a clean sweep from one side, through straight-on, to the other side.

Sweep and sample

Step the servo across a handful of angles and take a filtered reading at each. Reuse the clean distance function from reading the HC-SR04—a raw ping mid-sweep is far too noisy to steer on.

#include <Servo.h>
Servo scanner;

const int ANGLES[] = {30, 60, 90, 120, 150};
const int N = 5;

int bestAngle() {
  long best = -1;
  int bestA = 90;
  for (int i = 0; i < N; i++) {
    scanner.write(ANGLES[i]);
    delay(180);                 // let the servo REACH the angle first
    long d = medianCm();        // filtered read from the previous tutorial
    if (d > best) { best = d; bestA = ANGLES[i]; }
  }
  scanner.write(90);            // re-centre for driving
  return bestA;                 // > 90 = open to the left, < 90 = right
}

Don’t read mid-move

The single most common bug here is reading before the servo arrives. servo.write() returns instantly, but the SG90 takes time to swing to the new angle. Ping too soon and the distance belongs to wherever the sensor was, not where you asked it to point. A short delay after each write()—enough for the servo to settle—makes every reading trustworthy.

Turning the scan into a decision

The angle with the largest distance is the most open direction. A couple of refinements keep it sensible:

• Prefer straight ahead on ties. If two directions read similar, bias toward centre so the robot doesn’t weave needlessly.
• All directions close? That’s a dead end—back up and scan again rather than nosing further in.

Speed versus coverage

More angles give a finer picture but a slower loop, and a slow loop means the robot reacts late. Three to five angles—left, ahead, right, plus a couple in between—is the usual sweet spot. Hand that “most open” decision to the obstacle-avoidance algorithm, and try the whole behaviour in the obstacle avoidance simulator before you build it.

How long does the servo actually take?

“A short delay” is the vaguest instruction in the code above, and it is worth replacing with a number you measured. An SG90 is specified at roughly 0.1 s per 60° at 4.8 V, unloaded — so the settle time depends on how far it is travelling:

Two things make the real figure worse than the specification. Load: an HC-SR04 and its cable on the horn is small but not zero, and it slows the servo and adds overshoot. Voltage: at 4.0 V from a sagging pack, the same servo is noticeably slower than at 6 V.

Measure yours rather than trusting the table. Command a 60° step, and time how long until the distance reading stops changing:

void measureSettle() {
  scanner.write(60);  delay(500);            // known start
  scanner.write(120);                        // command the step
  const unsigned long t0 = millis();
  long prev = -1;
  for (int i = 0; i < 40; i++) {             // sample every 10 ms
    const long d = pingCm();
    Serial.print(millis() - t0); Serial.print(' '); Serial.println(d);
    delay(10);
  }
}

The reading settles at a definite moment. Use that number plus a small margin, and you have removed the largest source of nonsense from the whole behaviour.

Sweeping in one direction is faster than jumping about

The code above steps 30 → 60 → 90 → 120 → 150, which is already a single sweep, and that is deliberate. Reading angles in a scattered order means every step is a large one, and the settle cost is per step:

A full scan is a blind moment for the robot — it is stopped and not making progress — so halving its duration is a real improvement to how the robot feels. Sweep in order, and if you scan repeatedly, alternate direction so you never pay for the return trip.

Non-blocking scanning

The delay(180) in each step means a five-angle scan blocks for nearly a second. On a robot that is stopped anyway that is tolerable, but it also means bump switches go unread and any other behaviour is frozen.

The same state-machine treatment used for the ping applies here:

enum ScanState { SCAN_IDLE, SCAN_MOVING, SCAN_READING };

ScanState scanState = SCAN_IDLE;
uint8_t   scanIndex = 0;
unsigned long scanSince = 0;
long      scanResult[N];
bool      scanComplete = false;

void updateScan() {
  switch (scanState) {
    case SCAN_IDLE:
      break;

    case SCAN_MOVING:
      if (millis() - scanSince < SETTLE_MS) return;      // measured, above
      scanState = SCAN_READING;
      break;

    case SCAN_READING:
      scanResult[scanIndex] = medianCm();
      if (++scanIndex >= N) { scanComplete = true; scanState = SCAN_IDLE; scanner.write(90); }
      else { scanner.write(ANGLES[scanIndex]); scanSince = millis(); scanState = SCAN_MOVING; }
      break;
  }
}

The main loop calls updateScan() and carries on. Bump switches keep being read, the state machine keeps running, and the scan completes when it completes.

Interpolating between angles

Five readings give five bearings, but the true opening is rarely exactly at one of them. A weighted centroid over the open readings gives a bearing between the samples, in the same way a reflectance array gives a line position between sensors:

// Weight each angle by how open it is, ignoring anything below the stop threshold.
float openBearing() {
  float weighted = 0, total = 0;
  for (int i = 0; i < N; i++) {
    if (scanResult[i] < STOP_CM) continue;             // blocked, contributes nothing
    const float w = scanResult[i] - STOP_CM;           // how much clearance beyond the threshold
    weighted += w * ANGLES[i];
    total    += w;
  }
  return (total > 0) ? (weighted / total) : 90.0f;     // 90 = straight ahead
}

This is a real improvement over “pick the maximum”. Taking the maximum means a doorway sampled slightly off-centre sends the robot at the doorway’s edge; the centroid sends it at the doorway’s middle. It also degrades gracefully — if two adjacent angles are both open, the result is between them rather than arbitrarily one of them.

The cone limits what a scan can tell you

An HC-SR04’s beam has roughly a 15° half-angle, which at 1 m is a cone about 53 cm across. Sampling at 30° intervals therefore overlaps heavily — adjacent readings are largely looking at the same space.

The important consequence is that a scan does not give you angular resolution better than the beam. A reading of 68 cm at 120° means “something is 68 cm away, somewhere within about 15° of 120°” — which is fine for choosing a direction to turn, and useless for measuring the width of a gap. If you need the latter, a VL53L0X on the same servo has an effectively narrow spot and gives a genuinely resolved scan.

Troubleshooting