Line Follower (scritto codice - chiedo giudizio e consigli per ottimizzarlo)

Salve a tutto il forum,
sotto riporto il codice del line follower da me scritto e realizzato utilizzando tre sensory CNY70 montati nella parte anteriore del robottino " Ardusumo".
Vi chiedo di darmi qualche consiglio su come ottimizzare il codice e verificare se ho scritto correttamente il PID (penso di aver fatto qualche casino). Il robotino funziona, segue la linea‚Ķma credo che sia un caso! :smiley: inotre vorrei qualche consiglio da voi per estrapolare in maniera pi√Ļ elegante l‚Äôerrore di inseguimento‚Ķse avrete pazienza di dare uno sguardo sotto‚Ķ
Il codice di solito lo divido in shell perch√® mi resta pi√Ļ comodo‚Ķ
Ci sono tre shell…Main,Define e Regolatore…
Fatemi sapere cosa ne pensate.
Ciao A.
P.S. C’è anche un breve video…

DEFINE

//*************************************************
//                                                *
//                PINOUT                          *
//                                                *
//*************************************************


//piedinatura
int STBY = 4;              //pin per stby motori OUT              (STBY)
int vel_mot_s = 5;         //pin velocità motore sinistro OUT       (PWMA)
int vel_mot_d = 10;        //pin velocità motore destro   OUT       (PWMB)
int dir_A_mot_s = 7;       //pin A direzione motore sinistro OUT   (AIN2)
int dir_B_mot_s = 6;       //pin B direzione motore sinistro OUT   (AIN1)
int dir_A_mot_d = 8;       //pin A direzione motore destro OUT     (BIN1)
int dir_B_mot_d = 9;       //pin B direzione motore destro OUT     (BIN2)

int inputSirL = 13;        //ir linea left
int inputSirC = 3;         //ir linea center
int inputSirR = 2;         //ir linea right

//*************************************************
//                                                *
//                VARIABILI                       *
//                                                *
//*************************************************

unsigned long old_time = 0;
float err_in = 0;
float err_old = 0;
//float Kp = 150;
//float Ki = 0.5;
//float Kd = 1.1;
//float Kp = 100;
//float Ki = 1;
//float Kd = 0.5;  // ok per 0.4
//float Kp = 160;
//float Ki = 1.2;
//float Kd = 0.5;  // ok per 0.6
float Kp = 150;
float Ki = 100;
float Kd = 200;  // ok per 
float P = 0;
float I = 0;
float D = 0;
float err_out = 0;
unsigned long dt=0;
int integral=0;
float gasS=0;
float gasD=0;
float gasSx=0.5;
float gasDx=0.5;
int left=0;
int center=0;
int right=0;
int sir_stimL;
int sir_stimC;
int sir_stimR;

MAIN

void setup() {
  //preset
  Serial.begin(9600);            // initialize serial communication with computer

  pinMode(vel_mot_s,OUTPUT);
  pinMode(vel_mot_d,OUTPUT);
  pinMode(dir_A_mot_s,OUTPUT);
  pinMode(dir_B_mot_s,OUTPUT);
  pinMode(dir_A_mot_d,OUTPUT);
  pinMode(dir_B_mot_d,OUTPUT);
  pinMode(STBY,OUTPUT);

  pinMode(inputSirL,INPUT);
  pinMode(inputSirC,INPUT);   
  pinMode(inputSirR,INPUT);

  digitalWrite(STBY,HIGH);

  delay(5000);
} 

void loop() {

  regolatore();

}

REGOLAOTRE

