Scansione di un sensore ultrasonoro con servomotore

Un singolo sensore di distanza puntato in avanti può dire a un robot che c’è qualcosa davanti, ma non quale direzione sia libera. Quindi il robot gira alla cieca, spesso dritto contro un altro ostacolo o in un vicolo cieco. Montare l’HC-SR04 su un servomotore SG90 risolve il problema: il robot si ferma, guarda a sinistra e a destra e gira verso la direzione più libera, proprio come fa una persona davanti a un angolo cieco.

Perché un solo sensore fisso intrappola il robot

Immagina un robot che vede soltanto davanti a sé. Incontra un muro, sa che deve girare, ma non sa se sia meglio andare a sinistra o a destra. Se sceglie male, finisce nell’angolo da cui è appena venuto. Un sensore montato su un servomotore trasforma un numero, la distanza davanti, in una piccola mappa dello spazio davanti al robot, così la rotta diventa una scelta informata, non un lancio di moneta.

Montare il sensore sul servomotore

Fissa l’HC-SR04 alla leva del servomotore e monta il servomotore al centro anteriore del telaio, orientato in modo che la leva faccia scansionare il sensore da sinistra a destra, non in alto e in basso. Nella posizione centrale del servomotore il sensore deve puntare dritto davanti. Così ottieni una scansione pulita da un lato, frontalmente, fino all’altro lato.

Scansione e campionamento

Porta il servomotore su un piccolo insieme di angoli ed effettua a ogni passo una lettura filtrata. Riutilizza la funzione di lettura pulita della distanza di lettura dell’HC-SR04: un ping grezzo a metà scansione è troppo rumoroso per guidare il robot.

#include <Servo.h>
Servo scanner;

const int ANGLES[] = {30, 60, 90, 120, 150};
const int N = 5;

int bestAngle() {
  long best = -1;
  int bestA = 90;
  for (int i = 0; i < N; i++) {
    scanner.write(ANGLES[i]);
    delay(180);                 // let the servo REACH the angle first
    long d = medianCm();        // filtered read from the previous tutorial
    if (d > best) { best = d; bestA = ANGLES[i]; }
  }
  scanner.write(90);            // re-centre for driving
  return bestA;                 // > 90 = open to the left, < 90 = right
}

Non leggere durante il movimento

Il bug più comune qui è leggere prima che il servomotore arrivi a destinazione. servo.write() ritorna immediatamente, ma l’SG90 impiega tempo a ruotare verso il nuovo angolo. Se lanci il ping troppo presto, la distanza appartiene al punto in cui il sensore era, non a quello verso cui hai puntato. Un breve delay dopo ogni write(), sufficiente a consentire l’assestamento del servomotore, rende affidabili tutte le letture.

Trasformare la scansione in una decisione

L’angolo con la distanza maggiore è la direzione più libera. Due accorgimenti mantengono sensata la scelta:

• In caso di parità, preferisci dritto davanti. Se due direzioni restituiscono valori simili, orienta la scelta verso il centro, così il robot non serpeggia inutilmente.
• Tutte le direzioni sono bloccate? Sei in un vicolo cieco: indietreggia e scansiona di nuovo invece di avanzare ancora.

Velocità e copertura

Più angoli danno un’immagine più fine ma un ciclo più lento; un ciclo lento significa che il robot reagisce in ritardo. Da tre a cinque angoli — sinistra, davanti, destra, più un paio intermedi — è il punto di equilibrio abituale. Passa la decisione «più libera» all’algoritmo di evitamento ostacoli e prova l’intero comportamento nel simulatore di evitamento ostacoli prima di costruirlo.

Quanto impiega davvero il servomotore?

«Un breve ritardo» è l’istruzione più vaga nel codice precedente e vale la pena sostituirla con un valore misurato. Un SG90 è specificato a circa 0.1 s ogni 60° a 4.8 V, a vuoto, quindi il tempo di assestamento dipende dalla distanza percorsa:

Due fattori rendono il valore reale peggiore della specifica. Carico: un HC-SR04 e il suo cavo sulla leva hanno una massa piccola ma non nulla e rallentano il servomotore aggiungendo overshoot. Tensione: a 4.0 V con un pacco batteria scarico, lo stesso servomotore è nettamente più lento che a 6 V.

Misura il tuo, senza fidarti della tabella. Comanda un passo di 60° e cronometra quanto impiega la lettura della distanza a smettere di cambiare:

void measureSettle() {
  scanner.write(60);  delay(500);            // known start
  scanner.write(120);                        // command the step
  const unsigned long t0 = millis();
  long prev = -1;
  for (int i = 0; i < 40; i++) {             // sample every 10 ms
    const long d = pingCm();
    Serial.print(millis() - t0); Serial.print(' '); Serial.println(d);
    delay(10);
  }
}

La lettura si assesta in un momento definito. Usa quel valore più un piccolo margine e avrai eliminato la maggiore fonte di dati senza senso dall’intero comportamento.

Scansionare sempre nella stessa direzione è più veloce che saltare qua e là

Il codice precedente passa da 30 → 60 → 90 → 120 → 150, che costituisce già una singola scansione, ed è una scelta deliberata. Leggere gli angoli in ordine sparso significa che ogni passo è grande e il costo di assestamento si paga a ogni passo:

Una scansione completa è un momento di cecità per il robot: è fermo e non progredisce, quindi dimezzarne la durata migliora davvero la percezione del robot. Scansiona in ordine e, se esegui scansioni ripetute, alterna la direzione così non paghi mai il ritorno.

Scansione non bloccante

Il delay(180) a ogni passo fa sì che una scansione a cinque angoli blocchi il robot per quasi un secondo. Su un robot che comunque deve fermarsi può essere tollerabile, ma significa anche che i microinterruttori non vengono letti e qualsiasi altro comportamento resta congelato.

Qui si applica lo stesso trattamento con macchina a stati usato per il ping:

enum ScanState { SCAN_IDLE, SCAN_MOVING, SCAN_READING };

ScanState scanState = SCAN_IDLE;
uint8_t   scanIndex = 0;
unsigned long scanSince = 0;
long      scanResult[N];
bool      scanComplete = false;

void updateScan() {
  switch (scanState) {
    case SCAN_IDLE:
      break;

    case SCAN_MOVING:
      if (millis() - scanSince < SETTLE_MS) return;      // measured, above
      scanState = SCAN_READING;
      break;

    case SCAN_READING:
      scanResult[scanIndex] = medianCm();
      if (++scanIndex >= N) { scanComplete = true; scanState = SCAN_IDLE; scanner.write(90); }
      else { scanner.write(ANGLES[scanIndex]); scanSince = millis(); scanState = SCAN_MOVING; }
      break;
  }
}

Il ciclo principale chiama updateScan() e prosegue. I microinterruttori continuano a essere letti, la macchina a stati continua a funzionare e la scansione termina quando può terminare.

Interpolare tra gli angoli

Cinque letture danno cinque direzioni, ma lo spazio realmente aperto si trova quasi mai esattamente in una di esse. Un baricentro ponderato delle letture aperte fornisce una direzione compresa tra i campioni, esattamente come un array di sensori di riflessione restituisce la posizione di una linea tra sensori diversi:

// Weight each angle by how open it is, ignoring anything below the stop threshold.
float openBearing() {
  float weighted = 0, total = 0;
  for (int i = 0; i < N; i++) {
    if (scanResult[i] < STOP_CM) continue;             // blocked, contributes nothing
    const float w = scanResult[i] - STOP_CM;           // how much clearance beyond the threshold
    weighted += w * ANGLES[i];
    total    += w;
  }
  return (total > 0) ? (weighted / total) : 90.0f;     // 90 = straight ahead
}

Questo è un miglioramento reale rispetto al semplice «scegli il massimo». Scegliendo il massimo, una porta campionata appena fuori centro manda il robot verso il bordo della porta; il baricentro lo manda al centro. Inoltre degrada in modo controllato: se due angoli adiacenti sono entrambi aperti, il risultato è compreso tra loro anziché uno dei due scelto arbitrariamente.

Il cono limita ciò che una scansione può dirti

Il fascio di un HC-SR04 ha circa 15° di semiangolo, che a 1 m corrisponde a un cono di circa 53 cm di diametro. Campionare a intervalli di 30° comporta quindi forti sovrapposizioni: letture adiacenti osservano in gran parte lo stesso spazio.

La conseguenza importante è che una scansione non offre una risoluzione angolare migliore del fascio. Una lettura di 68 cm a 120° significa «qualcosa si trova a 68 cm di distanza, entro circa 15° da 120°»: è abbastanza per scegliere una direzione in cui girare e inutile per misurare la larghezza di un’apertura. Se ti serve quest’ultima, un VL53L0X sullo stesso servomotore ha un punto di misura effettivamente stretto e fornisce una scansione realmente risolta.

Risoluzione dei problemi