HC-SR04 ultrasone sensor: betrouwbare afstand in code

Een robot die obstakels vermijdt is zo goed als het getal waarop hij stuurt. Dat getal komt van een HC-SR04 ultrasone sensor: hij tikt, luistert naar de echo, en uit de looptijd volgt hoe ver het dichtstbijzijnde ding weg is. Deze gids maakt van die ruwe ping een afstand in centimeters die je kunt vertrouwen—een die de uitval en dwaalecho’s van de sensor overleeft in plaats van je robot een muur in te sturen.

Hoe de sensor de afstand meldt

De HC-SR04 spreekt een eenvoudige taal met twee pinnen. Je pulseert Trigger HIGH gedurende 10 microseconden; hij vuurt acht bursts van 40 kHz af en trekt Echo omhoog tot de reflectie terugkomt. De tijd dat Echo HIGH blijft is de looptijd, en geluid beweegt met ongeveer 343 m/s, dus:

distance_cm = echo_time_us * 0.0343 / 2

De deling door twee komt omdat het geluid heen én terug ging. De componentpagina behandelt de bedrading en de natuurkunde; hier gaat het erom de sensor schoon in code uit te lezen.

Lezen met pulseIn—en altijd een time-out instellen

De time-out is om een tweede reden belangrijk, los van de juistheid: pulseIn blokkeert, dus een gemiste echo legt je hele lus stil voor net zo lang als je toestaat. Zie niet-blokkerende timing voor wat dat een bewegende robot kost.

pulseIn meet hoe lang een pin HIGH blijft. De valkuil: komt de echo nooit terug—niets binnen bereik, of een zacht oppervlak dat het geluid verstrooit—dan blokkeert pulseIn zijn volledige standaardtime-out en staat je regellus stil. Geef altijd een expliciete time-out mee, afgestemd op je maximumbereik:

const int TRIG = 9, ECHO = 10;
const unsigned long TIMEOUT_US = 25000UL;  // ~4 m of round trip

long pingCm() {
  digitalWrite(TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  unsigned long us = pulseIn(ECHO, HIGH, TIMEOUT_US);
  if (us == 0) return -1;             // timed out: nothing in range
  return (long)(us * 0.0343 / 2.0);
}

-1 teruggeven bij een time-out—in plaats van 0—is belangrijk. Een 0 leest als “er raakt een obstakel de sensor”, en dat is het omgekeerde van “de weg is vrij”. Houd “geen echo” onderscheiden van “zeer dichtbij”.

Waarom de ruwe waarde heen en weer springt

Richt een HC-SR04 op een muur en de metingen flikkeren nog steeds, want de echte wereld is rommelig:

• Zachte of schuine doelen verstrooien de echo weg van de ontvanger, dus krijg je af en toe een enorme waarde of helemaal niets.
• De ~15°-bundel betekent dat de sensor het dichtstbijzijnde ding in een kegel meldt, dus een tafelpoot opzij kan een korte meting veroorzaken.
• Te snel pollen (minder dan ongeveer 60 ms ertussen) laat de echo van de vorige ping in de volgende meting doorsijpelen.

Voer je die ruwe sprongen rechtstreeks een stop-of-draai-beslissing in, dan schokt de robot. Je moet de stroom gladstrijken zonder vertraging toe te voegen.

Een mediaan-van-5-filter

Het juiste gereedschap is een mediaan, geen gemiddelde. Een gemiddelde smeert één wilde piek over meerdere metingen uit; een mediaan gooit hem gewoon weg. Neem vijf pings en gebruik de middelste:

int cmpLong(const void* a, const void* b) {
  return (*(long*)a) - (*(long*)b);
}

long medianCm() {
  long s[5];
  for (int i = 0; i < 5; i++) {
    long v = pingCm();
    s[i] = (v < 0) ? 400 : v;   // treat timeout as "far", not "touching"
    delay(30);                  // let the previous echo die away
  }
  qsort(s, 5, sizeof(long), cmpLong);
  return s[2];                  // the middle value
}

Twee details maken dit robuust: een time-out omzetten in een grote waarde “ver” (zodat een verloren echo nooit als obstakel leest), en de kleine delay tussen pings zodat echo’s elkaar niet in de weg zitten. Voor de theorie achter filteren zonder overmatig gladstrijken, zie sensorruis, afwijking en filtering.

Stop met de lus blokkeren

Alles hierboven laat de lus nog steeds stilstaan. pulseIn wacht, delay(30) wacht, en vijf pings om de 30 ms is 150 ms waarin de robot geen enkele andere sensor uitleest en geen enkele beslissing neemt. Bij 0,3 m/s is dat 4,5 cm blinde rit per meting.

De oplossing is de sensor als een kleine toestandsmachine aan te sturen waar de hoofdlus zonder ooit te wachten doorheen loopt:

enum PingState { IDLE, TRIGGERED, WAITING };

PingState state = IDLE;
unsigned long stateSince = 0;
long lastDistance = -1;

const unsigned long PING_PERIOD_MS = 60;   // let the previous echo die away
const unsigned long ECHO_TIMEOUT_MS = 30;  // beyond the sensor's range

void updatePing() {
  const unsigned long now = millis();

  switch (state) {
    case IDLE:
      if (now - stateSince < PING_PERIOD_MS) return;
      digitalWrite(TRIG, HIGH);
      stateSince = micros();          // note: microseconds for this short step
      state = TRIGGERED;
      break;

    case TRIGGERED:
      if (micros() - stateSince < 10) return;
      digitalWrite(TRIG, LOW);
      stateSince = now;
      state = WAITING;
      break;

    case WAITING:
      if (echoComplete) {             // set by the interrupt below
        lastDistance = echoMicros / 58.2;
        echoComplete = false;
        stateSince = now;
        state = IDLE;
      } else if (now - stateSince > ECHO_TIMEOUT_MS) {
        lastDistance = -1;            // no echo — far, or invisible
        stateSince = now;
        state = IDLE;
      }
      break;
  }
}

De lus roept nu updatePing() aan en gaat verder. lastDistance is altijd de meest recent voltooide meting, hoogstens 60 ms oud, en niets anders in de robot heeft daarop gewacht.

De echotijd meten met een interrupt

De toestandsmachine moet nog steeds weten wanneer de echo binnenkwam, en digitalRead pollen in de hoofdlus kost resolutie. Een interrupt op beide flanken van de Echo-pin kost niets:

volatile unsigned long echoStart = 0;
volatile unsigned long echoMicros = 0;
volatile bool echoComplete = false;

void echoISR() {
  if (digitalRead(ECHO) == HIGH) {
    echoStart = micros();
  } else if (echoStart != 0) {
    echoMicros = micros() - echoStart;
    echoStart = 0;
    echoComplete = true;
  }
}

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  attachInterrupt(digitalPinToInterrupt(ECHO), echoISR, CHANGE);
}

