一、这台车要同时干两件事，得先排优先级

先把任务拆清楚，否则代码会写成一团：

优先级怎么写进代码：避障先判、追踪后判；避障一旦触发，追踪这一轮直接跳过。

每一轮循环：
  1. 读前方距离
  2. 距离 < 20cm ?  -> 执行避障动作，continue（本轮不追踪）
  3. 否则 -> 找色块，算出 cx -> 比例控制转向

顺序反过来会出什么事：如果先追踪后避障，车会先朝目标猛冲一段再停车——每次都在离障碍 20cm 以内才反应，刹车距离全靠运气。安全逻辑永远写在最前面。

二、超声波怎么把【距离】变成【一个数】

HC-SR04 面板上两只"眼睛"：一只发射、一只接收。工作流程只有一句话——发一声音，听它什么时候回来：

① 给 Trig 脚一个至少 10 微秒的高电平脉冲  ->  "喊一声"
② 模块把 40kHz 的超声波打出去
③ 声波撞到障碍物反射回来
④ 模块把 Echo 脚拉高，高电平持续的时间 = 声波往返的总时间
⑤ 程序量出这段脉宽，乘声速再除以 2 = 距离

公式与数字（这一节请把数算清楚，后面调参全靠它）：

• 20°C 空气中的声速约 343 m/s = 0.0343 cm/μs
• 声波是往返的，所以单程要 除以 2
• 合并起来：距离(cm) = Echo 脉宽(μs) ÷ 58

验算一遍这个 58 的来历：1cm 往返 = 2cm ÷ 0.0343 cm/μs ≈ 58.3 μs。记住了它，就能反问【我的读数为什么跳】——温度每变化 10°C，声速大约变 0.6%，对 1 米处的测距来说就是约 6 毫米的漂移。所以超声波本来就不是精密仪器，别指望毫米级。

三、⚠️ 本节最危险的一条：Echo 脚的 5V 必须分压

HC-SR04 是 5V 器件：VCC 要 5V 供电，而它的 Echo 脚输出的高电平也是 5V。树莓派的 GPIO 是 3.3V 逻辑，直接把 5V 灌进 GPIO = 打坏那根引脚（轻则该脚报废、重则连带芯片）。

正解是分压：Echo 脚先过一个分压网络，把 5V 降到 3.3V 以下再进 GPIO。

验算：节点电压 = 5V × R下 ÷ (R上 + R下)。1kΩ 在上、2kΩ 在下 得到 3.33V，正好压在 3.3V 逻辑的容许线上（略高但可接受，实际器件通常接 1k/2k 就是这个组合）。

四、gpiozero 的 DistanceSensor：一行代码替你算完

gpiozero 把上面那一整套【发脉冲→量宽度→除 58】封装成了一个类：

from gpiozero import DistanceSensor

sensor = DistanceSensor(echo=24, trigger=23,      # 注意顺序是 echo 在前
                        max_distance=4.0,          # 超过 4m 就算【超量程】
                        queue_len=5)               # 取最近 5 次的中值，抹平抖动

sensor.value 是个妙用：value == 1.0 就说明【没有任何回波】——比数值本身更有信息量，因为超量程和"真的很远"是两回事。

sudo apt update
sudo apt install -y python3-gpiozero python3-lgpio

接线（BCM 编号）：

五、从【偏差】到【转向】：开关式 vs 比例控制

这是本节真正要讲透的一段。同一个偏差，有两种用它的方式：

开关式（if-else）——最简单，也最难看：

if cx < 160:
    turn_left_at_full_speed()
elif cx > 160:
    turn_right_at_full_speed()

问题在于它只有【全力】和【不转】两档。结果就是：目标偏一点点 → 全力猛转 → 一下转过头 → 再全力猛转回来。画面上看就是车在左右剧烈摆动，像喝醉了一样。

比例控制（P 控制）——把偏差按比例变成转向力度：

error = (cx - width / 2) / (width / 2)   # 归一化到 -1.0(最左) ~ +1.0(最右)
turn  = kp * error                       # kp 是比例系数，从 0.8 左右试起

偏差越大转得越猛、偏差小就轻轻修正——车就稳了。这就是比例控制的全部思想：输出与误差成正比，而不是与误差的正负号成正比。

再加上一个死区（dead zone），专治【正对目标还在微抖】：

