ESP8266 (ESP-WROOM-02) に距離センサー(HC-SR04)を繋いで、LPFで滑らかな値出力にしてみた

出力値 = a * 前回の出力値 ＋ (1-a) * センサの値

※ aは係数で0.8～0.95とかを指定します。小さいほど反応が良くなり、大きいほど滑らかになります。相反します。

int Echo = 14;  // Echoピン
int Trig = 15;  // Trigピン

float dstCMb = 0;   // 一回前の距離値

void setup() {
  // シリアル通信を開始
  Serial.begin(115200);
  Serial.println("Serial start.");

  // ピン設定
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
}

void loop() {

  // trigger
  digitalWrite(Trig, HIGH);
  delay(20); // msec
  digitalWrite(Trig, LOW);

  // echo
  int intervalUS = pulseIn(Echo, HIGH) / 2;   // 超音波の往復の時間を2で割って片道の時間（単位：μsec）にする

  // 時間を距離に変換
  float dstM = (float)intervalUS * 340.0 / 1000.0 / 1000.0;  // センサー出力の値（単位：m）  音速：340m/s(15℃)
  float dstCMs = dstM * 100.0;  // センサー出力の値（単位：cm）
  float dstCM;

  // ローパスフィルターで滑らかにする / 外れ値を除外する
  if (dstCMs <= 300.0) {                   // 3mを超えたら外れ値とする
    dstCM = 0.95 * dstCMb + 0.05 * dstCMs; // ローパスフィルター適用
    dstCMb = dstCM;                        // 前回値の保存
    Serial.println(dstCM);                // 距離を出力（単位：cm）
  }else{
    Serial.println(dstCMb);               // 外れ値の場合は前回値を出力
  }

}