Op een Arduino Uno moet dat pin 2 of 3 zijn—de enige twee pinnen voor externe interrupts—en als je robot wielencoders heeft, willen die allebei. Dat conflict is echt, en het is een van de vaker voorkomende redenen dat een project overgaat naar de pin-change-interrupts van een Nano, een Mega of een ESP32.

Let op de controle echoStart != 0. Zonder deze controle berekent een dalende flank zonder voorafgaande stijgende flank—wat gebeurt bij het eerste interrupt na een time-out—een onzinnige duur uit een verouderde tijdstempel.

Het filter kiezen

Mediaan is de juiste standaard, maar het is de moeite waard te weten wat de alternatieven daadwerkelijk doen, want ze falen op verschillende manieren:

Ultrasone ruis is niet Gaussisch—het zijn af en toe wilde uitschieters op een verder stabiel signaal, en dat is precies het geval waar een gemiddelde slecht mee omgaat en een mediaan perfect. Eén piek van 400 cm in een set van vijf metingen van 30 cm verplaatst het gemiddelde met 74 cm en de mediaan met helemaal niets.

Temperatuur, als je de nauwkeurigheid nodig hebt

De deler 58,2 gaat uit van 343 m/s, en dat is de geluidssnelheid bij 20 °C. Die is niet constant:

speed (m/s) = 331.3 + 0.606 x temperature_celsius

Voor obstakelvermijding negeer je dit volledig—7 cm op 2 m verandert geen enkele beslissing. Voor een robot die een opening meet, of een die in de winter in een garage werkt, corrigeer je:

float cmPerMicrosecond(float tempC) {
  return (331.3f + 0.606f * tempC) / 20000.0f;   // /1e6 for us, /2 for round trip
}

Heeft je robot al een MPU-6050—of vrijwel elke andere I²C-sensor—dan heb je gratis een temperatuurmeting beschikbaar.

Meerdere sensoren draaien

Twee HC-SR04’s die tegelijk pingen horen elkaars ping en leveren allebei wilde metingen op. Ze moeten elkaar afwisselen:

const int SENSORS = 3;
int current = 0;
long distance[SENSORS];

void updateAll() {
  if (pingComplete(current)) {
    distance[current] = lastDistance;
    current = (current + 1) % SENSORS;    // round-robin
    startPing(current);
  }
}

De prijs daarvan is de verversingsfrequentie: drie sensoren van elk 60 ms betekent dat elke losse sensor elke 180 ms een update krijgt. Is dat te langzaam—en op een bewegende robot is dat vaak zo—dan is het antwoord óf minder ultrasone sensoren, óf een VL53L0X ertussen zetten, die licht gebruikt en dus een sonar helemaal niet kan storen.

Problemen oplossen

Waar dit hiernaartoe gaat

Je hebt nu één schoon getal: de afstand recht vooruit. Twee dingen bouwen daarop voort. Monteer de sensor op een servo en zwaai hem om te zien welke kant open is—scan een ultrasone sensor met een servo—en voer het resultaat in de beslislus van het obstakelvermijdingsalgoritme. Probeer het hele waarnemen-en-sturen eerst in de simulator voor obstakelvermijding.