Home · Course structure · Worked example project

IE University · IRL-N3CSAI · Module 4 capstone · Worked example

Autonomous Raspberry Pi line-following car

One end-to-end build that ties the whole course together. We wire IR and camera sensors to a Raspberry Pi, read them over GPIO, drive two DC motors through an L298N H-bridge with PWM, and close the loop with a PID steering controller wrapped in a small follow / search / stop state machine. Every section pairs the reasoning with real, runnable Python and the control math behind it.

01 · Overview

The goal, the stack, and what we build

The goal is a small car that drives itself along a black line taped to the floor: it stays centred on the line, slows for curves, hunts for the line if it loses it, and stops cleanly at the end. This is the reactive-autonomy core of the Module 4 capstone — the same sense–think–act loop introduced in Session 1, now running tens of times a second on real hardware. The car is deliberately built from the cheapest reliable parts so the engineering lives in the software and tuning, not the bill of materials.

Sessions this project exercises

The worked example is the integration point for almost every session of the course:

S1 Sense–think–act loop S4 Sensors & actuators S5 PID control S9 Raspberry Pi & GPIO S10 PWM & H-bridge S11 Vision in the control loop S12 Autonomous navigation S13 Testing & debugging S14 Tuning for performance

Stack

ComputerRaspberry Pi
LanguagePython 3
GPIORPi.GPIO
Visionpicamera2 + OpenCV
Motor driverL298N H-bridge
ControlPID + state machine
Loop rate~50 Hz
DriveDifferential (2 wheels)

Hardware list

Everything connects to a single Raspberry Pi using BCM pin numbering. Motor power comes from a separate battery pack — the Pi supplies only logic-level signals (Session 10 safety rule).

Raspberry Pi (4B / Zero 2 W)
The car's brain — runs Linux, Python, the control loop and OpenCV.
5-channel IR line array
Digital reflectance sensors across the chassis floor; the primary line detector.
GPIO 5, 6, 13, 19, 26 (in)
Pi Camera Module
Optional richer line detection & curvature look-ahead via OpenCV.
CSI ribbon
L298N motor driver
Dual H-bridge: turns logic signals into motor direction + speed.
IN1 17, IN2 27, IN3 22, IN4 23
2× DC gear motors + wheels
Differential drive — steering is a speed difference between left and right.
ENA 18 (PWM), ENB 12 (PWM)
HC-SR04 ultrasonic (optional)
Front rangefinder for the stop / obstacle extension.
TRIG 20, ECHO 21
Battery pack + buck converter
Separate motor supply (6–9 V); common ground with the Pi.

Why line-following first. Line-following is the gentlest possible introduction to closed-loop autonomy: the error signal is one number (how far off-centre we are), the controller is a textbook PID, and the actuator mapping is a simple differential mix. Master it here and obstacle avoidance, vision steering, and ROS navigation are all variations on the same loop.

02 · System architecture

Sensors → controller → actuators

The car is a textbook sense–think–act pipeline. Each loop iteration the sense stage reads the IR array (and optionally a camera frame) and reduces it to a single line-position error; the think stage runs that error through a PID controller and a state machine to decide a steering correction and a base speed; the act stage mixes those into left/right motor duty cycles and writes them to the L298N over PWM.

SENSE THINK ACT IR array (5 ch) ─┐ ┌─ ENA/PWM ─ Left motor ├─> line error e ─> [ State machine ] │ Pi camera ───────┘ (px) FOLLOW / SEARCH / STOP │ L298N │ │ H-bridge v │ [ PID controller ] ├─ ENB/PWM ─ Right motor u = Kp·e + Ki·∫e │ + Kd·de/dt │ │ │ base_speed ± u ─ differential mix ─> (duty_L, duty_R) │ loop @ ~50 Hz (20 ms) ────────┘ (repeat)

Wiring

The Pi never drives a motor directly — its 3.3 V GPIO can't source the current (Session 9/10). Instead, four direction pins and two PWM "enable" pins command the L298N, which switches the separate motor battery. Sensors share a common ground with the Pi.

Raspberry Pi (BCM) L298N driver Motors GPIO17 ───────────────────────> IN1 ┐ GPIO27 ───────────────────────> IN2 ├─> OUT1/OUT2 ─────> Left DC motor GPIO18 (PWM, ENA) ─────────────> ENA ┘ GPIO22 ───────────────────────> IN3 ┐ GPIO23 ───────────────────────> IN4 ├─> OUT3/OUT4 ─────> Right DC motor GPIO12 (PWM, ENB) ─────────────> ENB ┘ GND ──────────────┬────────────> GND (common ground — required!) └──────────── battery (−) Battery pack (6–9 V +) ─────────> +12V / VS (motor supply, NOT from the Pi) IR array ──> GPIO 5,6,13,19,26 (digital in) + 3V3 + GND HC-SR04 ──> TRIG GPIO20, ECHO GPIO21 (via voltage divider on ECHO)