if abs(cx - width / 2) < 20:      # 中心 ±20 像素以内视为【已经对准】
    turn = 0.0                    # 不再修正，免得原地画抖

转向怎么落到轮子上——差速转向：

这里有个极容易搞反的地方，先把这张表刻住——左轮转得快，车向右拐（左侧行程更长，车头被推向右；这跟"踩左边油门"的直觉相反，却是刚体运动学的必然结果——想右转就加快左边的轮子）：

于是约定 turn > 0 表示【向右修正】，代码就写成：

left_speed  = base + turn       # turn>0 -> 左轮加速
right_speed = base - turn       #        -> 右轮减速，合起来就是右转

验算一次：画面宽 320，目标出现在 cx = 80（偏左）。error = (80-160)/160 = -0.5；取 kp = 0.8 → turn = -0.4（负 = 向左修正）；基速 base = 0.5 → 左轮 = 0.5 + (-0.4) = 0.1、右轮 = 0.5 - (-0.4) = 0.9。右轮 0.9、左轮 0.1 → 右轮快 → 大幅左转，方向和力度都对。

left_speed  = max(0.0, min(1.0, base - turn))
right_speed = max(0.0, min(1.0, base + turn))

六、动手 Demo：先验证，再接电机

分层验证的老规矩：先只跑传感器、确认读数合理，再接电机。

第一层：超声波读数

from gpiozero import DistanceSensor
from time import sleep

sensor = DistanceSensor(echo=24, trigger=23, max_distance=4.0, queue_len=5)
print("pin factory =", sensor.pin_factory)   # 必须不是 MockFactory

try:
    while True:
        if sensor.value == 1.0:
            print("超量程：前方 4 米内没有回波")
        else:
            print("distance = {:.1f} cm".format(sensor.distance * 100))
        sleep(0.25)                          # 4Hz，留出 23ms 的物理余量
except KeyboardInterrupt:
    pass
finally:
    sensor.close()

看到什么算成功：

pin factory = LGPIOFactory
distance = 42.3 cm
distance = 41.8 cm        <- 手掌慢慢靠近，数字连续变小
distance = 12.6 cm
超量程：前方 4 米内没有回波   <- 手拿开、探头朝空处

读结果：数字连续变化、靠近时变小就对；若是乱跳（比如 40 → 300 → 15）通常有两个原因：① 探测面是斜的、软材质（吸音）——换个硬纸板对着试；② queue_len 太小，改成 5 或 7。若 pin factory 是 MockFactory，先去装 python3-lgpio，别怀疑传感器。

第二层：接上电机，跑完整逻辑

from gpiozero import DistanceSensor, Motor
import cv2
import numpy as np
from time import sleep

# --- 执行端：15-2 的电机对象（引脚按你的接线改） ---
left  = Motor(forward=17, backward=18, enable=12, pwm=True)
right = Motor(forward=22, backward=23, enable=13, pwm=True)

# --- 感知端一：超声波 ---
sonar = DistanceSensor(echo=24, trigger=25, max_distance=4.0, queue_len=5)

# --- 感知端二：摄像头 ---
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
W = 320

lower = np.array([40, 70, 70])
upper = np.array([80, 255, 255])
kernel = np.ones((5, 5), np.uint8)


def clamp(v):
    return max(0.0, min(1.0, v))


def find_target_cx(frame):
    """返回目标中心横坐标；找不到返回 None（逻辑与 16-1 一致）"""
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, lower, upper)
    mask = cv2.erode(mask, kernel, iterations=1)
    mask = cv2.dilate(mask, kernel, iterations=2)
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL,
                                   cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None
    biggest = max(contours, key=cv2.contourArea)
    if cv2.contourArea(biggest) <= 500:
        return None
    x, y, w, h = cv2.boundingRect(biggest)
    return x + w // 2


def drive(left_speed, right_speed):
    left.value  = left_speed
    right.value = right_speed


def stop():
    left.stop()
    right.stop()


