# Lab 4 · 超声测距与自动门

信号线是 Trig → D8、Echo → D7、舵机控制 → D9，共用 GND。信号链是触发回波 → 算出距离 → 滤波 → 判断人在不在 → 更新舵机角度。对照 Tutorial 2 第 1 页，先理解距离任务，再用下面的两个状态机跟代码。

## 第 1–26 行 · 自动门的接线、参数和四个状态

`Servo.h` 产生舵机控制脉冲。`CLOSED_ANGLE = 0`、`OPEN_ANGLE = 90` 是模型的目标角。开门距离 5 cm，离开距离 10 cm，保持 3000 ms，每隔 15 ms 走 1°。超声每 60000 μs 尝试一次，超时设 30000 μs。代码里 `f` 表示小数是浮点类型，`UL` 表示非负长整数。

`DoorState` 记录关、正在开、开、正在关，默认 CLOSED。角度也从 0 开始。`lastServoStep` 记上次改角时刻，`lastPersonSeen` 记最后检测到近物体的时刻，`personPresent` 存有人的判断结果。

## 第 27–46 行 · 回波状态和三数中位数

回波过程按 `PING_IDLE → TRIG_HIGH → WAIT_ECHO_START → WAIT_ECHO_END` 运转。三个时间戳分别记发出触发、上次触发、Echo 开始。距离数组留三格，`sampleIndex` 写入位置，`sampleCount` 记录已填格数，`filterReady` 表示历史滤波结果是否可用。

`median3(a,b,c)` 用三次比较交换排序。拿 4、180、5 走一遍。4≤180，第一次不换。180>5，第二次把它们交换成 4、5、180。4≤5，第三次不换。排序后的中间值 $b=5$，单个 180 的尖峰就不会留在输出里。

## 第 47–76 行 · 每次测距结束才更新滤波并输出

1. 把送入值夹到 2–350 cm，再放进三格数组。`(sampleIndex+1)%3` 让下标按 0、1、2、0 循环，计数最高 3。
2. 只有一条时直接用它，两条时取均值，三条时用上面的中位数函数。因此启动阶段也有可用值。
3. 第一条直接作为历史值。从第二条起按「本轮中值×0.45 + 上轮滤波值×0.55」更新。例如 4、180、5 cm 的中位数为 5，若上轮值为 8 cm，结果 $2.25+4.40=6.65$ cm。再夹到 2–350 cm。
4. `Serial.print(filteredDistance, 2)` 打印两位小数，配合前后的标签和单位形成 `Distance: 6.65 cm`。然后回到 `PING_IDLE`。打印随完成的测距事件发生，Tutorial 2 原题要求每个循环打印。

## 第 77–118 行 · 每圈只查看一个回波步骤

| 状态 | 执行到这段时做什么 |
|---|---|
| PING_IDLE | 从上次触发起满 60000 μs，就拉高 Trig，记时间并换状态 |
| TRIG_HIGH | 高电平持续至少 12 μs 后拉低 Trig，等 Echo 变高 |
| WAIT_ECHO_START | Echo 高时记录开始时刻，换到下一状态。等不到则把 350 cm 送进滤波 |
| WAIT_ECHO_END | Echo 变低时用本次时刻减开始时刻，再换成距离。高太久也送 350 cm |

`nowUs - echoStartedUs` 给出往返时间。$343$ m/s 换成 $0.0343$ cm/μs，乘脉宽得到来回路程，再除 2 得单程。例如 292 μs，$292\times0.0343/2=5.008$ cm。

这里每到一个 `case` 只查看当前电平或经过时间，立即结束函数，舵机逻辑便能穿插执行。轮询的时间精度取决于循环频率。无回波按 350 cm 参与滤波，所以连续超时会把距离逐步拉向远处，最终可解除「有人」。

## 第 119–130 行 · 两个距离阈值怎么记住有人

滤波距离 ≤5 cm 设为 true，≥10 cm 设为 false，两者之间保留原值。例如距离按 12、4、7、11 cm 变化，判断依次是无人、有人、有人、无人。每轮判断有人都更新 `lastPersonSeen`。离开后该时间不再更新，因此可以用当前时刻减它，算等待了多久。

## 第 131–165 行 · 开、关、反向都靠状态机

- CLOSED 遇到人转 OPENING。
- OPENING 到开门角后转 OPEN。若已经无人且保持时间到，转 CLOSING。
- OPEN 保持 90°。无人且从最后检测起过了 3000 ms，转 CLOSING。
- CLOSING 到 0° 后转 CLOSED。中途又检测到人，就转 OPENING。

到步进时刻先更新时间，然后按当前状态选目标角。OPENING 和 OPEN 的目标是 90°，另外两个是 0°。比较当前角度，少则加一，多则减一，再用 `doorServo.write` 发角度指令，90 步按 15 ms 估算约 1.35 s。

**走一遍。** 在 1000 ms 检测到人开始开门，约 2350 ms 到 90°。最后检测到人的时刻为 3000 ms，则最快约 6000 ms 开始关。若关到约 60° 时又检测到人，目标立刻切回 90°，从当前角度往回开。

## 第 166–180 行 · 开机与主循环

开机设 Trig 输出、Echo 输入，先把 Trig 置 LOW。串口 115200。用 `attach` 关联 D9 后写 0°。给 `lastPingUs` 一个早 60000 μs 的时间，第一次循环便可触发测量。之后，每轮依次执行 `updateUltrasonic()`、`updateDoor()`，上面两个状态机不断各自前进一步。