Safety. Always tie the Pi ground to the L298N ground, never back-feed motor voltage into a GPIO pin, and drop the 5 V HC-SR04 echo to 3.3 V with a divider. Getting this wrong is the fastest way to brown-out or kill a Pi.

03 · Sensing

Detecting the line

The whole controller acts on one number: the line's offset from centre, in the same units every cycle. We compute it two ways — a fast IR array (primary) and an OpenCV camera fallback (richer, used for the vision extension) — and both return an error e where e = 0 means perfectly centred, positive means the line is to the right, negative to the left.

IR reflectance array

Five IR sensors sit in a row under the nose of the car. Each reads 1 over the dark line and 0 over the light floor (after calibrating the threshold to the real surface — Session 13). We assign each channel a fixed position weight and take the weighted average of the channels that see the line. With weights [-2, -1, 0, +1, +2], an all-centre reading gives 0; the line drifting under the rightmost sensor gives +2.

sensors.py — IR line array
import RPi.GPIO as GPIO

IR_PINS    = [5, 6, 13, 19, 26]   # left -> right, BCM numbering
IR_WEIGHTS = [-2, -1, 0, 1, 2]    # position of each channel

def setup_ir():
    GPIO.setmode(GPIO.BCM)
    for pin in IR_PINS:
        GPIO.setup(pin, GPIO.IN)

def read_line_error():
    """Return (error, on_line). error in [-2, +2]; 0 = centred."""
    bits = [GPIO.input(pin) for pin in IR_PINS]   # 1 = sees dark line
    active = sum(bits)
    if active == 0:
        return 0.0, False                         # line lost
    weighted = sum(w * b for w, b in zip(IR_WEIGHTS, bits))
    return weighted / active, True             # centre of mass of the line

Why a weighted centre-of-mass? Two adjacent channels firing (the line straddling them) averages to a half-step like +0.5, giving the controller a smooth, sub-sensor-resolution error instead of a coarse 5-level jump. That smoothness is what lets the PID steer gently rather than wobble.

Camera fallback (OpenCV)

For the vision extension we grab a frame, crop a thin region of interest near the front of the car, threshold it to isolate the dark line, and take the centroid of the line pixels. The error is the centroid's horizontal offset from the frame centre, in pixels — the exact "reduce a rich image to one number" pattern from Session 11. We keep the ROI small so the loop stays fast.

vision.py — OpenCV line centroid
import cv2
import numpy as np
from picamera2 import Picamera2

cam = Picamera2()
cam.configure(cam.create_preview_configuration(main={"size": (320, 240)}))
cam.start()

def read_line_error_cv():
    """Return (error_px, on_line). error_px > 0 means line is to the right."""
    frame = cam.capture_array()                 # RGB, 320x240
    gray  = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
    roi   = gray[180:210, :]                    # thin strip near the front

    # dark line on light floor -> invert so the line is white
    _, mask = cv2.threshold(roi, 70, 255,
                            cv2.THRESH_BINARY_INV)
    M = cv2.moments(mask)
    if M["m00"] == 0:
        return 0.0, False                       # no line pixels -> lost

    cx = M["m10"] / M["m00"]                  # centroid x (px)
    frame_center = roi.shape[1] / 2.0          # 160 px
    return cx - frame_center, True           # offset from centre
04 · Control

The PID steering controller

The line error tells us how wrong we are; the controller decides what to do about it. A PID controller (Session 5) computes a single steering correction u from the error, and we mix that correction into the two motor speeds to turn the car back toward the line.

The PID control law u(t) = Kp·e(t) + Ki·∫ e(t) dt + Kd· de(t)/dt

Pproportional: reacts to the present error. Bigger offset → harder correction. Too much alone overshoots and oscillates.

Iintegral: accumulates past error to erase a steady bias (e.g. a slightly stronger left motor). Too much causes integral windup and a late overshoot.

Dderivative: reacts to how fast the error is changing, damping the overshoot the P term creates. Too much amplifies sensor noise into jitter.

In software the loop runs at discrete steps of length dt (here ~20 ms, so ~50 Hz), so the integral becomes a running sum and the derivative a finite difference:

Discrete form (per loop step) integral ← integral + e · dt derivative = (e − e_prev) / dt u = Kp·e + Ki·integral + Kd·derivative

Building it step by step

1

A reusable PID class with anti-windup

We clamp the accumulated integral so it can't grow without bound while the motors are saturated — the standard fix for the windup pitfall flagged in Session 5.

pid.py — discrete PID with anti-windup
import time

