Robot SP4

[alieno75]

Ho progettato e costruito un robot a 4 zampe (Sp4: Spider 4 leggs) utilizzando ARDUINODUE.
Esso è dotato di 8 pulsanti: 4 sotto le zampe per rilevare se esiste ancora il terreno (ed in caso non ci sia torna indietro) e 4 per testare se c' e' la presenza di ostacoli.
Quando avanza sono testati solo gli ostacoli che si trovano avanti.
Quando avanza all' indietro sono testati solo gli ostacoli indietro.
Sono riuscito tramite il codice a fargli fare le suddette cose ma ora non sò perchè ma non mi esegue la riga di codice
che fa si che urtando un ostacolo a sx o dx vada indietr odi 3 passi e poi svolti nella direzione opposta l' ostacolo.
Sono riuscito a farlo avanzare all' indietro quando trova un ostacolo a sx per esempio ma dopo aver fatto 3 passi
non gira a dx.
Non capisco perche la parte di codice che lo fa girare a dx non funziona.
Essa funziona solo che faccio la chiamata diretta senza if() svolta(DX).
Riposto di seguito il codice che non funziona:

Code: Select all

//dopo la chiamata di svolta() viene impostato un numero di passi di 3
      //if(passi==3)           pulsante=0;   //riabilita variabile pulsante ostacolo premuto
      //se è stato premuto uno dei pulsanti rilevamento ostacoli decrementa passi in modo da fare 5 passi (passi=5) indietro
      //svolta(SX);
        if(pulsante==OSTACOLO_SX_AV)      svolta(DX);
        if(pulsante==OSTACOLO_DX_AV)      svolta(SX);
        if(pulsante==OSTACOLO_SX_IND)     svolta(DX);
        if(pulsante==OSTACOLO_DX_IND)     svolta(SX);


e questa il codice intero:

Code: Select all

#include <Servo.h>

//costanti avanbraccia/braccia
#define AVANBRACCIO_SU_DX 0
#define AVANBRACCIO_GIU_DX 180
#define AVANBRACCIO_SU_SX 180    //controllare 
#define AVANBRACCIO_GIU_SX 0     //controllare
#define BRACCIO_AV_DX 120     
#define BRACCIO_IND_DX 60   
#define BRACCIO_IND_SX 40
#define BRACCIO_AV_SX 0


//costanti gambe/piedi
#define GAMBA_AV_DX 30   
#define GAMBA_IND_DX -20
#define GAMBA_AV_SX  50
#define GAMBA_IND_SX 0     
#define PIEDE_SU_DX 180
#define PIEDE_GIU_DX 90
#define PIEDE_SU_SX 0
#define PIEDE_GIU_SX 180

//costanti di posizionamento centrale dei servi per reggere sp4
#define AVANBRACCIO_CENTRO_DX 10
#define AVANBRACCIO_CENTRO_SX 10   //controllare
#define PIEDE_CENTRO_DX 160
#define PIEDE_CENTRO_SX 20

//costanti che determinano gli ingressi (pulsanti/ostacolo)
#define OSTACOLO_DX_AV  51
#define OSTACOLO_DX_IND 49
#define OSTACOLO_SX_AV  47
#define OSTACOLO_SX_IND 43
#define PULS_ZAMPA_AV_SX  45
#define PULS_ZAMPA_IND_SX 41
#define PULS_ZAMPA_AV_DX  39
#define PULS_ZAMPA_IND_DX 37

//costanti di ritardo tra un movimento e l' altro
#define RITARDO 400         //imposta a 500msec il ritardo tra un movimento e l'altro
#define DELAY 1             //imposta a 1000msec.

//costanti per indicare la svolta
#define DX 1
#define SX 2

int pulsante=0;             //variabile per testare quale pulsante e' stato premuto
int passi=3;                //variabile che indica il numero di passi da fare in caso di urto contro ostacolo
 
