Self Balancing Robot - parte 5 - Usare gli encoder

Inizio il post premettendo che non servono gli encoder per tenere in equilibrio il robot. Ma se volete ottenere un robot che stia in equilibrio senza mostrare nervosismo e che sappia reagire bene all'azione di forze esterne, allora l'encoder è lo strumento corretto per aggiungere un ulteriore parametro alla stabilità.

In ogni caso avete visto in uno dei post precedenti che l'encoder mi è servito per  determinare la risposta dei motori in funzione del segnale pwm inviato, e calibrarne così la curva di risposta in modo da avere il più possibile due risposte identiche da parte dei due motori.

Esistono vari tipi di encoder, quelli montati sui motori nel mio progetto sono di tipo ad effetto Hall.

Non tratterò qui la teoria degli encoder visto che è ampiamente documentata in rete. Mi limiterò a spiegare come usare il dato che essi possono fornire e come ottenerlo via codice.

Partendo dalla documentazione dei motori, vediamo come collegare gli encoder. Dalla pagina del produttore si può leggere che questo encoder montato sul Pololu 29:1 ha una risoluzione di 1856 count per giro.

Questo potrebbe trarre in inganno. Se voi provate sperimentalmente a leggere il valore restituito dagli encoder troverete un valore molto più basso, per la precisione 1/4 di 1856. 

Qual'è il motivo? L'encoder lavora "in quadratura", ovvero restituisce su 2 pin diversi un'onda quadra alla stessa frequenza ma sfasate tra loro di 90°, in modo che elaborandole si possa capire non solo la velocità (proporzionale alla frequenza dell'onda) ma anche la direzione.

E' importante questa cosa: se osservate l'immagine sopra, noterete che la sequenza delle due onde combinate tra di loro si ripete esattamente dopo che sono avvenuti 4 cambi di stato nei due canali. Per capire la direzione del motore, dobbiamo monitorare il cambio di stato crescente, ovvero la transizione dallo stato LOW a HIGH, chiamato anche rising edge (in opposizione al falling edge che è il passaggi oda HIGH a LOW).

Dato che il ciclo inizia in questo caso con un rising edge sul canale A, in corrispondenza con uno stato LOW del canale B, per ritrovare la stessa situazione, dobbiamo aspettare altri 3 cambi di stato, uno su B, poi di nuovo uno su A e nuovamente uno su B.

Per capire la direzione non è sufficiente leggere il rising edge, ma valutare anche qual'è lo stato corrente dell'altro canale. Ovviamente un rising edge ottenuto quando il motore gira in una direzione, diventa un falling edge quando il motore gira all'opposto, e viceversa.

Questa è sicuramente una cosa che può creare confusione ed errori: a voi serve determinare qual'è il valore massimo che può darvi l'encoder quando il motore gira alla velocità massima, perchè quello è il riferimento per voi per capire a quale % di velocità max reale il vostro motore sta girando.

Tale valore può essere un parametro di feedback per il vostro algoritmo PID (vedremo nei prossimi post), come pure il valore da sparare a Processing per visualizzare graficamente l'andamento dei motori (anche questo sarà argomento per i prossimi post).

State attenti alla scala dei tempi nella quale lavorate. Vi faccio un esempio pratico.

Il vostro codice sarà basato su un loop, possibilmente basato su un tempo fisso. Ipotizziamo 10ms, che è un buon tempo di monitoraggio dei sensori e di pilotaggio dei motori in un progetto di self balancing.

Se noi vogliamo sapere quanti impulsi l'encoder ci darà tra un ciclo e l'altro, ovvero in un tempo di 10ms, dobbiamo fare il seguente ragionamento:

Il motore gira al massimo a 350 giri/minuto (rpm) e fa 1856 count per giro   

 350 rpm * 1856 count/rev  

significa 649600 c/pm

Se un minuto sono 60 secondi allora diviso per 60

 10826,66 c/ sec

diviso per quattro per il motivo spiegato sopra

 2706,66  c/sec

diviso mille per avere la scala in ms

2,706 c/ms

per 10 per ottenere il count max nel tempo di loop

ovvero:

  TOP_SPEED_PULSES=(float)350*(float)1856/(float)60/(float)4/(float)1000*(float)LOOP_TIME;

che con i nostri parametri da come risultato 27.    

Naturalmente questo è un valore teorico, quando proverete leggerete un valore sicuramente minore (nel mio caso 23) perchè i motori girano un po' meno del valore massimo dichiarato.

Per leggere l'encoder non è possibile utilizzare una tecnica di tipo polling in quanto le letture devono essere molto rapire e sincrone con l'encoder. Per questo è necessario utilizzare gli AttachInterrupt di Arduino.

Ecco un po' di codice di esempio:

definiamo innanzitutto i pin ai quali gli encoder sono collegati:

#define encoder0PinA  2
#define encoder0PinB  7
#define encoder1PinA  3
#define encoder1PinB  4

non a caso sono stati scelti i pin 2 e 3 perchè sono gli unici su Arduino Uno in grado di gestire gli attachInterrupt. A tal proposito leggetevi i post precedenti dove di parlava del routing dei pin, in modo da liberare i pin 2-3 e riservarli agli encoder.

Abilitiamo ora gli attachInterrupt, ovvero diciamo ad arduino che ad ogni rising edge sul pin dovrà essere fatta una chiamata alla funzione indicata

void setupEncoder(){
  //setup encoder 0
  pinMode(encoder0PinA, INPUT);
  digitalWrite(encoder0PinA, HIGH);      
  pinMode(encoder0PinB, INPUT);
  digitalWrite(encoder0PinB, HIGH);   
  attachInterrupt(0, rencoder0, RISING);  // encoder on pin 2
  //setup encoder 1  
  pinMode(encoder1PinA, INPUT);
  digitalWrite(encoder1PinA, HIGH);     
  pinMode(encoder1PinB, INPUT);
  digitalWrite(encoder1PinB, HIGH);       
  attachInterrupt(1, rencoder1, RISING);  // encoder on pin 3
  
}

e ora definiamo cosa fare quando viene scatta l'interrupt:

void rencoder0()  {                     
  if (digitalRead(encoder0PinA) == HIGH && digitalRead(encoder0PinB)==LOW) 
  {
    count_0++;
  } 
  else 
  {
    count_0--;
  }
}

void rencoder1()  {                    
  if (digitalRead(encoder1PinA) == HIGH && digitalRead(encoder1PinB)==LOW) 
  {
     count_1++;
  } 
  else 
  {
     count_1--;
  }
}

in pratica quando viene letto un rising edge su uno dei due canali, vengono invocate le rispettive funzioni e verificato lo stato di entrambi i canali. Lo stato determina univocamente il senso di rotazione e quindi l'incremento/decremento del contatore globale.

Per determinare la velocità è poi sufficiente all'interno del ciclo principale (quello basato su un tempo constante di 10ms), fare la differenza tra il valore corrente e il valore precedente.

Nel prossimo post: quella magic box chiamata PID