class PID:
    def __init__(self, kp, ki, kd, out_limit=100.0, i_limit=40.0):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.out_limit = out_limit      # clamp on the final output
        self.i_limit   = i_limit        # clamp on the integral (anti-windup)
        self.integral  = 0.0
        self.prev_e    = 0.0
        self.prev_t    = time.monotonic()

    def reset(self):
        self.integral = 0.0
        self.prev_e   = 0.0
        self.prev_t   = time.monotonic()

    def update(self, error):
        now = time.monotonic()
        dt  = max(now - self.prev_t, 1e-3)   # guard against dt = 0

        # proportional
        p = self.kp * error

        # integral, clamped to stop windup
        self.integral += error * dt
        self.integral = max(-self.i_limit, min(self.i_limit, self.integral))
        i = self.ki * self.integral

        # derivative on the error
        d = self.kd * (error - self.prev_e) / dt

        u = p + i + d
        u = max(-self.out_limit, min(self.out_limit, u))   # clamp output

        self.prev_e = error
        self.prev_t = now
        return u
2

Mix the correction into the two motors

With a differential drive, steering is a speed difference (Session 5). We start both wheels at a base speed, then add the PID output to one side and subtract it from the other. A positive u (line is to the right) speeds the left wheel and slows the right, swinging the nose right toward the line.

drive.py — differential mix & PWM output
import RPi.GPIO as GPIO

IN1, IN2, ENA = 17, 27, 18      # left motor
IN3, IN4, ENB = 22, 23, 12      # right motor
PWM_HZ        = 1000                # 1 kHz switching
DEADBAND      = 18.0                # min duty that actually turns the wheel

def setup_motors():
    GPIO.setmode(GPIO.BCM)
    for p in (IN1, IN2, IN3, IN4, ENA, ENB):
        GPIO.setup(p, GPIO.OUT)
    left  = GPIO.PWM(ENA, PWM_HZ); left.start(0)
    right = GPIO.PWM(ENB, PWM_HZ); right.start(0)
    return left, right

def _clamp_duty(duty):
    duty = max(0.0, min(100.0, duty))
    # jump over the deadband so a small command still moves the wheel
    return 0.0 if duty < 1.0 else DEADBAND + duty * (100 - DEADBAND) / 100

def drive(left_pwm, right_pwm, base, u):
    """base = forward speed (0-100); u = PID steering correction."""
    duty_l = _clamp_duty(base + u)        # u > 0 -> steer right
    duty_r = _clamp_duty(base - u)

    # both wheels forward (set direction pins once)
    GPIO.output(IN1, GPIO.HIGH); GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH); GPIO.output(IN4, GPIO.LOW)
    left_pwm.ChangeDutyCycle(duty_l)
    right_pwm.ChangeDutyCycle(duty_r)
PWM duty → motor speed V_avg = duty × V_supply

Pulse-width modulation switches the enable pin on/off at 1 kHz; the duty cycle sets the average voltage. Worked example: 75% duty on a 6 V pack delivers ≈4.5 V average, so the motor runs at roughly three-quarter speed. Below the deadband duty (≈18% here) static friction wins and the wheel only buzzes — so _clamp_duty maps any non-zero command above that floor (Session 10).

05 · Decision logic

The state machine & main loop

A pure PID line-follower has one fatal flaw: the moment the line disappears (a gap, a sharp corner, the end of the track) the error reads 0 and the car sails straight off. The fix from Session 12 is to add a little state. We wrap the controller in a three-state machine:

line seen ┌───────────────────────────────┐ v │ ┌─────────┐ line lost ┌─────────┐ line found (back to) │ FOLLOW │ ────────────> │ SEARCH │ ─────────────> FOLLOW │ run PID │ │ pivot │ └─────────┘ └─────────┘ │ │ search timeout │ end / obstacle v └───────────────────> ┌─────────┐ │ STOP │ motors = 0 └─────────┘
main.py — state machine + 50 Hz control loop
import time
import RPi.GPIO as GPIO
from pid     import PID
from sensors import setup_ir, read_line_error
from drive   import setup_motors, drive

# --- tuned constants (see the results table) ---
KP, KI, KD   = 22.0, 0.4, 9.0
BASE_SPEED   = 55.0          # forward duty on a straight
CURVE_SLOW   = 0.6           # speed multiplier when |error| is large
LOOP_DT      = 0.02          # 20 ms -> ~50 Hz
SEARCH_TIME  = 1.5           # seconds to hunt before giving up
SEARCH_TURN  = 45.0          # pivot duty while searching

FOLLOW, SEARCH, STOP = "FOLLOW", "SEARCH", "STOP"