try:
    while True:
        # ===== 1. 避障优先：一票否决 =====
        if sonar.distance < 0.20:            # 20cm 以内
            stop()
            sleep(0.2)
            drive(-0.5, -0.5)                # 退一点
            sleep(0.3)
            drive(0.5, -0.5)                 # 原地转开
            sleep(0.3)
            stop()
            continue                         # 本轮不追踪

        # ===== 2. 追踪：比例控制 =====
        ok, frame = cap.read()
        if not ok:
            break
        cx = find_target_cx(frame)

        if cx is None:
            stop()                           # 看不见目标 -> 停住等
        else:
            error = (cx - W / 2) / (W / 2)   # -1.0 ~ +1.0
            if abs(cx - W / 2) < 20:         # 死区：对准了就别修
                turn = 0.0
            else:
                turn = 0.8 * error           # kp = 0.8，turn>0 表示向右修正
            base = 0.5
            drive(clamp(base + turn), clamp(base - turn))   # 左快=右转

        sleep(0.05)
except KeyboardInterrupt:
    pass
finally:
    stop()                                   # ★ 绝不能省
    sonar.close()
    cap.release()
    cv2.destroyAllWindows()

看到什么算成功：把绿色物体放在车前约 1 米处并缓慢左右移动 → 车头跟着摆；拿手挡在车前方 20cm 内 → 车先停、再后退、再转开，然后继续找目标。

读结果：

• 车在目标正前方轻微左右摆 → 死区 20 像素太大或 kp 太大，把 kp 降到 0.5、死区提到 30 试。
• 车朝目标冲过去但没到就停 → 那个"目标"其实是把别的东西误判成了绿色，去看掩膜（16-1 的调试窗口）。
• 完全不动 → 先查 sensor.pin_factory 与 min(1.0, ...) 之后的 base 值；0.5 以下可能推不动（15-2 的静摩擦）。
• finally: stop() 不是可选项：没有它，Ctrl+C 之后车会以最后的速度一直跑下去——这是 15-4 立下的规矩，本节照样适用。

七、30 秒实验：看一眼 queue_len 到底在干什么

把 queue_len=5 改成 queue_len=1，重跑第一层 Demo，手掌静止不动，盯着读数：

• queue_len=1：数字后两位疯跳（42.3 / 39.8 / 44.1 / 41.0），幅度能到 ±3cm；
• queue_len=5：读数明显平稳，跳动收到 ±1cm 内。

原因：queue_len 让 gpiozero 连续采 5 次、取中值再给你——而树莓派跑的是非实时系统，一次读数可能被系统调度打断，单次读数的抖动主要来自操作系统而不是传感器。所以两个数都有用：要平稳用 5，要最快反应（比如车开得很快）用 1 或 2。

八、常见误区

1. 【HC-SR04 的 Echo 直接接 GPIO 就行】——它输出的是 5V 高电平，直接进 3.3V 的 GPIO 会打坏引脚。必须 1k/2k 分压（或改用 3.3V 原生模块）。这是本节唯一不可妥协的一条。
2. 【超声波能测到毫米级】——声速随温度漂；20°C 到 0°C 之间就有约 3% 的差。±1~2cm 是它的正常水平，别拿它做精密测量。
3. 【避障就是"到距离就停"】——原地停死会把车卡在那儿，之后永远出不来。正路是 停 → 后退 → 转开 → 继续，四步一个循环。
4. 【追踪用 if-else 就够，反正最后也能到】——开关式控制只有【全力】和【不转】两档，在现场的表现就是车左右剧烈摆动。要稳就必须用比例控制。
5. 【转向得靠舵机，那才叫自动驾驶】——差速转向（左右轮转速差）是小车底盘的常规做法，结构更简单、无额外舵机；只有"前轮转向"的车型才需要舵机打方向。
6. 【读一次 distance 就是一个可信的数】——单次读数受操作系统调度影响会跳。用 queue_len 平滑，并接受"它是便宜的传感器"这个事实。
7. 【sensor.value 和 sensor.distance 差不多】——value 是归一化到 0~1 的值，distance 是米；而且 value == 1.0 表示【超量程没回波】，这个判断只能靠 value 做。

• ☐ 下一节（16-3 车道自动检测系统）

相关笔记

• OpenCV 色彩检测与追踪 —— 本节 find_target_cx() 用的就是那一套流水线
• 树莓派的直流电机控制 —— 执行端：Motor 对象、speed 0~1、起步阈值
• 打造树莓派 WiFi 遥控视频车 —— 电源铁律与 finally: stop() 的出处
• Python 的 GPIO 模块 —— gpiozero 与 pin factory（在 Pi 5 上必须是 LGPIOFactory）
• 模拟输入 —— 对照：超声波走数字接口，所以树莓派没有 ADC 也不影响它
• 🍓 树莓派 · 索引 —— 全课路线图