Ultrasonic HC-SR04 with Median Filter

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

#define TRIG 5
#define ECHO 18
long measure() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long d = pulseIn(ECHO, HIGH, 30000);
  return d;
}
long median3(long a, long b, long c) {
  if (a > b) std::swap(a, b);
  if (b > c) std::swap(b, c);
  if (a > b) std::swap(a, b);
  return b;
}
void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
}
void loop() {
  long d1 = measure(), d2 = measure(), d3 = measure();
  long d = median3(d1, d2, d3);
  float cm = d / 58.0;
  Serial.printf("%.1f cm\n", cm);
  delay(300);
}