//*************************************************
//                                                *
//                REGOLATORE                      *
//                                                *
//************************************************* 
 
 void regolatore(){ 
 
  sir_stimR=digitalRead(inputSirR); // LEGGO I TRE SENSORI
  sir_stimC=digitalRead(inputSirC);
  sir_stimL=digitalRead(inputSirL);
 
  center =!sir_stimC;  // LI NEGO E ASSEGNO LORO UN NOME
  left    =!sir_stimL;
  right   =!sir_stimR;
   
   
 // CALCOLO L'ENTITA' DELL'ERRORE
 
  if ((left==1)&& (center==0)&& (right==0))
     {
       err_in=0.333;
      }
   
   if ((left==1)&& (center==1)&& (right==0))
     {
       err_in=0.166;
      }

   if ((left==0)&& (center==1)&& (right==0))
     {
       err_in=0;
      }
   if ((left==0)&& (center==1)&& (right==1))
     {
       err_in=-0.166;
      }
   if ((left==0)&& (center==0)&& (right==1))
     {
       err_in=-0.333;
      }
      
 // *******************PID*******************

dt= old_time - millis();
P = (err_in*Kp/250);
I = ((I+ err_in*Ki)/250/*dt*/);
D = (Kd*(err_in-err_old)/250/*dt*/);
err_old = err_in;    
err_out = P+I*D;

//  saturo erroRe
  if (err_out >1)
    {
      err_out = 1;
    }
  if (err_out < -1)
    {
      err_out = -1;
    }
    
//******************************************

 gasS=gasSx-err_out; // PASSO ERRORE ALLE VARIABILI DEL MOTORE
 gasD=gasDx+err_out; 
 
//******************************************

// ATTUO LA CORREZIONE AI MOTORI


  if (gasS > 1)
   {
     gasS=1;
    }
 if (gasS < -1)
 {
     gasS=-1;
    }
 if (gasS > 0)
    {
      digitalWrite(dir_A_mot_s,HIGH); // Motore sx gira Avanti
      digitalWrite(dir_B_mot_s,LOW); 
     }
 if (gasS < 0)
    {
      digitalWrite(dir_A_mot_s,LOW); // Motore sx gira Avanti
      digitalWrite(dir_B_mot_s,HIGH); 
     }
     
analogWrite(vel_mot_s,(abs(gasS)*250));

  if (gasD > 1)
   {
     gasD=1;
    }
 if (gasD < -1)
   {
     gasD=-1;
    }   
  if (gasD > 0)
    {
      digitalWrite(dir_A_mot_d,LOW); // Motore sx gira Avanti
      digitalWrite(dir_B_mot_d,HIGH); 
     }
 if (gasD < 0)
    {
      digitalWrite(dir_A_mot_d,HIGH); // Motore sx gira Avanti
      digitalWrite(dir_B_mot_d,LOW); 
     }
     
analogWrite(vel_mot_d,(abs(gasD)*250)); 

}

Filmat1.mp4 (1.95 MB)

