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 存有人的判断结果。

展开原代码第 1–26 行
   1  #include <Servo.h>
   2  
   3  // Lab 4 - Match the wiring diagram provided by the student.
   4  const byte TRIG_PIN = 8;
   5  const byte ECHO_PIN = 7;
   6  const byte SERVO_PIN = 9;
   7  
   8  // Adjust these to suit the actual door mechanism.
   9  const int CLOSED_ANGLE = 0;
  10  const int OPEN_ANGLE = 90;
  11  const float OPEN_DISTANCE_CM = 5.0f;
  12  const float LEAVE_DISTANCE_CM = 10.0f;  // Hysteresis
  13  const unsigned long HOLD_OPEN_MS = 3000UL;
  14  const unsigned long SERVO_STEP_MS = 15UL;
  15  const unsigned long PING_PERIOD_US = 60000UL;
  16  const unsigned long ECHO_TIMEOUT_US = 30000UL;
  17  
  18  Servo doorServo;
  19  
  20  enum DoorState { CLOSED, OPENING, OPEN, CLOSING };
  21  DoorState doorState = CLOSED;
  22  int currentAngle = CLOSED_ANGLE;
  23  unsigned long lastServoStep = 0;
  24  unsigned long lastPersonSeen = 0;
  25  bool personPresent = false;
  26  

第 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=5b=5,单个 180 的尖峰就不会留在输出里。

展开原代码第 27–46 行
  27  enum PingState { PING_IDLE, TRIG_HIGH, WAIT_ECHO_START, WAIT_ECHO_END };
  28  PingState pingState = PING_IDLE;
  29  unsigned long pingStartedUs = 0;
  30  unsigned long lastPingUs = 0;
  31  unsigned long echoStartedUs = 0;
  32  
  33  // 3-sample median plus exponential smoothing filter.
  34  float distanceSamples[3] = {350.0f, 350.0f, 350.0f};
  35  byte sampleIndex = 0;
  36  byte sampleCount = 0;
  37  float filteredDistance = 350.0f;
  38  bool filterReady = false;
  39  
  40  float median3(float a, float b, float c) {
  41    if (a > b) { float t = a; a = b; b = t; }
  42    if (b > c) { float t = b; b = c; c = t; }
  43    if (a > b) { float t = a; a = b; b = t; }
  44    return b;
  45  }
  46  

第 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.652.25+4.40=6.65 cm。再夹到 2–350 cm。
  4. Serial.print(filteredDistance, 2) 打印两位小数,配合前后的标签和单位形成 Distance: 6.65 cm。然后回到 PING_IDLE。打印随完成的测距事件发生,Tutorial 2 原题要求每个循环打印。
展开原代码第 47–76 行
  47  void finishDistanceMeasurement(float measuredCm) {
  48    measuredCm = constrain(measuredCm, 2.0f, 350.0f);
  49    distanceSamples[sampleIndex] = measuredCm;
  50    sampleIndex = (sampleIndex + 1) % 3;
  51    if (sampleCount < 3) sampleCount++;
  52  
  53    float medianValue;
  54    if (sampleCount == 1) {
  55      medianValue = distanceSamples[0];
  56    } else if (sampleCount == 2) {
  57      medianValue = (distanceSamples[0] + distanceSamples[1]) / 2.0f;
  58    } else {
  59      medianValue = median3(distanceSamples[0], distanceSamples[1], distanceSamples[2]);
  60    }
  61  
  62    if (!filterReady) {
  63      filteredDistance = medianValue;
  64      filterReady = true;
  65    } else {
  66      filteredDistance = 0.45f * medianValue + 0.55f * filteredDistance;
  67    }
  68    filteredDistance = constrain(filteredDistance, 2.0f, 350.0f);
  69  
  70    // One formatted output per measurement cycle; printing every CPU loop
  71    // would flood Serial and disturb time-critical Echo measurements.
  72    Serial.print("Distance: ");
  73    Serial.print(filteredDistance, 2);
  74    Serial.println(" cm");
  75    pingState = PING_IDLE;
  76  }

第 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 给出往返时间。343343 m/s 换成 0.03430.0343 cm/μs,乘脉宽得到来回路程,再除 2 得单程。例如 292 μs,292×0.0343/2=5.008292\times0.0343/2=5.008 cm。

这里每到一个 case 只查看当前电平或经过时间,立即结束函数,舵机逻辑便能穿插执行。轮询的时间精度取决于循环频率。无回波按 350 cm 参与滤波,所以连续超时会把距离逐步拉向远处,最终可解除「有人」。

展开原代码第 77–118 行
  77  
  78  // Non-blocking HC-SR04 timing; no pulseIn() or delay().
  79  void updateUltrasonic() {
  80    unsigned long nowUs = micros();
  81    switch (pingState) {
  82      case PING_IDLE:
  83        if ((unsigned long)(nowUs - lastPingUs) >= PING_PERIOD_US) {
  84          digitalWrite(TRIG_PIN, HIGH);
  85          pingStartedUs = nowUs;
  86          lastPingUs = nowUs;
  87          pingState = TRIG_HIGH;
  88        }
  89        break;
  90  
  91      case TRIG_HIGH:
  92        if ((unsigned long)(nowUs - pingStartedUs) >= 12UL) {
  93          digitalWrite(TRIG_PIN, LOW);
  94          pingState = WAIT_ECHO_START;
  95        }
  96        break;
  97  
  98      case WAIT_ECHO_START:
  99        if (digitalRead(ECHO_PIN) == HIGH) {
 100          echoStartedUs = nowUs;
 101          pingState = WAIT_ECHO_END;
 102        } else if ((unsigned long)(nowUs - pingStartedUs) > ECHO_TIMEOUT_US) {
 103          finishDistanceMeasurement(350.0f);
 104        }
 105        break;
 106  
 107      case WAIT_ECHO_END:
 108        if (digitalRead(ECHO_PIN) == LOW) {
 109          unsigned long pulseLengthUs = nowUs - echoStartedUs;
 110          float cm = pulseLengthUs * 0.0343f / 2.0f;
 111          finishDistanceMeasurement(cm);
 112        } else if ((unsigned long)(nowUs - echoStartedUs) > ECHO_TIMEOUT_US) {
 113          finishDistanceMeasurement(350.0f);
 114        }
 115        break;
 116    }
 117  }
 118  

第 119–130 行 · 两个距离阈值怎么记住有人

滤波距离 ≤5 cm 设为 true,≥10 cm 设为 false,两者之间保留原值。例如距离按 12、4、7、11 cm 变化,判断依次是无人、有人、有人、无人。每轮判断有人都更新 lastPersonSeen。离开后该时间不再更新,因此可以用当前时刻减它,算等待了多久。

展开原代码第 119–130 行
 119  void updateDoor() {
 120    unsigned long nowMs = millis();
 121  
 122    // Two thresholds prevent rapid opening/closing near the boundary.
 123    if (filteredDistance <= OPEN_DISTANCE_CM) {
 124      personPresent = true;
 125    } else if (filteredDistance >= LEAVE_DISTANCE_CM) {
 126      personPresent = false;
 127    }
 128  
 129    if (personPresent) lastPersonSeen = nowMs;
 130  

第 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°,从当前角度往回开。

展开原代码第 131–165 行
 131    switch (doorState) {
 132      case CLOSED:
 133        if (personPresent) doorState = OPENING;
 134        break;
 135      case OPENING:
 136        if (!personPresent && (nowMs - lastPersonSeen >= HOLD_OPEN_MS)) {
 137          doorState = CLOSING;
 138        } else if (currentAngle == OPEN_ANGLE) {
 139          doorState = OPEN;
 140        }
 141        break;
 142      case OPEN:
 143        if (!personPresent && (nowMs - lastPersonSeen >= HOLD_OPEN_MS)) {
 144          doorState = CLOSING;
 145        }
 146        break;
 147      case CLOSING:
 148        if (personPresent) {
 149          doorState = OPENING;
 150        } else if (currentAngle == CLOSED_ANGLE) {
 151          doorState = CLOSED;
 152        }
 153        break;
 154    }
 155  
 156    if (nowMs - lastServoStep >= SERVO_STEP_MS) {
 157      lastServoStep = nowMs;
 158      int target = (doorState == OPENING || doorState == OPEN)
 159                       ? OPEN_ANGLE : CLOSED_ANGLE;
 160      if (currentAngle < target) currentAngle++;
 161      if (currentAngle > target) currentAngle--;
 162      doorServo.write(currentAngle);
 163    }
 164  }
 165  

第 166–180 行 · 开机与主循环

开机设 Trig 输出、Echo 输入,先把 Trig 置 LOW。串口 115200。用 attach 关联 D9 后写 0°。给 lastPingUs 一个早 60000 μs 的时间,第一次循环便可触发测量。之后,每轮依次执行 updateUltrasonic()、updateDoor(),上面两个状态机不断各自前进一步。

展开原代码第 166–180 行
 166  void setup() {
 167    pinMode(TRIG_PIN, OUTPUT);
 168    pinMode(ECHO_PIN, INPUT);
 169    digitalWrite(TRIG_PIN, LOW);
 170    Serial.begin(115200);
 171    doorServo.attach(SERVO_PIN);
 172    doorServo.write(CLOSED_ANGLE);
 173    lastPersonSeen = millis();
 174    lastPingUs = micros() - PING_PERIOD_US;
 175  }
 176  
 177  void loop() {
 178    updateUltrasonic();
 179    updateDoor();
 180  }