//variabili per definire il senso della camminata del robot
int avanbraccio_giu_dx,avanbraccio_su_dx;
int avanbraccio_giu_sx,avanbraccio_su_sx;
int braccio_av_dx,braccio_ind_dx;
int braccio_av_sx,braccio_ind_sx;
int piede_giu_dx,piede_su_dx;
int piede_giu_sx,piede_su_sx;
int gamba_giu_dx,gamba_su_dx;
int gamba_giu_sx,gamba_su_sx;

int senso_avanti=0;   //variabile di stato per determinare il senso di marcia

void allinea_servi(void);
void move(Servo servo1, int pos_servo);
void inverti_senso(void);
int leggi_puls(int pulsante);

Servo braccio_dx;
Servo avanbraccio_dx;
Servo braccio_sx;
Servo avanbraccio_sx;
Servo gamba_dx;
Servo piede_dx;
Servo gamba_sx;
Servo piede_sx;

void setup() {
  //configurazione delle uscite PWM
  braccio_dx.attach(2);
  avanbraccio_dx.attach(3);
  braccio_sx.attach(4);
  avanbraccio_sx.attach(5);    //controllare forse il 12 funziona
  gamba_dx.attach(6);
  piede_dx.attach(7);
  gamba_sx.attach(11);
  piede_sx.attach(10);
  //configurazione degli 8 pulsanti;
  pinMode(51, INPUT);
  pinMode(49, INPUT);
  pinMode(47, INPUT);
  pinMode(45, INPUT);
  pinMode(43, INPUT);
  pinMode(41, INPUT);
  pinMode(39, INPUT);
  pinMode(37, INPUT);
}

 
void loop() {
  allinea_servi();    //chiama funzione di allineamento al centro servi per reggere sp4
  inverti_senso();    //imposta senso iniziale avanti 
  delay(5000);        //genera una temporizzazione di 5 sec prima di procedere con la camminata
  while(1)
  {
  while(passi!=0)
  {   
      //passo gamba sx
      move(piede_sx, piede_su_sx);               //(passo 1) alza piede sx
      delay(RITARDO);
      move(gamba_sx, GAMBA_AV_SX);              //(passo 2) gamba sx avanti         
      delay(RITARDO);
      move(piede_sx, piede_giu_sx);              //(passo 3) abbassa piede sx   
      delay(RITARDO);
      move(gamba_sx, GAMBA_IND_SX);               //(passo 4) gamba sx indietro   
      if(senso_avanti==0)                        //se sto avanzando in avanti non testare gli ostacoli che si trovano dietro
      {
        if(leggi_puls(OSTACOLO_SX_IND)==LOW)         {move(gamba_sx, GAMBA_AV_SX);  inverti_senso(); pulsante=OSTACOLO_SX_IND;};        //leggi pulsante ostacoli ind sx
      }
      delay(RITARDO);
      move(piede_sx, PIEDE_CENTRO_SX);           //(passo 5) piede sx centro 
      delay(RITARDO);
     
      //passo braccio sx
      move(avanbraccio_sx, avanbraccio_su_sx);   //(passo 6) avanbraccio sx su   non funziona     
      delay(RITARDO);
      move(braccio_sx, BRACCIO_AV_SX);          //(passo 7) braccio sx in avanti   
      if(senso_avanti==1)                          //se sto avanzando in avanti testa gli ostacoli che si trovano avanti
      {
        if(leggi_puls(OSTACOLO_SX_AV)==LOW)         {move(braccio_sx, BRACCIO_IND_SX);  inverti_senso(); pulsante=OSTACOLO_SX_AV;};    //leggi pulsante ostacoli av sx
      }
      delay(RITARDO);
      move(avanbraccio_sx, avanbraccio_giu_sx);  //(passo 8) abbassa avanbraccio sx  non funziona
      delay(RITARDO); 
      move(braccio_sx, BRACCIO_IND_SX);          //(passo 9) braccio sx indietro
      delay(RITARDO);
      move(avanbraccio_sx, AVANBRACCIO_CENTRO_SX);  //(passo 10) avanbraccio sx centro  non funziona
      delay(RITARDO);
     
      //passo gamba dx
      move(piede_dx, piede_su_dx);               //(passo 11) alza piede dx       
      delay(RITARDO);
      move(gamba_dx, GAMBA_AV_DX);               //(passo 12) gamba dx avanti     
      delay(RITARDO);
      move(piede_dx, piede_giu_dx);              //(passo 13) abbassa piede dx     
      delay(RITARDO);
      move(gamba_dx, GAMBA_IND_DX);             //(passo 14) gamba dx indietro
      if(senso_avanti==0)                          //se sto avanzando in indietro testa gli ostacoli che si trovano indietro
      { 
        if(leggi_puls(OSTACOLO_DX_IND)==LOW)         {move(gamba_dx, GAMBA_AV_DX);  inverti_senso(); pulsante=OSTACOLO_DX_IND;};    //leggi pulsante ostacoli ind dx
      }
      delay(RITARDO);
      move(piede_dx, PIEDE_CENTRO_DX);           //(passo 15) piede dx centro     
      delay(RITARDO);
     
      //passo braccio dx
      move(avanbraccio_dx, avanbraccio_su_dx);   //(passo 16) alza avanbraccio dx
      delay(RITARDO);
      move(braccio_dx, BRACCIO_AV_DX);          //(passo 17) braccio dx avanti
      if(senso_avanti==1)                          //se sto avanzando in avanti testa gli ostacoli che si trovano avanti
      {
        if(leggi_puls(OSTACOLO_DX_AV)==LOW)         {move(braccio_dx, BRACCIO_AV_DX);  inverti_senso(); pulsante=OSTACOLO_DX_AV;};    //leggi pulsante ostacoli av dx
      }
      delay(RITARDO);
      move(avanbraccio_dx, avanbraccio_giu_dx);    //(passo 18) abbassa avanbraccio dx
      delay(RITARDO);
      move(braccio_dx, BRACCIO_IND_DX);            //(passo 19) braccio dx indietro 
      delay(RITARDO);
      move(avanbraccio_dx, AVANBRACCIO_CENTRO_DX); //(passo 20) braccio dx centro
      delay(RITARDO);
     
      //passo braccio sx
      move(avanbraccio_sx, avanbraccio_su_sx);    //(passo 21) avanbraccio sx su   non funziona     
      delay(RITARDO);
      move(braccio_sx, BRACCIO_AV_SX);            //(passo 22) braccio sx avanti
      if(senso_avanti==1)                            //se sto avanzando in avanti testa gli ostacoli che si trovano avanti
      {
        if(leggi_puls(OSTACOLO_SX_AV)==LOW)         {move(braccio_sx, BRACCIO_IND_SX);  inverti_senso(); pulsante=OSTACOLO_SX_AV;};    //leggi pulsante ostacoli av sx
      }
      delay(RITARDO);
      move(piede_sx, avanbraccio_giu_sx);        //(passo 23) abbassa avanbraccio sx   non funziona
      delay(RITARDO);
      move(braccio_sx, BRACCIO_IND_SX);           //(passo 24)  braccio sx indietro 
      delay(RITARDO);
      move(piede_sx, AVANBRACCIO_CENTRO_SX);     //(passo 25)  avanbraccio sx centro     non funziona
      delay(RITARDO); 
      //dopo la chiamata di svolta() viene impostato un numero di passi di 3
      //if(passi==3)           pulsante=0;   //riabilita variabile pulsante ostacolo premuto
      //se è stato premuto uno dei pulsanti rilevamento ostacoli decrementa passi in modo da fare 5 passi (passi=5) indietro
      //svolta(SX);
        if(pulsante==OSTACOLO_SX_AV)      svolta(DX);
        if(pulsante==OSTACOLO_DX_AV)      svolta(SX);
        if(pulsante==OSTACOLO_SX_IND)     svolta(DX);
        if(pulsante==OSTACOLO_DX_IND)     svolta(SX);
     
      }
      //passi=3;
      }
      pulsante=0;     
}



