A 2-axis ultrasonic distance scanner built around an ESP32, two SG90 servo motors, and an HC-SR04 sensor. The scanner sweeps a room along the X and Y axes, streams distance measurements to a PC over a serial connection, and renders the results live as a heatmap.
The goal of this project was to build an ultrasonic room scanner that rotates an ultrasonic sensor across the X and Y axes using two servo motors, collecting distance measurements as it sweeps the room. The system streams the collected data to a PC over a serial connection in real time, where a companion script visualizes it as a heatmap.
The idea was inspired by a project from another student that mounted an ultrasonic sensor on a single servo motor, rotated it along the X axis, and plotted the resulting distance profile. This project extends that concept by adding a second axis (Y), producing a full 2D map of the room instead of a single line.
| Component | Quantity | Purpose |
|---|---|---|
| ESP32 Dev Module | 1 | Microcontroller — drives the servos, reads the ultrasonic sensor, and sends data over UART |
| 30-pin expansion board | 1 | Easier wiring and more stable mechanical/power connections (multiple power inputs) |
| HC-SR04 ultrasonic sensor | 1 | Measures distance via ultrasonic echo; 2–400 cm range (reliable up to ~200 cm in practice) |
| SG90 servo motor | 2 | One drives the X axis (pan), the other the Y axis (tilt); 360° range |
| 3D-printed PLA brackets | 3 | Custom-designed mounts supporting the mechanical structure |
| Jumper wires | — | Connects the components together |
| M1.6 screws | 10 | Fastens and joins components |
| M1.6 nuts | 10 | Fastens and joins components |
- If the bracket holes are too tight or M1.6 hardware isn't available, a 1.6 mm drill bit can be used to widen them.
- A 0°–180° servo (common in many Arduino kits) can be used instead of a 360° SG90.
- The 30-pin expansion board isn't strictly required — the ESP32 alone can drive the same setup, though this introduces power-delivery issues (see Challenges).
Unlike Arduino boards, the ESP32 isn't plug-and-play — getting serial communication working typically requires installing the correct USB-to-serial drivers (commonly CP210x or CH340, depending on the board) plus the relevant software packages.
The ESP32 runs at 5V and struggled to supply enough power to the servos directly. The 30-pin expansion board solves this by allowing multiple power sources to be connected for more stable operation across all components. Each side of the board exposes three pin rows — ground, power (VCC), and signal — which makes wiring the 3-pin servo connectors straightforward.
A small analog servo rated at 1.8 kg/cm torque, available in either 360° or 0°–180° variants depending on the manufacturer (the housings look identical). It's controlled with a 50 Hz PWM signal, where pulse widths between 500 μs and 2400 μs set the shaft position. On the ESP32, the ESP32Servo library abstracts this away, exposing a simple servo.write(<angle>) call.
The sensor works on an echo principle: the TRIG pin emits a short ultrasonic pulse, which reflects off a surface and returns on the ECHO pin. The microcontroller times the round trip and computes distance as distance = time * 0.0343 / 2. It's wired to ESP32 pins 4 (TRIG) and 5 (ECHO), runs at 3.3V (5V also works, tested unintentionally), and offers ~3mm accuracy with a 15° measurement cone.
All three brackets were designed in Tinkercad:
- Sensor mount (Y axis) — holds the HC-SR04 and attaches to the Y-axis servo. Based on an existing model, extended with mounting options on both ends, holes for press-fitting onto the servo horn, and sensor mounting holes (may need to be widened depending on printer tolerance).
- Pan bracket (X axis) — holds the Y-axis servo + sensor assembly and rotates the whole unit around the X axis. Wall height is tall enough to allow a full 360° sweep without obstruction; an opening in the wall accepts the Y-axis servo.
- Base — holds the X-axis servo and serves as the stand for the entire assembly. No screws needed since it rests flat on a surface.
The project consists of two parts: firmware running on the ESP32, and a Python script running on the PC for live visualization.
The firmware moves the servos, measures distance, and streams results over serial. Execution follows a fixed order: initialization, then waiting for commands, then scanning.
Scan configuration
const int TRIG_PIN = 4;
const int ECHO_PIN = 5;
const int SERVO_X_PIN = 18;
const int SERVO_Y_PIN = 19;
const int X_START = 0; const int X_END = 90; const int X_STEP = 5;
const int Y_START = 30; const int Y_END = 90; const int Y_STEP = 5;
const int SETTLE_MS = 100;
const int X_SETTLE_MS = 300;
const int RETURN_DELAY_MS = 8;X_STEP/Y_STEP control scan resolution — lower values mean denser, more accurate scans at the cost of longer scan times. SETTLE_MS/X_SETTLE_MS control how long the firmware waits after a servo move before measuring (too short and residual vibration corrupts readings). RETURN_DELAY_MS controls how fast the servo returns to its starting position.
Initialization
void setup() {
Serial.begin(115200);
Serial.setTimeout(1500);
delay(1000);
Serial.flush();
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
servoX.attach(SERVO_X_PIN);
servoY.attach(SERVO_Y_PIN);
servoX.write(0); delay(500);
servoY.write(30); delay(500);
Serial.println("Ready. Type HELP for commands.");
}Serial runs at 115200 baud, which must match the Python script's BAUD_RATE. Both servos move to their home position, then the firmware prints Ready, which the Python script waits for before issuing any commands.
Command handling
void loop() {
if (Serial.available()) {
String cmd = Serial.readStringUntil('\n');
cmd.trim();
if (cmd.length() > 0) handleCommand(cmd);
}
yield();
}
void handleCommand(String cmd) {
if (cmd == "SCAN") runScan();
else if (cmd == "STOP") stopRequest = true;
else if (cmd == "HOME") { slowMove(...); }
else if (cmd == "HELP") Serial.println("Commands: SCAN | STOP | HOME | HELP");
}Four commands are supported: SCAN starts a scan, STOP aborts one in progress, HOME returns both servos to their start position, and HELP lists the available commands. The Python script sends SCAN automatically, so no manual input is needed during normal use.
Distance measurement
float readDistance() {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
long duration = pulseIn(ECHO_PIN, HIGH, 30000);
if (duration == 0) return -1;
return duration * 0.0343 / 2.0;
}A 10 μs pulse on TRIG triggers the sensor; pulseIn() then times the echo on ECHO. Distance is time * speed of sound (0.0343 cm/μs) / 2, divided by two because the sound travels the distance twice (out and back). The 30000 μs timeout caps the maximum measurable distance and can be tuned for different conditions.
Scanning
void runScan() {
Serial.println("SCAN_START");
for (int x = X_START; x <= X_END; x += X_STEP) {
servoX.write(x);
delay(X_SETTLE_MS);
for (int y = Y_START; y <= Y_END; y += Y_STEP) {
servoY.write(y);
delay(SETTLE_MS);
float dist = readDistance();
if (dist > 0 && dist < 400) {
Serial.print(x); Serial.print(",");
Serial.print(y); Serial.print(",");
Serial.println(dist, 1);
}
}
slowMove(servoY, Y_END, Y_START);
}
Serial.println("SCAN_DONE");
}A nested loop sweeps X in the outer loop and Y in the inner loop, measuring distance at every (x, y) pair and sending it as a comma-separated x,y,distance line that the Python script parses on the fly. Readings beyond 400 cm (out of sensor range) are discarded. After each row, the Y axis returns smoothly to its start position via slowMove(). SCAN_START and SCAN_DONE mark the beginning and end of a sweep for the Python side.
Smooth movement
void slowMove(Servo &servo, int fromAngle, int toAngle) {
if (fromAngle > toAngle) {
for (int pos = fromAngle; pos >= toAngle; pos--) {
servo.write(pos); delay(RETURN_DELAY_MS); yield();
}
} else {
for (int pos = fromAngle; pos <= toAngle; pos++) {
servo.write(pos); delay(RETURN_DELAY_MS); yield();
}
}
}Instead of jumping directly to a target angle, the servo is stepped one degree at a time with a small delay between steps. This reduces mechanical stress and avoids vibrations that would otherwise corrupt subsequent readings. The yield() call lets the ESP32's background tasks run during the loop.
Runs on the PC, opens the serial connection to the ESP32, reads incoming points, and renders them as a live heatmap. Requires pyserial, numpy, and matplotlib:
pip install pyserial numpy matplotlibConnection settings
COM_PORT = "COM5"
BAUD_RATE = 115200
MAX_DIST = 150
COLORMAP = "plasma"COM_PORT must match the serial port the ESP32 is connected to (e.g. COM5 on Windows). BAUD_RATE must match the firmware's Serial.begin() value. MAX_DIST caps the color scale (points farther than this are clamped to the same color). COLORMAP accepts any matplotlib colormap name, e.g. viridis.
Startup
if __name__ == '__main__':
threading.Thread(target=serial_reader, daemon=True).start()
run_visualizer()A background daemon thread handles serial reading while the main thread runs the visualizer. The two run in parallel: the serial thread continuously collects points, while the visualizer thread checks for new data and refreshes the plot roughly every 300 ms.
Thread-safe data sharing
data_lock = threading.Lock()
scan_points = []
new_data_flag = threading.Event()scan_points holds all measurements received so far as (x, y, distance) tuples, guarded by data_lock since both threads access it concurrently. new_data_flag signals the visualizer thread whenever new data has arrived, avoiding unnecessary redraws.
Reading serial data
def serial_reader():
ser = serial.Serial(COM_PORT, BAUD_RATE, timeout=2)
while True: # wait for 'Ready'
line = ser.readline().decode('utf-8').strip()
if 'Ready' in line:
break
time.sleep(2)
ser.write(b'SCAN\n') # trigger a scan
while True: # read results
line = ser.readline().decode('utf-8').strip()
if line == 'SCAN_START':
scan_points.clear()
elif line == 'SCAN_DONE':
new_data_flag.set()
else:
parts = line.split(',')
if len(parts) == 3:
x, y, d = float(parts[0]), float(parts[1]), float(parts[2])
scan_points.append((x, y, d))
new_data_flag.set()The function waits for the Ready message from the ESP32 (plus an extra 2-second buffer) before sending SCAN, then parses each incoming line: SCAN_START clears the previous data set, SCAN_DONE marks the scan as finished, and any x,y,distance line is appended to the point list.
Building the heatmap
def build_heatmap(points):
pts = np.array(points)
x_vals = np.unique(pts[:, 0])
y_vals = np.unique(pts[:, 1])
if len(x_vals) < 2 or len(y_vals) < 2:
return None
grid = np.full((len(y_vals), len(x_vals)), np.nan)
xi = {v: i for i, v in enumerate(x_vals)}
yi = {v: i for i, v in enumerate(y_vals)}
for x, y, d in pts:
grid[yi[y], xi[x]] = d
return grid, x_vals, y_valsConverts the unordered list of (x, y, distance) triples into a 2D grid that matplotlib can render as an image, with unmeasured cells left as NaN.
Live display
def run_visualizer():
fig, ax = plt.subplots(figsize=(9, 6))
cbar = None
def update(_=None):
nonlocal cbar
if not new_data_flag.is_set(): return
new_data_flag.clear()
# ... build grid and draw heatmap ...
im = ax.imshow(
np.clip(grid, 0, MAX_DIST),
origin='lower', aspect='auto',
extent=[x_vals[0], x_vals[-1], y_vals[0], y_vals[-1]],
cmap=COLORMAP, vmin=0, vmax=MAX_DIST
)
timer = fig.canvas.new_timer(interval=300)
timer.add_callback(update)
timer.start()
plt.show()A timer refreshes the plot every 300 ms but skips redrawing if no new data has arrived, keeping CPU usage low. Values are clamped to [0, MAX_DIST] so a few far-away outliers don't wash out the color scale — each point's color represents distance relative to MAX_DIST, not an absolute value.
The project successfully met its goals: the system scans a room, streams the data to a PC, and renders it live as a heatmap. A number of technical issues were resolved along the way, leading to a much better understanding of how microcontrollers, servo motors, and ultrasonic sensors behave in practice.
Hardware
- PLA brackets crack or snap under excessive mechanical stress — first when the Y-axis bracket was press-fit onto the servo without screws, and again when screws were over-tightened and started warping the brackets.
- Powering the servos was a problem both with an Arduino and later with the ESP32, eventually solved by switching to the 30-pin expansion board, which supports an additional power input.
- Initial attempts to move the servos one degree per scan step didn't work well in practice; introducing
X_STEP/Y_STEPto control the step size (and thus resolution) fixed this. 5° turned out to be the finest practical resolution achievable.
Software
- Porting Arduino code to the ESP32 by simply swapping the servo library broke
slow_move(). The cause was the ESP32's FreeRTOS watchdog, which expects the program to "check in" regularly — solved by callingyield(). - Servos moved abruptly at the start and end of each scan; introducing
slowMove()for these transitions fixed it and likely extends component lifespan.
- Higher-precision servo motors
- Sturdier mechanical brackets
- 3D visualization of the scanned data
- Faster scanning via a more efficient movement/filtering algorithm
Images / reference designs:
- 3D ultrasonic scanner design
- ESP32 Dev Module
- 30-pin expansion board
- HC-SR04 ultrasonic sensor
- SG90 servo motor
- M1.6 screw
- M1.6 nut
- ESP32 + expansion board wiring scheme
- SG90 wiring scheme
- HC-SR04 wiring scheme
- Tinkercad — used for designing the 3D-printed brackets