Autonomous Obstacle-Avoiding Robot with Camera and Ultrasonic Sensor — Part 16: The Robot Sees, Senses, and Decides

New here? This part combines the HC-SR04 ultrasonic sensor from Part 14 and the Pi Camera from Part 15 into one autonomous system — the robot drives, detects obstacles, analyzes which direction is clearer, and steers accordingly. If the hardware setup or remote connection feels unfamiliar, Parts 12–15 have the full context. Otherwise, let’s go.

---

The Loop Closes

A quick check-in on where we are:

Learn to Move → Perception → Localization → Planning → Control → [repeat from Perception]

✅ Learn to Move — Part 13: the C101 drives on its own.
✅ Perception (touch) — Part 14: HC-SR04 detects obstacles ahead.
✅ Perception (vision) — Part 15: Pi Camera sees left and right.
🎯 Part 16: all of it working together.

Until now, each sensor worked in isolation. The HC-SR04 knew something was in the way. The camera knew which side had more room. But they never talked to each other. The robot that stopped for obstacles in Part 14 still picked its turn direction by coin flip — random.choice(['left', 'right']). Statistically fine. Robotically unsatisfying.

Today that changes. The HC-SR04 triggers the stop. The camera analyzes the scene. The robot picks the better direction — not randomly, but based on what it actually sees.

---

A Note on "Good Enough" Vision

The Pi Camera Module 1 we’re using is, to put it kindly, a piece of history. 5MP, narrow field of view, image quality that makes modern cameras nervous. Combined with a Raspberry Pi 3B that the rest of the world has largely moved past, every visual "decision" this robot makes comes with an asterisk: approximately correct, under reasonable lighting conditions, no guarantees.

That’s fine. The logic is sound — edge density on the left versus the right is a legitimate way to measure which direction has more open space. The camera hardware just adds some real-world fuzziness to the output.

Upgrades exist: a servo motor to sweep the camera left and right before deciding, a Pi Camera Module 3 with a wider lens. For now, we don’t need them. What we have works.

---

Connecting to the Pi

The robot’s brain now lives entirely on the Raspberry Pi 3B — no laptop needed at runtime. But to write and upload code, we borrow the laptop’s keyboard and screen wirelessly.

SSH (command line access):

# Windows: PuTTY → Host Name = Pi's IP → SSH → port 22
# macOS/Linux:
ssh pi3@192.168.1.42

VNC (full desktop view via RealVNC Viewer):
Enable it on the Pi first if you haven’t already:

sudo raspi-config

→ Interface Options → VNC → Yes → Finish

Then open RealVNC Viewer on the laptop, enter the Pi’s IP, log in. The Pi desktop appears on your screen.

Full details in Part 12.

Writing and running code:
Open Thonny via VNC → Menu (raspberry icon) → Programming → Thonny Python IDE. Activate the virtual environment first:

source my_project_env/bin/activate

Save your .py files inside the my_project_env folder to avoid import errors. To transfer code from laptop to Pi, copy it to a USB drive, plug the drive into the Pi, and paste into Thonny via VNC.

---

The Code

import RPi.GPIO as GPIO
import time
import cv2
import numpy as np
from picamera2 import Picamera2

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

ENA = 18; IN1 = 27; IN2 = 22
IN3 = 23; IN4 = 24; ENB = 13
TRIG = 17; ECHO = 25

for pin in [ENA, ENB, IN1, IN2, IN3, IN4, TRIG]:
    GPIO.setup(pin, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)

pwm_A = GPIO.PWM(ENA, 50)
pwm_B = GPIO.PWM(ENB, 50)
pwm_A.start(0)
pwm_B.start(0)

# Start camera
picam = Picamera2()
picam.preview_configuration.main.format = 'RGB888'
picam.configure("preview")
picam.start()
time.sleep(2)  # Warm up

def get_distance():
    GPIO.output(TRIG, True)
    time.sleep(0.00001)
    GPIO.output(TRIG, False)
    start_time = stop_time = time.time()
    while GPIO.input(ECHO) == 0:
        start_time = time.time()
    while GPIO.input(ECHO) == 1:
        stop_time = time.time()
    return (stop_time - start_time) * 34300 / 2

def set_motor(in1, in2, pwm, speed):
    GPIO.output(in1, GPIO.HIGH if speed >= 0 else GPIO.LOW)
    GPIO.output(in2, GPIO.LOW if speed >= 0 else GPIO.HIGH)
    pwm.ChangeDutyCycle(abs(speed))

def forward(s): set_motor(IN1, IN2, pwm_A, s); set_motor(IN3, IN4, pwm_B, s)
def backward(s): set_motor(IN1, IN2, pwm_A, -s); set_motor(IN3, IN4, pwm_B, -s)
def turn_left(s): set_motor(IN1, IN2, pwm_A, -s); set_motor(IN3, IN4, pwm_B, s)
def turn_right(s): set_motor(IN1, IN2, pwm_A, s); set_motor(IN3, IN4, pwm_B, -s)
def stop_robot(): set_motor(IN1, IN2, pwm_A, 0); set_motor(IN3, IN4, pwm_B, 0)