//funzione di allineamento nella posizione centrale dei servi su/giu
void allinea_servi(void)
{
    move(avanbraccio_sx, AVANBRACCIO_CENTRO_SX);    //metti servo avanbraccio sx al centro
    move(avanbraccio_dx, AVANBRACCIO_CENTRO_DX);    //metti servo avanbraccio dx al centro
    move(piede_sx, PIEDE_CENTRO_SX);                //metti servo piede sx al centro
    move(piede_dx, PIEDE_CENTRO_DX);                //metti servo piede dx al centro
    move(gamba_dx, GAMBA_IND_DX);                   //metti servo gamba dx indietro
    move(gamba_sx, GAMBA_IND_SX);                   //metti servo gamba sx indietro
    move(braccio_dx,BRACCIO_AV_DX);                 //metti servo braccio dx avanti
    move(braccio_sx,BRACCIO_AV_SX);                 //metti servo braccio sx avanti
}

//funzione di spostamento servi
void move(Servo servo1, int pos_servo){
        //metti servo nella posizione del punto morto
        servo1.write(pos_servo);
}




//funzione per invertire il senso di marcia
void inverti_senso(void)
{
  //ad ogni chiamata della funzione inverti la var di stato che imposta il senso di marcia
  if(senso_avanti==1)    senso_avanti=0;
  else senso_avanti=1;
  //se var di stato è 1
  if(senso_avanti==1)   
  {
      //imposta variabili per andare avanti
      piede_su_sx=PIEDE_SU_SX;
      piede_giu_sx=PIEDE_GIU_SX;
      piede_su_dx=PIEDE_SU_DX;
      piede_giu_dx=PIEDE_GIU_DX;
      avanbraccio_su_sx=AVANBRACCIO_SU_SX;
      avanbraccio_giu_sx=AVANBRACCIO_GIU_SX;
      avanbraccio_su_dx=AVANBRACCIO_SU_DX;
      avanbraccio_giu_dx=AVANBRACCIO_GIU_DX;
  }
  else
  {
      //se var è 0
      //imposta variabili per andare indietro
      piede_su_sx=PIEDE_GIU_SX;
      piede_giu_sx=PIEDE_SU_SX;
      piede_su_dx=PIEDE_GIU_DX;
      piede_giu_dx=PIEDE_SU_DX;
      avanbraccio_su_sx=AVANBRACCIO_GIU_SX;
      avanbraccio_giu_sx=AVANBRACCIO_SU_SX;
      avanbraccio_su_dx=AVANBRACCIO_GIU_DX;
      avanbraccio_giu_dx=AVANBRACCIO_SU_DX; 
  }
}