nessuno mi vuol consigliare!?... =(

Non è bello fare un up dopo appena 12 ore...

Non entro nel merito del PID perché non ho le competenze per analizzare questo algoritmo, lato software posso dirti che mescoli spesso assegnazioni di numeri con decimali e numeri con interi a variabili di tipo float, cosa che sarebbe meglio evitare per non confondere il compilatore in alcune situazioni e trovarsi casting tra tipi diversi fatti in automatico. Esempio:

P = (err_in*Kp/250);

deve diventare:

P = (err_in*Kp/250.0);

Ci sono altri casi simili nel codice.

alexsgv forse se metti un bel po di commenti √© pi√ļ semplice leggere il codice e percui aiutarti. Ciao Uwe

leo72: Non è bello fare un up dopo appena 12 ore...

Non entro nel merito del PID perché non ho le competenze per analizzare questo algoritmo, lato software posso dirti che mescoli spesso assegnazioni di numeri con decimali e numeri con interi a variabili di tipo float, cosa che sarebbe meglio evitare per non confondere il compilatore in alcune situazioni e trovarsi casting tra tipi diversi fatti in automatico. Esempio:

P = (err_in*Kp/250);

deve diventare:

P = (err_in*Kp/250.0);

Ci sono altri casi simili nel codice.

mi puoi spiegare meglio questa cosa? alla fine devo tirare fuori un numero senza virgola per passarlo al pwm per i motori...però i calcoli dentro il PID li volevo fare con la virgola. Se mi puoi anche solo reindirizzare ad un articolo che spiega meglio come gestire questa cosa, te ne sarei grato. A.

uwefed: alexsgv forse se metti un bel po di commenti √© pi√ļ semplice leggere il codice e percui aiutarti. Ciao Uwe

Scusa, hai ragione. Ok, commento tutto e poi riposto il codice aggiornato!

Sotto riporto il codice con un p√≤ pi√Ļ di commenti:

DEFINE

//*************************************************
//                                                *
//                PINOUT                          *
//                                                *
//*************************************************


//piedinatura
int STBY = 4;              //pin per stby motori OUT              (STBY)
int vel_mot_s = 5;         //pin velocità motore sinistro OUT       (PWMA)
int vel_mot_d = 10;        //pin velocità motore destro   OUT       (PWMB)
int dir_A_mot_s = 7;       //pin A direzione motore sinistro OUT   (AIN2)
int dir_B_mot_s = 6;       //pin B direzione motore sinistro OUT   (AIN1)
int dir_A_mot_d = 8;       //pin A direzione motore destro OUT     (BIN1)
int dir_B_mot_d = 9;       //pin B direzione motore destro OUT     (BIN2)

int inputSirL = 13;        //ir linea left
int inputSirC = 3;         //ir linea center
int inputSirR = 2;         //ir linea right

//*************************************************
//                                                *
//                VARIABILI                       *
//                                                *
//*************************************************

unsigned long old_time = 0; //Varianile in cui salvo il vecchio valore di millis() per la parte derivativa
float err_in = 0;           //Variabile in cui scrivo il valore di errore in ingresso
float err_old = 0;          //Variabile in cui salvo il vecchio errore per la parte derivativa
//float Kp = 150;           //Coef. della parte Proporzionale
//float Ki = 0.5;           //Coef. della parte Integrale
//float Kd = 1.1;           //Coef. della parte Derivativa
//float Kp = 100;
//float Ki = 1;
//float Kd = 0.5;  // ok per 0.4
//float Kp = 160;
//float Ki = 1.2;
//float Kd = 0.5;  // ok per 0.6
float Kp = 150;
float Ki = 100;
float Kd = 200;  // I valori sono ancora da aggiustare... 
float P = 0;     // Variaile in cui salvo la parte Proporzionale
float I = 0;     // Variaile in cui salvo la parte Integrale
float D = 0;     // Variaile in cui salvo la parte Derivativa
float err_out = 0; // Varibile dove salvo la somma di P+I+D
unsigned long dt=0; //Varibile dove salvo il dt per la parte derivativa
int integral=0;    // Varibile dove salvo il valore integrale
float gasS=0;      // Varibile dove passo il valore della correzione per il motore SX
float gasD=0;      // Varibile dove passo il valore della correzione per il motore DX
float gasSx=0.5;   // Varibili dove assegno un valore di partenza per i motori per far andare il robottino avanti
float gasDx=0.5;
int left=0;        //Variabile a cui assegno il valore del sensore SX negato
int center=0;      //Variabile a cui assegno il valore del sensore CX negato
int right=0;       //Variabile a cui assegno il valore del sensore DX negato
int sir_stimL;     //Variabile a cui assegno il valore del sensore SX
int sir_stimC;     //Variabile a cui assegno il valore del sensore CX
int sir_stimR;     //Variabile a cui assegno il valore del sensore DX

MAIN

void setup() {
  //preset
  Serial.begin(9600);            // initialize serial communication with computer

  pinMode(vel_mot_s,OUTPUT);     //PWM per il motore SX
  pinMode(vel_mot_d,OUTPUT);     //PWM per il motore DX
  pinMode(dir_A_mot_s,OUTPUT);   //Con questi due pin seleziono il senso di rotazione del motore (cambio la configurazione del ponte nel Driver)
  pinMode(dir_B_mot_s,OUTPUT);
  pinMode(dir_A_mot_d,OUTPUT);   //Con questi due pin seleziono il senso di rotazione del motore (cambio la configurazione del ponte nel Driver)
  pinMode(dir_B_mot_d,OUTPUT);
  pinMode(STBY,OUTPUT);          //Con questo pin disabilito il Driver dei Motori

  pinMode(inputSirL,INPUT);      // Pin in ingresso per la lettura del Senosre CNY70 SX
  pinMode(inputSirC,INPUT);      // Pin in ingresso per la lettura del Senosre CNY70 CX 
  pinMode(inputSirR,INPUT);      // Pin in ingresso per la lettura del Senosre CNY70 DX

  digitalWrite(STBY,HIGH);       //Presetto il Driver ON 

  delay(5000);                   // Ritardo prima inizio programma
} 

void loop() {

  regolatore();                  //Chiamo la Funzione Regolatore
                                 //Nella shell Regolatore, acquisisco i 3 sensori e faccio i vari 
                                 //calcoli nel PID ed infine passo la correzione ai due motori

}

REGOLATORE

//*************************************************
//                                                *
//                REGOLATORE                      *
//                                                *
//************************************************* 
 
 void regolatore(){ 
 
  sir_stimR=digitalRead(inputSirR); // LEGGO I TRE SENSORI
  sir_stimC=digitalRead(inputSirC);
  sir_stimL=digitalRead(inputSirL);
 
  center =!sir_stimC;  // LI NEGO E ASSEGNO LORO UN NOME
  left    =!sir_stimL;
  right   =!sir_stimR;
   
   
 // CALCOLO L'ENTITA' DELL'ERRORE
 
  if ((left==1)&& (center==0)&& (right==0))  //all'interno di questi if determino l'entità dell'errore
     {                                       // positivo di valore xx se il diverge verso DX   
       err_in=0.333;                         // negativo se diverge verso SX
      }
   
   if ((left==1)&& (center==1)&& (right==0))
     {
       err_in=0.166;
      }

   if ((left==0)&& (center==1)&& (right==0))
     {
       err_in=0;
      }
   if ((left==0)&& (center==1)&& (right==1))
     {
       err_in=-0.166;
      }
   if ((left==0)&& (center==0)&& (right==1))
     {
       err_in=-0.333;
      }
      
 // *******************PID*******************

dt= old_time - millis();                        //salvo il dt tra il vecchio tempo e quello nuovo
P = (err_in*Kp/250);                            //Moltiplico l'errore per il Kp e divido per 250 per adattarlo al valore del PWM
I = ((I+ err_in*Ki)/250/*dt*/);                 //Moltiplico l'errore per il Ki e divido per 250 per adattarlo al valore del PWM
D = (Kd*(err_in-err_old)/250/*dt*/);            //Moltiplico l'errore per il Kd e divido per 250 per adattarlo al valore del PWM
err_old = err_in;                               //salvo il valore dell'errore per il prossimo calco derivativo 
err_out = P+I+D;                                // Sommo i tre contributi del regolatre

//  saturo errore
  if (err_out >1)
    {
      err_out = 1;
    }
  if (err_out < -1)
    {
      err_out = -1;
    }
    
//******************************************

 gasS=gasSx-err_out; // PASSO ERRORE ALLE VARIABILI DEL MOTORE
 gasD=gasDx+err_out; 
 
//******************************************

// ATTUO LA CORREZIONE AI MOTORI


  if (gasS > 1)
   {
     gasS=1;
    }
 if (gasS < -1)
 {
     gasS=-1;
    }
 if (gasS > 0)
    {
      digitalWrite(dir_A_mot_s,HIGH); // Se l'errore è maggiore di zero 
      digitalWrite(dir_B_mot_s,LOW);  // faccio girare il motore sx indietro
     }
 if (gasS < 0)
    {
      digitalWrite(dir_A_mot_s,LOW);  // Se l'errore è minore di zero 
      digitalWrite(dir_B_mot_s,HIGH); // faccio girare il motore sx avanti 
     }
     
analogWrite(vel_mot_s,(abs(gasS)*250)); //applico il nuovo valore di PWM al motore
                                        // ne prendo solo il modulo e lo moltiplico per 250.

  if (gasD > 1)
   {
     gasD=1;
    }
 if (gasD < -1)
   {
     gasD=-1;
    }   
  if (gasD > 0)
    {
      digitalWrite(dir_A_mot_d,LOW);   // Se l'errore è maggiore di zero 
      digitalWrite(dir_B_mot_d,HIGH);  // faccio girare il motore dx avanti
     }
 if (gasD < 0)
    {
      digitalWrite(dir_A_mot_d,HIGH); // Se l'errore è minore di zero
      digitalWrite(dir_B_mot_d,LOW);  // faccio girare il motore dx indietro
     }
     
analogWrite(vel_mot_d,(abs(gasD)*250)); //applico il nuovo valore di PWM al motore
                                        // ne prendo solo il modulo e lo moltiplico per 250.

}
if ((left==1)&& (center==0)&& (right==0))  //all'interno di questi if determino l'entità dell'errore
     {                                       // positivo di valore xx se il diverge verso DX   
       err_in=0.333;                         // negativo se diverge verso SX
      }
   
   if ((left==1)&& (center==1)&& (right==0))
     {
       err_in=0.166;
      }

   if ((left==0)&& (center==1)&& (right==0))
     {
       err_in=0;
      }
   if ((left==0)&& (center==1)&& (right==1))
     {
       err_in=-0.166;
      }
   if ((left==0)&& (center==0)&& (right==1))
     {
       err_in=-0.333;
      }

non é la stessa cosa di:

err_in =(left - right)/(1+center)/3;

ciao Uwe

uwefed:

if ((left==1)&& (center==0)&& (right==0))  //all'interno di questi if determino l'entità dell'errore

{                                       // positivo di valore xx se il diverge verso DX   
       err_in=0.333;                         // negativo se diverge verso SX
      }
   
   if ((left==1)&& (center==1)&& (right==0))
     {
       err_in=0.166;
      }

if ((left==0)&& (center==1)&& (right==0))
     {
       err_in=0;
      }
   if ((left==0)&& (center==1)&& (right==1))
     {
       err_in=-0.166;
      }
   if ((left==0)&& (center==0)&& (right==1))
     {
       err_in=-0.333;
      }



non é la stessa cosa di: 


err_in =(left - right)/(1+center)/3;




ciao Uwe

Yes thanks!!! molto pi√Ļ elegante!!! :sweat_smile:

Mentre sto riguardando il codice...

Ho aggiunto la correzione di leo72 per evitare casini con i decimali e ora mi sembra che risponda alle variazioni dopo la virgola. Grazie Leo72

Ho modificato la discriminazione dell'errore con la funzione suggeritmi da Uwe (perchè non m'era venuta in mente... =( ) Grazie Uwe

Ora volevo inserire la chiamata al regolatore dentro l'interrupt scatenato dalla saturazione di un timer... avrei per√≤ bisogno di capire quanto dura la funzione regolatore per scegliere quando chiamarla e lasciare un p√≤ di tempo per fare "du' cagate" nel main... che so...tipo passare un paio di letture sulla seriale, scrivere una procedura per abilitare una funzione di debug...cose simili. secondo voi qual'√® il metodo pi√Ļ efficace per stimare la durata di una funzione?! Avevo pensato di acquisire due volte il millis(). Una volta in ingresso e una volta in uscita, fare la sottrazione e vedere quanto √® il delta...cosa ne pensate?! Ciao Alex

Ho usato la funzione micros() perch√® millis() non era adatta‚Ķ da quel che vedo sembra che la funzione regolatore duri soltanto poco pi√Ļ di 200us‚Ķ (vedi allegato)

Ora arriva la parte difficile…come faccio per scatenare un interraput con un timer?!..
Buio pi√Ļ assoluto‚Ķ Pauraaaaa!!!

durata Regolatore.PNG

alexsgv: Ora arriva la parte difficile...come faccio per scatenare un interraput con un timer?!.. Buio pi√Ļ assoluto.... Pauraaaaa!!!!

La prima cosa è studiarsi BENE il datasheet del micro e capire COME si setta un timer e che tipo di interrupt può sollevare. Una volta capito questo, devi scrivere le cosiddette ISR, Interrupt Service Routine, o routine di gestione dell'interrupt. Vale a dire una porzione di codice che il microcontrollore va ad eseguire quando si scatena l'evento da te impostato.

Mesi fa ho scritto un paio di presentazioni in PDF per l'incontro di Bassano che si è tenuto a giugno, se cerchi sul forum quella discussione, nelle ultime pagine della stessa, troverai questi documenti. Puoi studiarli per avere una prima infarinatura.

La prima cosa è studiarsi BENE il datasheet del micro e capire COME si setta un timer e che tipo di interrupt può sollevare. Una volta capito questo, devi scrivere le cosiddette ISR, Interrupt Service Routine, o routine di gestione dell'interrupt. Vale a dire una porzione di codice che il microcontrollore va ad eseguire quando si scatena l'evento da te impostato.

Mesi fa ho scritto un paio di presentazioni in PDF per l'incontro di Bassano che si è tenuto a giugno, se cerchi sul forum quella discussione, nelle ultime pagine della stessa, troverai questi documenti. Puoi studiarli per avere una prima infarinatura.

Trovati! mo me li studio! Grazie Leo.

alexsgv: Ho modificato la discriminazione dell'errore con la funzione suggeritmi da Uwe (perchè non m'era venuta in mente... =( )

Controlla con un serial print il risultato della funzione. Se il compilatore fa autocasting e converte in intero potresti avere dalla divisione risultati inattesi. Nel caso, per risolvere, forza il casting oppure prova a dividere per 3.0 e non per 3.

PaoloP:

alexsgv: Ho modificato la discriminazione dell'errore con la funzione suggeritmi da Uwe (perchè non m'era venuta in mente... =( )

Controlla con un serial print il risultato della funzione. Se il compilatore fa autocasting e converte in intero potresti avere dalla divisione risultati inattesi. Nel caso, per risolvere, forza il casting oppure prova a dividere per 3.0 e non per 3.

Grazie Paolo. Avevo già controllato fino alla fine della catena e si porta dietro correttamente i decimali. Dopo la nota di @Leo72 avevo aggiunto la virgola a tutte le costanti.

A questo punto averi bisogno di un guru dei PID per avere qualche info in merito... Ciao Alex