Zumo linefollower

Ieri ho testato il mio zumo linefollower ossia il robot Zumo della Pololu che ho acquistato qualche mese fa dal sito  www.robotics-3d.com

Ho già dedicato alcuni articoli in cui puoi leggere come ho assemblato la zumo shield compresa nel kit ed i sensori QTR opzionali.

I sensori QTR del zumo linefollower

Per inseguire una linea puoi usare i sensori ottici che preferisci o acquistare i sensori dedicati a questo modello di robot cingolato.

Come vedi in figura i sensori ottici presenti sulla basetta sono 6 posti in modo sfalsato tra loro secondo questa immagine presente sul sito del produttore:

per leggere i sensori utilizzi una classe che la pololu rilascia gratuitamente come tutte quelle necessarie al zumo linefollower ed usate in questo sketch.

Lo sketch di esempio Zumo linefollower

Quando ti cimenti con un progetto nuovo ed il produttore ti fornisce uno o più esempio io ti consiglio di partire da quelli per evitare di impazzire su problemi software e scoprire che il prodotto è malfunzionante per cui parti dall’esempio fornito. In questo caso ho preso l’esempio LineFollower.ino:

#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>
#include <ZumoMotors.h>
#include <ZumoBuzzer.h>
#include <Pushbutton.h>

ZumoBuzzer buzzer;
ZumoReflectanceSensorArray reflectanceSensors;
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON);
int lastError = 0;

const int MAX_SPEED = 200;

void setup() {
  
  motors.flipLeftMotor(true);
  //motors.flipRightMotor(true);
  
  // Play a little welcome song
  buzzer.play(">g32>>c32");

  // Initialize the reflectance sensors module
  reflectanceSensors.init();

  // Wait for the user button to be pressed and released
  button.waitForButton();

  // Turn on LED to indicate we are in calibration mode
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);

  // Wait 1 second and then begin automatic sensor calibration
  // by rotating in place to sweep the sensors over the line
  delay(1000);
  int i;
  for(i = 0; i < 80; i++)
  {
    if ((i > 10 && i <= 30) || (i > 50 && i <= 70))
      motors.setSpeeds(-200, 200);
    else
      motors.setSpeeds(200, -200);
    reflectanceSensors.calibrate();

    // Since our counter runs to 80, the total delay will be
    // 80*20 = 1600 ms.
    delay(20);
  }
  motors.setSpeeds(0,0);

  // Turn off LED to indicate we are through with calibration
  digitalWrite(13, LOW);
  buzzer.play(">g32>>c32");

  // Wait for the user button to be pressed and released
  button.waitForButton();

  // Play music and wait for it to finish before we start driving.
  buzzer.play("L16 cdegreg4");
  while(buzzer.isPlaying());
}

void loop()
{
  unsigned int sensors[6];
  int position = reflectanceSensors.readLine(sensors);
  int error = position - 2500;
  int speedDifference = error / 4 + 6 * (error - lastError);
  lastError = error;

  int m1Speed = MAX_SPEED + speedDifference;
  int m2Speed = MAX_SPEED - speedDifference;

  if (m1Speed < 0) m1Speed = 0;
  if (m2Speed < 0) m2Speed = 0;
  if (m1Speed > MAX_SPEED) m1Speed = MAX_SPEED;
  if (m2Speed > MAX_SPEED) m2Speed = MAX_SPEED;

  motors.setSpeeds(m1Speed, m2Speed);
}

l’ho ripulito dai commenti, che trovi nello sketch di esempio per renderlo facilmente leggibile e aggiungere la mia descrizione dello sketch.

Parti dalle prime linee 01-05: includi tutte le librerie necessarie al controllo del robot;

linea 07: inizializza un oggetto buzzer come istanza della classe ZumoBuzzer, ti erve a controllare in modo semplice il buzzer e fargli eseguire delle melodie o dei suoni;

linea 08: inizializza un oggetto reflectanceSensors come istanza della classe ZumoReflectanceSensorArray è il cuore e controllo della striscia di sensori QTR;

linea 09: inizializza un oggetto motors come istanza della classe  ZumoMotors che ti permetterà di controllare facilmente i motori dello zumo linefollower;

linea 10: inizializza un oggetto button come istanza della classe Pushbutton che terrà sotto controllo la pressione del bottone;

linea 11: definisci una variabile intera lastError che ti servirà nella fase di loop per determinare il valore di errore letto dai sensori ad ogni ciclo di loop();

linea 13: definisci una costante di tipo intero MAX_SPEED in cui imposti la velocità massima a cui i motori possono arrivare in fase di accellerazione, il valore massimo è 400, io l’ho dimezzato per consentire al zumo linefollower più tempo in fase di valutazione della linea da seguire;