//funzione di svolta a dx o a sx
void svolta(int senso)
{
    //se il senso comandato è a dx vai a dx
    if(senso==DX)
    {
      //piede_su_sx=PIEDE_SU_SX;
      //piede_giu_sx=PIEDE_GIU_SX;
      //avanbraccio_su_sx=AVANBRACCIO_SU_SX;
      //avanbraccio_giu_sx=AVANBRACCIO_GIU_SX;
      piede_su_dx=PIEDE_GIU_DX;
      piede_giu_dx=PIEDE_SU_DX;
      avanbraccio_su_dx=AVANBRACCIO_GIU_DX;
      avanbraccio_giu_dx=AVANBRACCIO_SU_DX;
    }
    //se il senso comandato è a sx vai a sx
    if(senso==SX)
    {
      piede_su_sx=PIEDE_GIU_SX;
      piede_giu_sx=PIEDE_SU_SX;
      avanbraccio_su_sx=AVANBRACCIO_GIU_SX;
      avanbraccio_giu_sx=AVANBRACCIO_SU_SX;
      piede_su_dx=PIEDE_SU_DX;
      piede_giu_dx=PIEDE_GIU_DX;
      avanbraccio_su_dx=AVANBRACCIO_SU_DX;
      avanbraccio_giu_dx=AVANBRACCIO_GIU_DX;
    }
    passi=3; //imposta i passi successivi a 3 in modo da fare tre passsi a dx o sx
}

