Rover 4WD nunchuck

Bruno ha già condiviso con noi appassionati il suo progetto Robot 4WD ed oggi ha voluto condividere l’evoluzione chiamandola Rover 4WD nunchuck:

Una delle prerogative del blog è quella di offrire a tutti gli appassionati come Te e Bruno di pubblicare e condividere i progetti realizzati.

Il progetto di Bruno ha subito una significativa evoluzione da Robot 4WD a Rover 4WD nunchuck.

Modifche del RObot 4WD

Le modifiche per la nuova versione del mio robot 4WD sono sia hardware che software.

A livello di elettronica e collegamenti ho aggiunto un pacco batterie da 4 AA ricaricabili per alimentare la bread board. Spulciando il blog ho trovato un articolo di Mauro sull’alimentazione extra dei servi e quindi ne ho approfittato per apportare questa modifica. Per rendere facile collegare e scollegare anche questa alimentazione ho utilizzato un paio di capicorda dei jumper per intestare i cavi del portabatterie. Fatto questo è bastato lasciare solo il collegamento tra la breadboard e la massa di arduino.

I nuovi programmi prevedono la funzione per seguire una fonte luminosa quindi, dopo gli opportuni test e calibrazioni su un’altra board, ho collegato 2 fotoresistenze da 20-50 KOhm

Con questo collegamento in pratica si ottiene un partitore di tensione che permette, tramite lettura analogica, di conoscere quale resistenza è più illuminata.

L’ultima modifica hardware riguarda proprio la scelta dei pin. Ho voluto provare a filoguidare il robot tramite un nunchuck. Di default i pin dedicati al clock e dati sono il 4 e 5.
Ho spostato quindi sul pin 3 l’ingresso del sensore di distanza e collegato le fotoresistenze sul 2.

Modifiche software per il Rover 4WD nunchuck

Come nella prima versione uso un interrupt per rilevare la pressione del push button (quello ross sul retro del robot) che comanda l’avvio e l’annullamento dell’esecuzione del programma selezionato.
Utilizzando gli interrupt è possibile “infilarsi” in qualsiasi punto dell’esecuzione del codice.

Nella prima versione del progamma anche la selezione del programma era collegata ad un interrupt ma ho notato che la selezione era difficile anche, penso, a causa dell’instabilità del collegamento del push button sulla bread board.
Quindi ho utilizzato la classica lettura digitale nel loop.

Lo sketch

// PROGRAMMA  LED                DESCRIZIONE
// 0          spento             gira evitando ostacoli
// 1          acceso             segui oggetto
// 2          lampeggio veloce   segui luce
// 3          lampeggio lento    filoguidato con nunchuck

#include <Servo.h>
#include <Wire.h>

// valori da verificare con il proprio hardware
// valore minimo joystick nunchuck
int min_x=46;
int min_y=46;

// valore a riposto joystick nunchuck
int mid_x=173;
int mid_y=171;

// valore massimo joystick nunchuck
int max_x=255;
int max_y=255;

uint8_t outbuf[6];
int cnt=0;
int E_DX=5; 
int M_DX=4;
int E_SX=6;
int M_SX=7;

// valori da verificare con il proprio hardware
#define VELOCITA_DRITTO 110
#define VELOCITA_SVOLTA 200  

int esegui=0;
int programma=0;
int statoLedProgramma=0;

#define IN_PROGRAMMA 2                  // ingresso pulsante selezione programma
#define OUT_LED_PROGRAMMA 12            // uscita led rosso
#define OUT_LED_AVVIO 13                // uscita led verde
#define IN_IR A3                        // ingresso analogico sensore distanza
#define IN_FOTO A2                      // ingresso partitore fotoresistenze

// soglie distanza per i programmi giro e segui
#define SENSIBILITA_EVITA  200          
#define SENSIBILITA_SEGUI_INDIETRO  300
#define SENSIBILITA_SEGUI_A  200
#define SENSIBILITA_SEGUI_B  50

unsigned long tempoLampeggio = 0;
#define TEMPO_LAMPEGGIO_BREVE 200       // in millisecondi
#define TEMPO_LAMPEGGIO_LUNGO 600       // in millisecondi
#define OUT_SERVO_IR 9                  // PWR servo testa sensore distanza

// valori da verificare con il proprio hardware
// parametri per il partitore di tensione con fotoresistenze
#define VOLT_CENTRO 3
#define TOLLERANZA_VOLT 0.30
#define VOLT_DESTRA 1
#define VOLT_SINISTRA 3.5

// vettori angoli servo per programma segui
byte angoliSegui[2]={45,120};

// angoli e relative direzioni per programma giro
byte angoli[4]={10,45,120,170};
char direzioni[4]={'S','s','d','D'};

// angolo posizione "avanti" del servo
byte angoloAvanti=85;

Servo testaIR;

void setup()
{
  pinMode(M_DX,OUTPUT);
  pinMode(M_SX,OUTPUT);
  pinMode(IN_PROGRAMMA,INPUT);

  attachInterrupt(1,interruptStartStop,FALLING);

  pinMode(OUT_LED_PROGRAMMA, OUTPUT);
  pinMode(OUT_LED_AVVIO, OUTPUT);
  pinMode(OUT_SERVO_IR, OUTPUT);

  testaIR.attach(OUT_SERVO_IR);
  testaIR.write(angoloAvanti);

  //Serial.begin(9600);
  nunchuck_init();

  accendiLed();
  fermo();
}

void nunchuck_init()
{
  int n;
  for(n=0;n<6;n++) outbuf[n]=0;

  Wire.begin();

  // inizio blocco inizializzazione per nunchuck non originale
  Wire.beginTransmission(0x52);
  Wire.write(0xF0);
  Wire.write(0x55);
  Wire.endTransmission();

  Wire.beginTransmission(0x52);
  Wire.write(0xFB);
  Wire.write(0x00);
  Wire.endTransmission();
  // fine blocco per nunchuck non originale

  Wire.beginTransmission(0x52);
  Wire.write(0x40);
  Wire.write(0);
  Wire.endTransmission();

  //Serial.println("nunchuk ok");
}

void nunchuck_ack()
{
  Wire.beginTransmission(0x52);
  Wire.write(0);
  Wire.endTransmission();
}

void loop()
{
  accendiLed();
  BottoneProgramma();

  if(esegui)
  {
    switch(programma) {
    case 0:
      faiUnGiro();
      break;

    case 1:
      segui();
      break;

    case 2:
      luce();
      break;

    case 3:
      guida();
      break;
    }
  }
  else
    fermo();
}

void guida()
{
  Wire.requestFrom(0x52,6);

  while(Wire.available())
    outbuf[cnt++]=nunchuck_decode_byte(Wire.read());

  if(cnt>=5)
  {
    usaNunchuckData();
    cnt=0;
    nunchuck_ack();
    delay(50);
  }
}

void usaNunchuckData()
{
  int n;
  int x;
  int y;
  int v=0;

  char Dd='H';
  char Ds='H';

  int c_button=1;
  int z_button=1;

  if(outbuf[5]& 1)
    z_button=0; 

  if(outbuf[5]& 2)
    c_button=0; 

  if(z_button==1 && c_button==1)
    if(esegui==0) esegui=1;

  int joy_x=constrain((int)outbuf[0],min_x,max_x);
  int joy_y=constrain((int)outbuf[1],min_y,max_y);

  if(c_button==1) // considero solo X {
    if(joy_x==mid_x) v=0;
    else
      if(joy_x>mid_x) {
        Dd='H';
        Ds='L';
        v=map(joy_x-mid_x,0,max_x-mid_x,0,255);
      } else {
        Dd='L';
        Ds='H';
        v=map(joy_x,min_x,mid_x,255,0);
      }
  } else {
    if(joy_y==mid_y) v=0;
    else
      if(joy_y>mid_y) {
        Dd='H';
        Ds='H';
        v=map(joy_y-mid_y,0,max_y-mid_y,0,255);
      } else {
        Dd='L';
        Ds='L';
        v=map(joy_y,min_y,mid_y,255,0);
      }
  }

  switch(Dd) {
  case 'H': 
    digitalWrite(M_DX,HIGH);
    break;

  case 'L': 
    digitalWrite(M_DX,LOW);
    break;
  }

  switch(Ds) {
  case 'H':
    digitalWrite(M_SX,HIGH);
    break;

  case 'L':
    digitalWrite(M_SX,LOW);
    break;
  }

  analogWrite(E_DX,v);
  analogWrite(E_SX,v);
  /*
   Serial.print(joy_x); Serial.print(","); Serial.print(joy_y);
   Serial.print("\t");

   Serial.print(x); Serial.print(",");
   Serial.print(y); Serial.print("\t");

   Serial.print("SX: "); Serial.print(Ds);
   Serial.print("\t");

   Serial.print("DX: "); Serial.print(Dd);
   Serial.print("\t");

   Serial.print("v: "); Serial.print(v);
   Serial.print("\t");

   Serial.print("\r\n");
   */
}

char nunchuck_decode_byte(char x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}

void luce()
{
  if(esegui) {
    if(analogRead(IN_IR)<=SENSIBILITA_EVITA) {
      int volt=map(analogRead(IN_FOTO),0,1023,0,5);
      if(volt <= VOLT_CENTRO + TOLLERANZA_VOLT && volt >= VOLT_CENTRO - TOLLERANZA_VOLT)
        fermo();
      else
        if(volt <= VOLT_DESTRA)
          destra();
        else
          if(volt <= VOLT_SINISTRA)
            avanti();
          else
            sinistra();
    } else {
      fermo();
    }
  } else {
    fermo();
  }
}

void aspetta(int tempo) {
  unsigned long tempoVolta=millis();
  while(millis()-tempoVolta < tempo) {
    accendiLed();
  } 
}

void segui() {
  if(esegui) {
    if(analogRead(IN_IR)>SENSIBILITA_SEGUI_INDIETRO) { indietro(); }
    else {
      if(analogRead(IN_IR)>SENSIBILITA_SEGUI_B && analogRead(IN_IR)<SENSIBILITA_SEGUI_A) { avanti(); }
      else {
        fermo();
        if(analogRead(IN_IR)<SENSIBILITA_SEGUI_B) {
          // valuta direzione
          muovi(valutaDirezioneSegui());
        }
      } 
    }
  }
}

char valutaDirezioneSegui() {
  unsigned long adesso=millis();
  unsigned long tempoVolta=0;
  int valori[2];
  char direzione = 'A';

  for(int i=0;i<=1;i++) {
    testaIR.write(angoliSegui[i]);

    tempoVolta=millis();
    while(millis()-tempoVolta < 1500) { accendiLed(); } 

    int distanza=analogRead(IN_IR);
    valori[i]=distanza;
  } 

  int distanzaminima=0;

  for(int i=0;i<=1;i++) {
    if(valori[i]>distanzaminima) {
      distanzaminima=valori[i];
      direzione=direzioni[i];
    }  
  }

  testaIR.write(angoloAvanti);
  return direzione;
}

void faiUnGiro() {
  if(esegui) {
    if(analogRead(IN_IR)<=SENSIBILITA_EVITA) { avanti(); }
    else {
      fermo();
      aspetta(750);
      indietro();
      aspetta(500);
      fermo();
      aspetta(750);

      muovi(valutaDirezioneEvita());
    }
  }
}

void muovi(char direzione) {
  // I tempi di rotazione possono variare in base al proprio hardware o allo stato di carica delle batterie
  switch(direzione) { 
  case 'I':
    sinistra();
    aspetta(1350);
    fermo();
    aspetta(750);
    break;

  case 'S':
  case 's':
    sinistra();
    aspetta(500);
    fermo();
    aspetta(500);
    break;

  case 'D':
  case 'd':
    destra();
    aspetta(500);
    fermo();
    aspetta(500);
    break;
  }
}

char valutaDirezioneEvita()
{
  unsigned long adesso=millis();
  unsigned long tempoVolta=0;
  int valori[4];
  char direzione = 'A';

  for(int i=0;i<4;i++) {
    testaIR.write(angoli[i]);
    tempoVolta=millis();
    while(millis()-tempoVolta < 1500) { accendiLed(); } 

    int distanza=analogRead(IN_IR);
    valori[i]=distanza;
  } 

  int distanzaminima=999999;

  for(int i=0;i<4;i++) {
    if(valori[i]<distanzaminima) {
      distanzaminima=valori[i];
      direzione=direzioni[i];
    }  
  }

  if(distanzaminima>SENSIBILITA_EVITA) { direzione='I'; }

  testaIR.write(angoloAvanti);
  return direzione;
}

void interruptStartStop() {    
  if(esegui==0) { esegui=1; }
  else {
    esegui=0;
    fermo();
  } 
}

void BottoneProgramma() {
  if(digitalRead(IN_PROGRAMMA)==HIGH) {
    if(programma<3) { programma++; }
    else { programma=0; }

    esegui=0;
    fermo();
    delay(600);
  }
}

void accendiLed() {
  unsigned long adesso=millis();
  switch(programma) {
  case 0: 
    statoLedProgramma=0; 
    break;              

  case 1: 
    statoLedProgramma=1; 
    break;

  case 2: 
    if(adesso-tempoLampeggio > TEMPO_LAMPEGGIO_BREVE) {
      tempoLampeggio=adesso;

      if(statoLedProgramma == 0) { statoLedProgramma=1; }
      else                       { statoLedProgramma=0; }
    }
    break;

  case 3: 
    if(adesso-tempoLampeggio > TEMPO_LAMPEGGIO_LUNGO) {
      tempoLampeggio=adesso;

      if(statoLedProgramma == 0) { statoLedProgramma=1; }
      else                       { statoLedProgramma=0; }
    }
    break;
  }

  digitalWrite(OUT_LED_PROGRAMMA,statoLedProgramma);

  if(esegui) { digitalWrite(OUT_LED_AVVIO,HIGH); }
  else { digitalWrite(OUT_LED_AVVIO,LOW); }
}

void fermo() {
  analogWrite(E_DX,0);
  analogWrite(E_SX,0);
}

void avanti() {
  digitalWrite(M_DX,HIGH);
  digitalWrite(M_SX,HIGH);  

  analogWrite(E_DX,VELOCITA_DRITTO);
  analogWrite(E_SX,VELOCITA_DRITTO);
}

void indietro() {
  digitalWrite(M_DX,LOW);
  digitalWrite(M_SX,LOW);  

  analogWrite(E_DX,VELOCITA_DRITTO);
  analogWrite(E_SX,VELOCITA_DRITTO);
}

void destra() {
  digitalWrite(M_DX,HIGH);
  digitalWrite(M_SX,LOW);  

  analogWrite(E_DX,VELOCITA_SVOLTA);
  analogWrite(E_SX,VELOCITA_SVOLTA);
}

void sinistra() {
  digitalWrite(M_DX,LOW);
  digitalWrite(M_SX,HIGH);  

  analogWrite(E_DX,VELOCITA_SVOLTA);
  analogWrite(E_SX,VELOCITA_SVOLTA);
}

Lo sketch prevede la possibilità di eseguire 4 programmi per controllare il Rover 4WD nunchuck, ciascun programma è descritto così da Bruno:

Il primo, corrispondente al led rosso spento, permette al robot di muoversi evitando gli ostacoli. Rispetto alla prima versione ho aggiunto 2 angoli e relative letture. Il servo non si muove esattamente di 180 gradi per cui i valori sono frutto di varie prove per rendere simmetriche le posizioni. L’idea, avendo 4 letture al posto di 2, è quella di poter far ruotare differentemente il robot in base a “quanto” a destra o a sinistra è disponibile più spazio.

Il secondo programma, corrispondente al led acceso fisso, è quello per seguire un oggetto. In questo caso volevo realizzare la possibilità di farsi seguire dal robot. La cosa è un po’ difficile da realizzare con un solo sensore di distanza. Ho provato comunque e il risultato mi pare accettabile…o almeno divertente. 🙂
Il funzionamento è simile ma “contrario” a quello del primo programma. Ho utilizzato 3 valori differenti di sensibilità per sapere quando fermare o far avanzare il robot.
Sotto una certa distanza (SENSIBILITA_SEGUI_INDIETRO) il robot indietreggia, tra SENSIBILITA_SEGUI_A e SENSIBILITA_SEGUI_B il robot avanza seguendo l’oggetto.
Se nessuna di queste condizioni si realizza vuol dire che l’oggetto seguito è stato perso quindi il robot si ferma e inizia a cercare l’oggetto più vicino a destra o a sinistra. Se non c’è qualcosa nel range di distanza per cui ripartire il robot ruota e ripete la scansione.

Terzo programma, led lampeggiante veloce, inseguimento luce: ancora una volta ho attinto ad un tutorial di Mauro 😉
Ho utilizzato come base l’inseguitore solare per capire da quale direzione arriva la luce maggiore. Nella funzione luce() verifico sempre che lo spazio davanti al robot sia sufficiente al movimento come per evitare gli ostacoli. Se c’è spazio converto il valore analogico letto in volt tramite la funzione map. Per poter comandare il robot e utilizzo 3 valori di riferimento e una tolleranza. Tutti i valori li ho trovati con il solito circuito di prova separato dal robot.

Il quarto ed ultimo programma, quello per la guida tramite nunchuck, corrisponde al lampeggio lento del led rosso. Questo programma è stato un po’ più complicato da realizzare perchè, nonostante i buoni tutorial del blog, ho riscontrato alcuni problemi con l’inizializzazione e lettura dei valori dal nunchuck.
Prima di tutto, non avendo sotto mano l’adattatore suggerito negli articoli, ho tagliato il cavo e aperto il connettore per poter capire quali cavi utilizzare. Metodo piuttosto rozzo ma ha funziona 0:)
Capita la piedinatura ho intestato i cavi e realizzato una sorta di prolunga con i jumper M/M e F/F.
Dopo qualche ricerca ho trovato una sequenza di inizializzazione funzionante.
Anche il codice per leggere la posizione del joystick e dell’accelerometro è diverso da quello del blog. Questa versione si può limare ancora un po’ per pesare il meno possibile su arduino.
In ogni caso il funzionamento è il seguente: tramite la funzione constrain limito il valore letto tra il minimo e il massimo ricavati con il programma di prova. In questo modo inizio a scartare le letture anomale.
Per semplificare l’interpretazione del movimento considero i movimenti sull’asse X solo se il pulsante C è premuto.
Prima anomalia: premendo il pulante Z sia i valori di Z che C risultano premuti. Boh, sarà il dispositivo farlocco? Comunque utilizzo questa lettura per fermare l’esecuzione dei programmi.

Sia per il movimento in avanti o indietro che per la rotazione per prima cosa confronto il valore letto con la relativa posizione a riposo. Se corrisponde imposto la variabile di appoggio v a zero in modo da fermare il robot.
Valori superiori o inferiori mi dicono come impostare le direzioni dei motori.
La velocità è calcolata tramite la funzione map per convertire la posizine a valori compresi tra 0 e 255 e il gioco è fatto: a questo punto ho tutti i valori per impostare i motori.

La cosa più strana è che nei programmi di prova e a robot sollevato la lettura è stabile mentre una volta in uso il fondo corsa a destra piomba al valore del fondo corsa a sinistra.
Il problema a quanto pare è proprio il dispositivo non originale e l’assorbimento dei motori. Per ovviare al problema, invece di alimentare il nunchuck con i 3.3 volt di arduino, ho utilizzato i 5 delle 4 AA supplementari.
Nel sorgente pubblicato ho lasciato i commenti per visualizzare i vari valori.

Credo sia tutto, ringrazio Mauro per la serie infinita di tutorial e lo spazio messo a disposizione.

Grazie Bruno per la tua voglia di fare e condividere 🙂