def decide_direction():
    print("Obstacle detected! Analyzing with camera...")
    frame = picam.capture_array()
    frame = cv2.flip(frame, -1)  # Remove this line if your camera is not mounted upside down
    gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
    edges = cv2.Canny(gray, 50, 150)
    h, w = edges.shape
    left_density = np.sum(edges[:, :w//2]) / 255
    right_density = np.sum(edges[:, w//2:]) / 255
    print(f"Edge density — Left: {left_density:.0f}, Right: {right_density:.0f}")
    if left_density < right_density:
        print("Left is clearer. Turning left!")
        return "left"
    else:
        print("Right is clearer. Turning right!")
        return "right"

try:
    speed = 50
    print("Running. Press Ctrl+C to stop.")
    while True:
        dist = get_distance()
        print(f"Distance: {dist:.1f} cm")

        if dist > 20:
            forward(speed)
        else:
            stop_robot()
            backward(50)
            time.sleep(0.8)
            stop_robot()
            choice = decide_direction()
            if choice == "left":
                turn_left(speed)
            else:
                turn_right(speed)
            time.sleep(0.8)
            stop_robot()

        time.sleep(0.2)

except KeyboardInterrupt:
    print("Stopped.")
finally:
    stop_robot()
    pwm_A.stop()
    pwm_B.stop()
    GPIO.cleanup()
    picam.stop()

Press F5 to run. Place the robot on an open floor, stand back, and watch it go.

---

What the Code Does — Step by Step

The main loop runs every 0.2 seconds:

1. HC-SR04 fires a pulse and measures distance
2. If distance > 20cm → keep driving forward
3. If distance ≤ 20cm → obstacle detected:
   • Stop
   • Reverse for 0.8 seconds (create space to turn)
   • Camera captures a frame and analyzes edge density left vs right
   • Turn toward the clearer side for 0.8 seconds
   • Stop, then resume the loop

How decide_direction() works:

cv2.Canny finds edges in the grayscale image — the outlines of walls, furniture, objects. The frame is split down the middle. The total edge count on each half is compared: more edges means more stuff, less clear path. The side with fewer edges wins.

Left density 1240, Right density 3890 → Left is clearer → turn left

This replaces the random.choice from Part 14. Same structure, better decision.

---

Three Problems That Came Up During Development

Mixing code from two separate projects looks simple on paper. In practice, the cocktail doesn’t always mix itself.

Problem 1 — Picamera2 wouldn’t start

# ❌ Wrong — capture_file() doesn't work before configure() and start()
picam = Picamera2()
picam.capture_file("test.jpg")

# ✅ Correct
picam = Picamera2()
picam.preview_configuration.main.format = 'RGB888'
picam.configure("preview")
picam.start()
time.sleep(2)  # Give the camera time to warm up

Problem 2 — stop_robot() didn’t actually stop the robot

# ❌ Wrong — sets PWM duty cycle to 0 but IN1–IN4 pins stay HIGH
def stop_robot():
    pwm_A.ChangeDutyCycle(0)
    pwm_B.ChangeDutyCycle(0)

# ✅ Correct — explicitly resets all direction pins
def stop_robot():
    set_motor(IN1, IN2, pwm_A, 0)
    set_motor(IN3, IN4, pwm_B, 0)

Problem 3 — Wrong decision logic copied from the internet

While assembling this part, an online reference used brightness to decide direction — brighter side = more open space. Sounds reasonable until you test it in a room with uneven lighting, a window on one side, or a white wall that reflects more light than a dark hallway. The robot would consistently turn toward the lamp.

Edge density is more reliable: it measures structure in the image, not light level. A clear path has few edges. A wall or piece of furniture has many.

The lesson: when mixing code from multiple sources, always check that the logic matches your actual goal — not just that it compiles and runs.

---

What We Just Built

The C101 robot now:

✅ Drives autonomously — no laptop, no keyboard, no remote
✅ Detects obstacles with the HC-SR04
✅ Analyzes the scene with the Pi Camera
✅ Makes an informed turn decision based on what it sees
✅ Resumes forward motion and repeats

Is it perfect? No. The camera is old, the field of view is narrow, and "edge density" is a rough approximation of "open space." But the architecture is real. Every commercial obstacle-avoiding robot uses some version of this exact loop — sense, stop, look, decide, move. Ours just runs on a $50 Pi and a camera that predates most of the people reading this.

That’s not nothing. That’s actually quite a lot.

---

Next up: Part 17 — Voice commands on the Pi. No laptop required.