//funzione lettura pulsanti/ostacoli
int leggi_puls(int pulsante)
{
  int count;
  //testa pulsante finecorsa dentro l' intervallo di tempo di 1000msec.
  //in questo modo si fa si che la camminata possa continuare pure dopo il test del pulsante
  for(count=0;count<DELAY;count+=1)
  {
      delay(1000);
      if(digitalRead(pulsante)==LOW)
      {
        return LOW;
      }
      return HIGH;
  }
}


Nessuno mi può aiutare a capire cosa è successo? Non so come si utilizza il debuger interno di arduino
quindi se qualcuno è cosi gentile da aiutarmi a capire come si attiva gliene sarei grato.

[Leonardo]

Non esiste un debugger interno nell'IDE Arduino. Puoi utilizzare dei led o la porta seriale per cercare di fare il punto del programma.

Il pulsante fisicamente è ok o è danneggiato?

[alieno75]

pulsante e' un variabile che non fa capo a nessun pulsante: tutti i pulsanti sono ok già testati.
Testati pure gli ingressi.
Il problema è che senza debuger non posso vedere che valori assume la var pulsante.

[deluca]

si che lo puoi fare !!

o invii il valore sulla seriale, oppure copi il valore su un altro pin/port di uscita e visualizzi il valore pilotando semplicemente un led o una serie di led

[alieno75]

Ho scaricato la seguente libreria: ---> https://github.com/ivanseidel/DueTimer/releases che serve a gestire l' interrupt da timer per ARDUINODUE ma non capisco dove si mette il codice da eseguire quando il timer
finisce di contare e manda un segnale al piedino di INT.
Giovanni fammi sapere.

[deluca]

alieno75
non mi è chiaro quando dici che "il timer finisce di contare e manda il segnale al piedino INT." !!
questa operazione viene eseguita in automatico?

cmq, il codice che vuoi eseguire sull'overflow del timer in questione,
deve essere inserito nella chiamata all'handler dell'interrupt timer.

[alieno75]

Si hai ragione viene eseguita in automatico.
Ho installato le librerie DueTimer scaricate qui ---> https://github.com/ivanseidel/DueTimer/releases
poi ho aggiunto questo codice come indicato nel README:

Code: Select all

//funzione di risposta dell' interrupt del rilevamento pulsanti pavimento  (alzata robot)
void pavimento()
{
  if(leggi_puls(PULS_ZAMPA_IND_SX)==HIGH)
  {
     
           
  }
}


void setup() {
  //configurazione delle uscite PWM
  braccio_dx.attach(2);
  avanbraccio_dx.attach(3);
  braccio_sx.attach(4);
  avanbraccio_sx.attach(9);   
 
  gamba_dx.attach(6);
  piede_dx.attach(7);
  gamba_sx.attach(11);
  piede_sx.attach(10);
 
  //configurazione degli 8 pulsanti;
  pinMode(51, INPUT);
  pinMode(49, INPUT);
  pinMode(47, INPUT);
  pinMode(45, INPUT);
  pinMode(43, INPUT);
  pinMode(41, INPUT);
  pinMode(39, INPUT);
  pinMode(37, INPUT);
 
  //configurazione interrupt rilevazione alzata robot
  Timer6.attachInterrupt(pavimento);
  Timer6.start(100000);         //rileva pulsanti ogni 100msec.
}


Una volta compilato e trasferito noto che i servi del robot non si muovono piu e sono flosci e non ricevono segnale.
Se commento l' if() in mezzo alla funzione dell' handler tutto torna a funzionare ed il robot si muove.
Il mio scopo e' quello di far si che la funzione di interrupt testi se il robot e' appoggiato per terra o sollevato per aria, nel secondo caso deve fermare la camminata, non appena viene rimesso per terra torna a camminare.
Non capisco perche succede che i motori non ricevono segnale se inserisco del codice nella funzione.