New here? This part adds basic room mapping to the C101 robot — as it drives, it builds a simple grid map showing where obstacles are. The setup builds on Parts 12–18. If the Raspberry Pi environment feels unfamiliar, Part 12 has the full walkthrough. Otherwise, let's go.
An Honest Disclaimer Before We Start
Every other part of this blog has been tested on real hardware before being written up. This one is different, and it's worth saying plainly: the 18650 battery pack powering this robot died mid-project, and the code below has not been run on the actual C101.
What follows is code that's been carefully reviewed for logic and structure — and in the process of that review, several real bugs were found and fixed. We're documenting both the fixes and the fact that this hasn't been hardware-tested, because that's the honest version of what happened. If you build this and find something that needs adjusting, that's expected, not a failure. Consider yourself the test pilot for this one.
Where We Are
Learn to Move → Perception → Localization → Planning → Control → [repeat from Perception]
By any reasonable measure, the C101 has earned its stripes. It drives on its own, avoids obstacles, picks a turn direction based on what the camera sees, responds to voice commands, and now has a web dashboard. Call it the robotics equivalent of a high school diploma.
This part touches a step we haven't really addressed yet: Localization — knowing where you are, not just what's around you. Robot vacuums do this constantly: they build a rough map of the room as they clean, so they don't bump into the same couch leg forty times.
We're doing a simplified version: a basic occupancy grid map — a grid of cells, each marked either "empty" or "obstacle here," built up as the robot drives around and pings the HC-SR04.
A Reviewer's Confession: What Was Actually Wrong With the First Draft
Before getting to the working code, it's worth walking through what didn't work — because the mistakes themselves are instructive.
Bug 1 — The mapping loop was nested inside the wrong block
The original draft tried to insert the entire mapping loop — plt.ion(), the scanning while loop, all of it — in the middle of the existing autonomous driving loop, right after forward(speed). The indentation didn't line up with the surrounding code, which means Python would have thrown a SyntaxError before the program even started.
Bug 2 — move_forward() doesn't exist
The mapping loop called a function named move_forward(). Every other part of this project calls the same function forward(). A simple naming mismatch — but it would have caused an immediate NameError.
Bug 3 — robot_angle never actually changes
This is the subtle one. The map-building math relies on robot_angle to calculate where an obstacle sits relative to the robot. But nothing in the original code ever updated that variable when the robot turned. turn_left() and turn_right() spin the motors — they have no idea that, conceptually, the robot's heading should now be different. Without updating robot_angle after a turn, the map would assume the robot is always facing the same direction, even after multiple turns. The map would end up confidently, consistently wrong.
None of these are exotic bugs. They're the kind that show up constantly when assembling code from multiple sources and contexts — which is exactly what's been happening since Part 14. The lesson holds steady: reviewing code carefully before running it catches problems that running it blindly would have just turned into confusing crashes.
What This Map Actually Is (And Isn't)
To be clear about expectations: this is not SLAM (Simultaneous Localization and Mapping) in the way a real robot vacuum or warehouse robot does it. Real SLAM systems use wheel encoders, IMUs, or LIDAR to track position with real precision.
We have none of that. What we have is:
- One ultrasonic sensor, pointed straight ahead
- A rough estimate of how far the robot has moved, based on speed and elapsed time (no encoders — just guessing from motor duration)
- A rough estimate of heading, nudged by a fixed amount every time the robot turns
It's less "precision cartography" and more "best guess based on vibes." But the architecture — sense, estimate position, mark the grid, repeat — is the same architecture real mapping systems use. We're just running it with much cheaper inputs.
Setup
Install matplotlib for visualization:
pip install matplotlibEverything else — GPIO, camera, motor functions — builds directly on Part 17's code.
The Code
import RPi.GPIO as GPIO
import time
import cv2
import numpy as np
from picamera2 import Picamera2
import math
import matplotlib.pyplot as plt
# GPIO setup
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)
# --- Grid Map configuration ---
MAP_SIZE_CM = 400 # Real-world map size (4m x 4m)
GRID_SIZE_CM = 10 # Resolution of each cell (10cm x 10cm)
GRID_ROWS = int(MAP_SIZE_CM / GRID_SIZE_CM)
GRID_COLS = int(MAP_SIZE_CM / GRID_SIZE_CM)
occupancy_grid = np.zeros((GRID_ROWS, GRID_COLS)) # 0 = empty, 1 = obstacle
# Robot starts at the center of the map
robot_x = GRID_ROWS / 2
robot_y = GRID_COLS / 2
robot_angle = 0.0 # 0 = facing along positive X axis
# Camera setup
picam = Picamera2()
picam.preview_configuration.main.format = 'RGB888'
picam.configure("preview")
picam.start()
time.sleep(2)
speed = 50
# --- Robot functions ---
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 if 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:
return "left"
else:
return "right"
# --- Map update function ---
def update_map(distance, robot_x, robot_y, robot_angle):
if 2 < distance < 400: # Ignore noise and out-of-range readings
angle_rad = math.radians(robot_angle)
obstacle_x = robot_x + (distance * math.cos(angle_rad)) / GRID_SIZE_CM
obstacle_y = robot_y + (distance * math.sin(angle_rad)) / GRID_SIZE_CM
if 0 <= obstacle_x < GRID_ROWS and 0 <= obstacle_y < GRID_COLS:
occupancy_grid[int(obstacle_x)][int(obstacle_y)] = 1
print(f"Obstacle at: X={obstacle_x*GRID_SIZE_CM:.0f}cm, Y={obstacle_y*GRID_SIZE_CM:.0f}cm")
# --- Map display function ---
def draw_map():
plt.clf()
plt.imshow(occupancy_grid, cmap='gray_r', origin='lower')
plt.title("Simple Occupancy Grid Map")
plt.xlabel("Y axis (grid cells)")
plt.ylabel("X axis (grid cells)")
plt.pause(0.1)
# --- Main mapping loop ---
try:
plt.ion()
fig = plt.figure()
print("Mapping started. Running for 15 seconds...")
start_time = time.time()
while time.time() - start_time < 15:
dist = get_distance()
update_map(dist, robot_x, robot_y, robot_angle)
draw_map()
if dist < 30:
print("Obstacle close! Stopping and turning...")
stop_robot()
backward(50)
time.sleep(0.5)
stop_robot()
choice = decide_direction()
if choice == "left":
turn_left(speed)
robot_angle += 45 # Rough heading update — no encoder feedback
else:
turn_right(speed)
robot_angle -= 45
time.sleep(0.8)
stop_robot()
else:
forward(speed)
# Rough position estimate based on speed and elapsed time
angle_rad = math.radians(robot_angle)
robot_x += (speed * 0.02 * math.cos(angle_rad)) / GRID_SIZE_CM
robot_y += (speed * 0.02 * math.sin(angle_rad)) / GRID_SIZE_CM
time.sleep(0.2)
print("Mapping complete. Close the plot window to exit.")
plt.ioff()
plt.show()
except KeyboardInterrupt:
print("Stopped by user.")
finally:
stop_robot()
pwm_A.stop()
pwm_B.stop()
GPIO.cleanup()
picam.stop()How the Map Math Works
Marking an obstacle:
angle_rad = math.radians(robot_angle)
obstacle_x = robot_x + (distance * math.cos(angle_rad)) / GRID_SIZE_CM
obstacle_y = robot_y + (distance * math.sin(angle_rad)) / GRID_SIZE_CMThis is basic trigonometry: given a distance and a heading angle, calculate the (x, y) position of the obstacle relative to the robot's current position. Divide by GRID_SIZE_CM to convert from real-world centimeters into grid cell units.
Estimating position while moving:
robot_x += (speed * 0.02 * math.cos(angle_rad)) / GRID_SIZE_CM
robot_y += (speed * 0.02 * math.sin(angle_rad)) / GRID_SIZE_CMThis is the weakest link in the whole system. There's no wheel encoder telling us how far the robot has actually traveled — just a rough multiplier (speed * 0.02) standing in for "distance covered in this time slice." It will drift. Over a short run it's a reasonable approximation; over a long one, the estimated position and the real position will diverge noticeably.
Updating heading after a turn:
robot_angle += 45 # or -= 45Same limitation. We don't know exactly how many degrees the robot turned — turn_left(speed) for 0.8 seconds doesn't reliably produce a 45° turn every time, since it depends on battery charge, floor friction, and motor wear. It's a placeholder that keeps the math from being completely wrong, not a precise measurement.
What Real Robot Vacuums Do Differently
This is the gap between our $50 setup and a $300 robot vacuum: encoders on every wheel (measuring exact rotation), often a gyroscope or full IMU (measuring exact turning angle), sometimes a LIDAR sensor spinning 360° instead of one fixed ultrasonic ping. All of that feeds into proper SLAM algorithms that correct for drift in real time.
We have one ultrasonic sensor and some educated guessing. The resulting map will be rough, will drift over time, and will probably misplace a few walls. But the underlying idea — sense the environment, estimate where you are, record what you've found, repeat — is identical to what's running inside that $300 vacuum. We're just doing it with a flashlight instead of their floodlight.
What We're Building Toward
This map isn't the end goal by itself — it's a building block. Once a robot has even a rough sense of "I've already been here" and "there's a wall over there," it can start making smarter decisions than "drive until something's close, then guess a direction." That's where Part 20 is headed: combining everything built so far — driving, obstacle avoidance, vision, voice, web control, and now basic mapping — into one system that runs through an actual obstacle course.
Next up: Part 20 — Phase 2 Boss Fight. Everything we've built, working together at once.