def main():
    setup_ir()
    left, right = setup_motors()
    pid = PID(KP, KI, KD)

    state          = FOLLOW
    last_dir       = 1.0     # +1 line was last to the right, -1 to the left
    search_started = 0.0

    try:
        while True:
            t0 = time.monotonic()
            error, on_line = read_line_error()

            # ---- THINK: state transitions ----
            if state == FOLLOW:
                if on_line:
                    last_dir = 1.0 if error >= 0 else -1.0
                else:
                    state, search_started = SEARCH, t0
                    pid.reset()                 # drop stale integral

            elif state == SEARCH:
                if on_line:
                    state = FOLLOW
                elif t0 - search_started > SEARCH_TIME:
                    state = STOP

            # ---- ACT ----
            if state == FOLLOW:
                u     = pid.update(error)
                speed = BASE_SPEED * (CURVE_SLOW if abs(error) > 1.0 else 1.0)
                drive(left, right, speed, u)
            elif state == SEARCH:
                # pivot in place toward where the line vanished
                drive(left, right, 0.0, last_dir * SEARCH_TURN)
            else:  # STOP
                drive(left, right, 0.0, 0.0)
                break

            # ---- keep a steady loop rate ----
            time.sleep(max(0.0, LOOP_DT - (time.monotonic() - t0)))
    finally:
        GPIO.cleanup()                          # always release the pins

if __name__ == "__main__":
    main()

Three details that matter. (1) We pid.reset() on entering SEARCH so a stale integral doesn't yank the car when the line returns. (2) The fixed LOOP_DT sleep keeps dt roughly constant, which keeps the integral and derivative terms meaningful. (3) GPIO.cleanup() in a finally block guarantees the motors stop even if the program crashes — a safety habit from Session 13.

06 · Results

Behaviour & tuning

With the tuned gains above (Kp=22, Ki=0.4, Kd=9) the car tracks a 2 cm line at ≈0.3 m/s, holds the centre to within one sensor width on straights, slows automatically into curves, and recovers from a lost line by pivoting back onto it. The behaviour we observed across runs:

PID tuning table

How we got there — the classic "raise Kp until it oscillates, add Kd to damp, add just enough Ki" procedure from Session 5, recorded run by run:

KpKiKdObserved behaviourVerdict
500Sluggish; drifts off on any curve, never really centres.Too soft
1500Follows straights, but overshoots and weaves (S-ing) on curves.P too high alone
3000Fast reaction but sustained oscillation — visibly unstable.Oscillates
2206Derivative damps the weave; smooth on curves, slight residual bias.Almost
2209Crisp, well-damped tracking; tiny steady drift on long arcs.Add I
220.49Steady drift gone; centres precisely, recovers cleanly.Tuned ✓
222.09Integral windup — late overshoot after a long curve.I too high

Reading the table. P alone trades sluggishness for oscillation; D buys back the stability so you can keep P high; a small I erases the last steady-state offset without inducing windup. This is exactly the intuition the live PID tuner demo lets you feel by sliding the gains.

07 · Learning outcomes

Mapping to course outcomes

This single build hits the Module 4 learning outcomes and reaches back into Modules 1–2:

08 · Going further

Extensions

Once the line-follower is solid, every harder behaviour is a variation on the same loop:

Obstacle avoidance

Add the HC-SR04 to the loop: if distance < 20 cm, override the PID with a "back up and turn" escape before resuming FOLLOW. This is the obstacle-avoidance behaviour of the Session 12 demo bolted onto the line-follower.

Pure-vision steering

Drop the IR array and feed read_line_error_cv() straight into the same PID. Add curvature look-ahead by sampling two ROIs (near + far) to slow earlier for sharp bends — the Session 11 vision-in-the-loop idea.

Pepper / NLP control

Wire a Module 3 intent classifier to the car: spoken commands ("follow the line", "stop", "search") become state-machine transitions, turning the NLP pipeline into a voice front-end for the controller.

Move to ROS

Re-cast sense / think / act as ROS nodes publishing on /line_error and /cmd_vel. You gain logging, visualization (rqt, rosbag) and a clean path toward deliberative, map-based navigation.

09 · References

References

Diwakar Vaish · Python Robotics Projects · Packt, 2018 · ISBN 9781788832922

The course companion text. Raspberry Pi setup, GPIO, motor control via H-bridge, sensor interfacing and reactive autonomy in Python — the exact stack this project uses. Publisher preview ↗

John J. Craig · Introduction to Robotics: Mechanics and Control · 3rd ed. · Pearson, 2004

The compulsory text. Ch. 9 (linear control of manipulators) is the theory behind the PID feedback law and stability conditions used here.

RPi.GPIO · Python GPIO library documentation

Reference for GPIO.setup, GPIO.PWM, ChangeDutyCycle and GPIO.cleanup as used throughout the code.

OpenCV · picamera2 · library documentation

Reference for frame capture, cvtColor, threshold and moments used in the camera line-detection fallback.

Introduction to Robotics Lab (IRL-N3CSAI) · IE University · 2025–26

The official course syllabus this project is grounded in. Open the syllabus (PDF) ↗ · full course structure.