linee 17-18: all’interno della funzione setup() di arduino imposta la direzione dei motori, nel mio esempio ho dovuto impostare a true il valore del metodo flipLeftMotor dell’istanza motors definita alla linea 09 in modo da permettere al robot di controllare in modo corretto i motori. La corretta impostazione dei motori dipende da come li hai montati in fase di assemblaggio e sono disponibili i metodi di correzione dia per il motore destro ( Right ) sia per quello sinistro ( Left ) in quanto una volta saldati i motori è più comodo correggere via software il verso di rotazione che non dissaldare e risaldare in modo inverso;

linea 21: imposta ed esegui un suono mediante il buzzer;

linea 24: inizializza i sensori di riflessione ( ZumoReflectanceSensor ) che si occuperanno della lettura della linea;

linea 27: controlla la pressione del pulsante, il metodo waitForButton() dell’istanza button, come dice il nome stesso, attende che tu prema il pulsante connesso al pin 12 di Arduino;

linee 30-31: imposta la modalità di OUTPUT per il led collegato al pin 13 e lo accende per indicarti di aver ricevuto il comando di pressione del bottone e scandisce l’inizio della fase di calibrazione dei sensori;

linee 37-48: imposta un ciclo da 0 a 80 letture dei sensori per eseguire la calibrazione, negli intervalli 0-10, 30-40 e 70-80 i motori si muoveranno in modo che il motore di sinistra vada a velocità 200 e il destro a velocità -200; negli intervalli 10-20 e 50-70 i motori si muovono in modo esattamente inverso. Per ciascun ciclo usi il metodo calibrate() per impostare la calibrazione dei sensori sulla base della linea che vuoi seguire ed attendi 20 millisecondi tra un ciclo ed il successivo. In questo modo tutto il ciclo di calibrazione dura 1600 millisecondi ( 80 * 20 );

linea 49: imposta a 0 entrambi i motori;

linee 52-53: spegni il led sul pin 13, la calibrazone è finita, ed emetti un suono dal buzzer per indicare il termine di questa fase;

linea 56: attendi nuovamente la pressione del bottone, che inicherà al zumo linefollower di iniziare a inseguire la linea;

linee 59-60: dopo la pressione del pulsante ( P12 ) il robot emette un conto alla rovescia mediante l’invocazione del metodo play e attende che il suono sia concluso con un ciclo while;

linea 65: ad ogni ciclo di loop() imposti un array di 6 elementi da passare al metodo di lettura della linea messo a disposizione della classe ZumoReflectanceSensor;

linea 66: utilizza il metodo readLine() dell’istanza reflectanceSensors per leggere il valore da tutti i sensori presenti sulla barra anteriore del zumo robot. Il metodo readLine per 6 sensori restituisce un valore da 0 a 5000 in funzione della lettura eseguita dal sensore corrispondente della linea, se ad esempio la linea si trova sotto il primo sensore, quello più a sinistra, il valore riportato sarà 0 mentre se si trova sul secondo 1000, e così fino al 6 sensore che riporta 5000;

linea 67: il punto centrale trai valori riportati dalla lettura della linea è dunque 2500 per cui se il massimo valore che i sensori possono leggere è 5000 per impostare la linea al centro dovrai sottrarre 2500;

linea 68: è il punto più complesso dello sketch in cui calcoli la differenza di velocità che un motore deve avere rispetto all’altro in funzione di un algoritmo di PID ( se vuoi approfondire come opera questo algoritmo puoi leggerlo qui ) che restituisce un valore in base all’errore letto. In particolare usi, in questo esempio, solo le componenti P e D, proporzionale e derivata e da esperimenti eseguiti dalla pololu consigliano un valore di P di 1/4 ed un valore di D pari a 6 per cui la formula diventa: error * 1/4 + 6* (error – lasterror ) da cui error / 4 + 6* (error – lasterror );

linea 69: imposti lastError uguale all’errore cacolato alla linea 67;

linee 71-72: calcola il valore di velocità del robot sia per il motore 1 ( left ) sia per il motore 2 ( right ) sommando e sottraendo dalla velocità massima questo valore. Comprendi che quando l’error è pari a 0, ossia leggi sui sensori che la linea è al centro, la velocità dei due motori è identica e costante;

linee 74-77: imposti un controllo per il limite superiore ed inferiore di velocità da applicare ai motori in cui eviti che i motori possano ricevere velocità negative che vorrebbe dire inverti la rotta, in tal caso il corrispettivo motore si ferma;

linea 79: impartisci la velocità calcolata a ciascun motore.

Il video

Ecco il video dello zumo linefollower in azione su una linea da 2cm di spessore: