Diff: main.cpp

Revision:

2:b94303d66b9d

Parent:

1:11cbd2bf65d7

--- a/main.cpp	Tue Mar 19 19:24:04 2019 +0000
+++ b/main.cpp	Thu Oct 10 10:04:43 2019 +0000
@@ -2,232 +2,573 @@
 #include "QEI.h"
 #include "Moteur.h"
 #include "math.h"
-
-Serial pc(SERIAL_TX,SERIAL_RX);
+#include "TransmitterIR.h"
 
-QEI encodeur_a (D2, D3, NC, 1200, QEI::X4_ENCODING);
-QEI encodeur_b (D4, D5, NC, 1200, QEI::X4_ENCODING);
-
+#include "tca9548a.h"
+#include "VL53L0X.h"
 
-/*pour 8v*/
-static float Pl = 0.01; //0.03
-static float Il = 0.0001; //0.001
-static float Dl = 0;
+#define SDA_pin D14
+#define SCL_pin D15
 
-static float Pa = 0.01; //0.01
-static float Ia = 0.0001; //0.0001
-static float Da = 0;
+#define NB_CAPTEUR_TOF 3
+
+#define PI 3.14159265359
 
 #define LINEIQUE 0
 #define ANGULAIRE 1
 
-#define FREQ_CORRECTION 0.01
+#define FREQ_CORRECTION 0.008
 #define ERR_STATIQUE 100
 #define LIMITE_TEMPS 1/FREQ_CORRECTION /*3 secondes*/
 
-#define DISTANCE_ENTRE_ROUES 270 /*en mm*/
-#define DIMENSION_ROUE_A 70 /*en mm*/
-#define DIMENSION_ROUE_B 70 /*en mm*/
+#define DISTANCE_ENTRE_ROUES 195.0 /*en mm*/ /*179 ? */
+#define DIMENSION_ROUE_A 70.0 /*en mm*/
+#define DIMENSION_ROUE_B 70.0 /*en mm*/
 #define TIC_PAR_TOUR_A 1200
 #define TIC_PAR_TOUR_B 1200
 
-#define LIMITE_VITESSE 0.5
+#define LIMITE_VITESSE ((float) 0.5)
+
+#define COEF_DIV ((float) 0.9)
+
+#define VITESSE_MIN ((float) 0.2)
+
+#define DISTANCE_STOP 180
+
+TCA9548A i2cMux(SDA_pin,SCL_pin);
+
+Serial pc(SERIAL_TX, SERIAL_RX); //9600 bauds
+VL53L0X *ToF_Capteur[NB_CAPTEUR_TOF];
+int32_t ToF_Mesure[NB_CAPTEUR_TOF];
+
+
+QEI encodeur_a (D2, D3, NC, 1200, QEI::X4_ENCODING);
+QEI encodeur_b (D4, D5, NC, 1200, QEI::X4_ENCODING);
+Ticker timer;
+DigitalIn way(PC_8,PullDown);
+DigitalIn languette(D12,PullDown);
+TransmitterIR led(PB_7);
+
+int cptcapt=0;
+int attention=0;
+/*pour 8v*/
+static float Pl = 0.01; //0.03
+static float Il = 0.002; //0.001
+static float Dl = 0.01;
+
+static float Pa = 0.03; //0.01
+static float Ia = 0.001; //0.0001
+static float Da = 0.001;
+
+static float errPrev = 0;
+static float errSum = 0;
+static float errDif = 0;
+
+
 
 void pos(int commande_a, int commande_b);
 void pos_a(int commande_a);
 void pos_b(int commande_b);
 void deplacement(int theta, int r);
 float PID(float erreur,float type);
- 
-int main() {
+void event_b();
+void asservissement();
+void err_clear();
+void enc_reset();
+void commande_ligne( float commande_dist);
+void commande_rot( float commande_deg);
+void glissiere(int i);
+void experience();
+
+
+float nbmicrodep=0.0;
+
+int flagbutton=0;
+int flag2=0;
+
+int cpt=0;
+
+
+InterruptIn button(USER_BUTTON);
+
+float dist = 0.0;
+float distmp;
+float orientation_deg = 0.0;
+
+float pulse_a;
+float pulse_b;
+
+float pid_a;
+float pid_b;
+
+float pid_dist;
+float pid_ori;
+
+float err_dist;
+float err_ori;
 
-    float distance = 0;
-    float orientation_deg = 90;
-    
-    float pulse_a;
-    float pulse_b;
-    
-    float pid_a;
-    float pid_b;
-    
-    float pid_dist;
-    float pid_ori;
-    
-    float err_dist;
-    float err_ori;
-    
-    float n_commande_a;
-    float n_commande_b;
+float n_commande_a;
+float n_commande_b;
+
+float res_a = DIMENSION_ROUE_A*3.14/TIC_PAR_TOUR_A;
+float res_b = DIMENSION_ROUE_B*3.14/TIC_PAR_TOUR_B;
+
+float orientation_dist;
+
+int main()
+{
+    printf("bjr \n");
+
+
+    //printf("%f\n\r",orientation_dist);
+    printf("hello\n");
+    button.rise(&event_b);
+
+    moteur_init();
+
+    //initialisation des capteurs
+    printf("hello2\n");
+
+
+    // Initialisation des capteurs ToF
+
+
+    printf("hello3\n");
+
+    timer.attach(&asservissement,FREQ_CORRECTION);
+
+
+    //while(1) {
+
+
+    // printf("%i - %i - %i - %i - %i  \n \r",ToF_Mesure[0],ToF_Mesure[1],ToF_Mesure[2],ToF_Mesure[3],ToF_Mesure[4]);
+    //printf("%i \r",ToF_Mesure[0]);
+
+    //}
     
-    float res_a = (float)DIMENSION_ROUE_A*3.14/(float)TIC_PAR_TOUR_A;
-    float res_b = (float)DIMENSION_ROUE_B*3.14/(float)TIC_PAR_TOUR_B;
-    
-    float orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * 3.14 / 360;
-    
-    printf("%f\n\r",orientation_dist);
-    
-    moteur_init();
-    
-    while(1)
+    while (!languette){wait(0.01);}
+    experience();
+    for(int i=1 ; i<NB_CAPTEUR_TOF ; i++) {
+        printf(" initialisation du capt : %d \n \r ",i);
+
+        i2cMux.select(i);
+        ToF_Capteur[i] = new VL53L0X(SDA_pin,SCL_pin);
+        ToF_Capteur[i]->init();
+        ToF_Capteur[i]->setTimeout(50);
+        ToF_Capteur[i]->startContinuous();
+
+    }
+    if (way)
     {
-        pulse_a = (float)encodeur_a.getPulses() * res_a;
-        pulse_b = (float)encodeur_b.getPulses()* res_b;
-        
-        err_dist = distance - (pulse_a + pulse_b) /2;
-        err_ori = orientation_dist - (pulse_a - pulse_b) /2;   
+
+        commande_ligne(350);
+        wait(0.2);
+        commande_rot(90);
+        wait(0.2);
+        commande_ligne(1100);
+        wait(0.2);
+        commande_rot(170);
+        glissiere(2);
+        wait(0.2);
+        commande_ligne(-500);
+        wait(0.2);
+        glissiere(1);
+    }
+
+    else 
+    {
         
-        printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
-        
-        pid_dist = PID(err_dist,LINEIQUE);
-        pid_ori = PID(err_ori,ANGULAIRE);
-        
-        pid_a = - pid_dist + pid_ori;
-        pid_b = - pid_dist - pid_ori;
+    
+        //wait(0.5);
+        //commande_rot(-90);
+        //wait(0.2);
+        //glissiere(2);
+        //glissiere(1);
+        commande_ligne(350);
+        wait(0.2);
+        commande_rot(-90);
+        wait(0.2);
+        commande_ligne(1100);
+        wait(0.2);
+        glissiere(2);
+        wait(0.2);
+        commande_ligne(1500);
+        wait(0.2);
+        glissiere(1);
         
         
-        //printf("%f\n\r",pulse_a);
-        
-        n_commande_a = pid_a;
-        n_commande_b = pid_b;
-        
-        
-        if (n_commande_a>1)
-            n_commande_a=LIMITE_VITESSE;
-        else if (n_commande_a<-LIMITE_VITESSE)
-            n_commande_a = -LIMITE_VITESSE;
-        
-        if (n_commande_b>LIMITE_VITESSE)
-            n_commande_b=LIMITE_VITESSE;
-        else if (n_commande_b<-LIMITE_VITESSE)
-            n_commande_b = -LIMITE_VITESSE;
+    }
+
+
+
+    //wait(1);
+    //commande_ligne(1550);
+    //wait(1);
+    //glissiere(2);
+    //wait(1);
+    //commande_ligne(2250);
+    //wait(1);
+    //glissiere(1);
+
+    return 0;
+
+}
+
+float PID(float erreur,float type)
+{
+
+    float P,I,D;
+
+    errSum += erreur;
+
+    errDif = erreur - errPrev;
+    errPrev = erreur;
+
+    if (type) {
+        P = erreur * Pl;
+        I = errSum * Il;
+        D = errDif * Dl;
+    } else {
+        P = erreur * Pa;
+        I = errSum * Ia;
+        D = errDif * Da;
+    }
+
+    return P + I + D;                //Le résultat est la somme des trois
+    //composantes calculées précédemment
+}
+
+void event_b()
+{
+    button.disable_irq();
+    flagbutton =1;
+    button.enable_irq();
+}
+
+void asservissement ()
+{
+
+
+
+    /*
+            if (ToF_Mesure[0]<100)attention=1;
+    else attention=0;
+        */
+
+
+
+    //printf("%i - %i \n \r ",ToF_Mesure[cptcapt], cptcapt);
+
+    orientation_dist = orientation_deg * (float)DISTANCE_ENTRE_ROUES * (float)PI / 360.0;
+
+    pulse_a = (float)encodeur_a.getPulses() * res_a;
+    pulse_b = (float)encodeur_b.getPulses()* res_b;
+
+    err_dist = dist - (pulse_a + pulse_b) /2;
+    err_ori = orientation_dist - (pulse_a - pulse_b) ;
+
+    pid_dist = PID(err_dist,LINEIQUE);
+    pid_ori = PID(err_ori,ANGULAIRE);
+
+    pid_a = - pid_dist + pid_ori;
+    pid_b = - pid_dist - pid_ori;
+
+    n_commande_a = pid_a;
+    n_commande_b = pid_b;
+
+
+    if (n_commande_a>LIMITE_VITESSE) {
+        n_commande_a=LIMITE_VITESSE*COEF_DIV;
+        errSum=0;
+    } else if (n_commande_a<-LIMITE_VITESSE) {
+        n_commande_a = -LIMITE_VITESSE*COEF_DIV;
+        errSum=0;
+    }
+
+    if (n_commande_b>LIMITE_VITESSE) {
+        n_commande_b=LIMITE_VITESSE*COEF_DIV;
+        errSum=0;
+    } else if (n_commande_b<-LIMITE_VITESSE) {
+        n_commande_b = -LIMITE_VITESSE*COEF_DIV;
+        errSum=0;
+    }
+
+    if (abs(n_commande_a)<0.05) {
+        n_commande_a=0;
+    }
+    if (abs(n_commande_b)<0.05) {
+        n_commande_b=0;
+    }
+
+    if ((abs(n_commande_a)>0.05)&&(abs(n_commande_a)<VITESSE_MIN)) {
+        n_commande_a=VITESSE_MIN;
+    }
+    if ((abs(n_commande_b)>0.05)&&(abs(n_commande_b)<VITESSE_MIN)) {
+        n_commande_b=VITESSE_MIN;
+    }
+
+
+    n_commande_a*=COEF_DIV;
+    n_commande_b*=COEF_DIV;
+
+}
+
+void err_clear()
+{
+
+    errPrev = 0;
+    errSum = 0;
+    errDif = 0;
+}
+
+void enc_reset()
+{
+    encodeur_a.reset();
+    encodeur_b.reset();
+}
+
+void commande_ligne( float commande_dist)
+{
+
+
+    err_clear();
+    enc_reset();
+    dist=0;
+    orientation_deg=0;
+
+    int compteur=1;
+
+    nbmicrodep=abs(commande_dist)/20;
+
+    while (compteur< nbmicrodep) {
+               
+        flag2=0;
+        if (commande_dist<0) {
+            //printf("negatif\n\r");
+            dist=-compteur*20;
+        } else {
+            //printf("positif\n\r");
+            dist=compteur*20;
+        }
+       
+   for(int i=1 ; i<NB_CAPTEUR_TOF ; i++) {
+            i2cMux.select(i);
+            //wait_ms(50);
+            ToF_Mesure[i] = ToF_Capteur[i]->readRangeContinuousMillimeters();
+            //wait_ms(50);
+        }
+        // printf("%i  \n \r ",ToF_Mesure[0]);
+        if((ToF_Mesure[1]<DISTANCE_STOP)||(ToF_Mesure[2]<DISTANCE_STOP)) {
+
+            timer.detach();
+            //printf("bloqué \t");
+            moteur_a(0);
+            moteur_b(0);
+            while((ToF_Mesure[1]<DISTANCE_STOP)||(ToF_Mesure[2]<DISTANCE_STOP)) {
+                for(int i=1 ; i<NB_CAPTEUR_TOF ; i++) {
+                    i2cMux.select(i);
+                    //wait_ms(50);
+                    ToF_Mesure[i] = ToF_Capteur[i]->readRangeContinuousMillimeters();
+
+                    //wait_ms(50);
+                }
+                //printf("yooooooo");
+                //printf("%i  \n \r ",ToF_Mesure[0]);
+            }
+            //printf("débloqué \t");
+            timer.attach(&asservissement,FREQ_CORRECTION);
+            moteur_b(n_commande_b);
+            moteur_a(n_commande_a);
+
+             wait(0.001);
+        }
+
+        flag2=0;
+ 
+        //printf("commande_dist : %f \n \r",dist);
+
+        while((flag2==0) && (flagbutton==0)) {
+
+            //printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
+            //printf("%f\n\r",pulse_a);
+            //printf("%d\n\r",flag2);
             
-        
-        moteur_a(n_commande_a);
-        moteur_b(n_commande_b);
-        wait(FREQ_CORRECTION);     
-        
+          
+                
+            if ((err_dist<7 && err_dist>-7 ) && (err_ori<10 && err_ori>-10))
+                cpt++;
+            else {
+                cpt=0;
+            }
+
+            if (cpt > 1000*FREQ_CORRECTION) {
+                // micro déplacement terminé
+                flag2=1;
+                cpt=0;
+                err_clear();
+                
+                
+                //enc_reset();
+            }
+
+            if (!flag2 ) {
+                //printf("%1.4f,%1.4f,%1.4f,%1.4f,%1.4f,%1.4f,%d\r\n",err_dist,dist,n_commande_a,pulse_a,err_ori,commande_dist,cpt);
+                //printf("%1.4f,%1.4f\r\n",n_commande_a,n_commande_b);
+                //printf("%1.4f,%1.4f,%d\r\n",err_dist,err_ori,cpt);
+
+
+                //printf(" %f - %f  \n \r", n_commande_a , n_commande_b);
+                       //printf("c\t");
+                moteur_a(n_commande_a);
+                moteur_b(n_commande_b);
+                wait(0.001); // tester d'enlever ça
+
+            }
+
+
+        }
+        compteur++;
+        printf("%d\t",compteur);
+     //printf("d \t");
     }
-    
-    return 0;    
+    moteur_a(0);
+    moteur_b(0);
+    compteur=0;
+                    //printf("e \t");
 }
 
 
 
+void glissiere(int i)
+{
+    if (i==1) {
+        moteur_c(1);
+
+
+
 
-float PID(float erreur,float type)
-{ 
-    static float errPrev = 0;
-    static float errSum = 0;
-    static float errDif = 0;
- 
-    float P,I,D;
- 
-    errSum += erreur;                 
- 
-    errDif = erreur - errPrev;      
-    errPrev = erreur;
- 
-    if (type){
-        P = erreur * Pl;                  
-        I = errSum * Il;                 
-        D = errDif * Dl;                 
+    } else if(i==2) {
+        moteur_c(2);
+
     } else {
-        P = erreur * Pa;                  
-        I = errSum * Ia;                 
-        D = errDif * Da; 
+        moteur_c(0);
     }
- 
-    return P + I + D;                //Le résultat est la somme des trois
-                                     //composantes calculées précédemment
+
 }
 
+void commande_rot( float commande_deg)
+{
+
+
+    err_clear();
+    enc_reset();
+    dist=0;
+    orientation_deg=0;
+    int compteur=1;
+    nbmicrodep=2*(abs(commande_deg))/10;
+
+
+    while (compteur< nbmicrodep) {
+        
+            flag2=0;
+            if (commande_deg<0) {
+                orientation_deg=-compteur*10;
+            } else {
+                orientation_deg=compteur*10;
+            }
+
+            for(int i=1 ; i<NB_CAPTEUR_TOF ; i++) {
+            i2cMux.select(i);
+            //wait_ms(50);
+            ToF_Mesure[i] = ToF_Capteur[i]->readRangeContinuousMillimeters();
+            //wait_ms(50);
+        }
+        // printf("%i  \n \r ",ToF_Mesure[0]);
+        if((ToF_Mesure[1]<DISTANCE_STOP)||(ToF_Mesure[2]<DISTANCE_STOP)) {
+
+            timer.detach();
+
+            moteur_a(0);
+            moteur_b(0);
+            //printf("bloqué \t");
+            while((ToF_Mesure[1]<DISTANCE_STOP)||(ToF_Mesure[2]<DISTANCE_STOP)) {
+                for(int i=0 ; i<NB_CAPTEUR_TOF ; i++) {
+                    i2cMux.select(i);
+                    //wait_ms(50);
+                    ToF_Mesure[i] = ToF_Capteur[i]->readRangeContinuousMillimeters();
+                    //printf("%i  \n \r ",ToF_Mesure[i]);
+                }
+                wait(0.01);
+                //printf("%i  \n \r ",ToF_Mesure[0]);
+            }
+            //printf("débloqué \t");
+            timer.attach(&asservissement,FREQ_CORRECTION);
+
+            moteur_a(n_commande_a);
+            moteur_b(n_commande_b);
+             wait(FREQ_CORRECTION);
+        }
+            flag2=0;
+            while((flag2==0) && (flagbutton==0)) {
+         //printf("a \t");
+                //printf("%f\t%f\t%f\t%f\n\r",err_dist,err_ori,pulse_a,pulse_b);
+                //printf("%f\n\r",pulse_a);
+                //printf("%d\n\r",flag2);
+                
+
+       //printf("b \t");
+                if ((err_dist<7 && err_dist>-7 ) && (err_ori<10 && err_ori>-10))
+                    cpt++;
+                    
+                else {
+                    cpt=0;
+                }
+
+                if (cpt > 1000*FREQ_CORRECTION) {
+                    // micro déplacement terminé
+
+                    flag2=1;
+                    cpt=0;
+                    err_clear();
+                    //printf("c \t");
+                    //enc_reset();
+                }
+
+                if (!flag2) {
+                    //printf("%1.4f,%1.4f,%1.4f,%1.4f,%1.4f,%1.4%d\r\n",err_ori,orientation_deg,n_commande_a,pulse_a,err_ori,cpt);
+                    //printf("%1.4f,%1.4f\r\n",n_commande_a,n_commande_b);
+                    //printf("%1.4f,%1.4f,%d\r\n",err_dist,err_ori,cpt);
+                    //printf("d \t");
+
+                    //printf(" %f - %f  \n \r", n_commande_a , n_commande_b);
+                    moteur_a(n_commande_a);
+                    moteur_b(n_commande_b);
+                    wait(FREQ_CORRECTION); // tester d'enlever ça
+
+                }
+                //printf("%d\r\n",compteur);
 
 
 
-/*asservissement des deux roues individuellement*/
-    /*pas très utile pour le déplacement mais intéressant*/
-    /*de garder comme un exemple*/
-    
-    /*
-
-void pos(int commande_a, int commande_b)
-{
+            }
+            compteur++;
+            printf("%d\t",compteur);
+           // printf("e \t");
 
-    
-    
-    int erreur_a = 0;
-    int somme_erreur_a = 0;
-    
-    int derniere_erreur_a = 0;
-    int derivee_a = 0;    
-    
-    float n_commande_a;
-    
-    int validation_position_a = 0;
-    
-    int erreur_b = 0;
-    int somme_erreur_b = 0;
-    
-    int derniere_erreur_b = 0;
-    int derivee_b = 0;    
-    
-    float n_commande_b;
-    
-    int validation_position_b = 0;
-    
-    do
-    {
-        
-        erreur_a = encodeur_a.getPulses()-commande_a;
-        erreur_b = encodeur_b.getPulses()-commande_b;
-        
-        somme_erreur_a = somme_erreur_a + erreur_a;
-        somme_erreur_b = somme_erreur_b + erreur_b;
-        
-        derivee_a = erreur_a - derniere_erreur_a;
-        derivee_b = erreur_b - derniere_erreur_b;
-        
-        n_commande_a = erreur_a * P + somme_erreur_a * I + derivee_a * D;
-        n_commande_b = erreur_b * P + somme_erreur_b * I + derivee_b * D;
-        
-        derniere_erreur_a = erreur_a;
-        derniere_erreur_b = erreur_b;
-        
-        if (n_commande_a>1)
-            n_commande_a=1;
-        else if (n_commande_a<-1)
-            n_commande_a = -1;
-            
-        if (n_commande_b>1)
-            n_commande_b=1;
-        else if (n_commande_b<-1)
-            n_commande_b = -1;
-            
-        moteur_a(-n_commande_a);
-        moteur_b(-n_commande_b);
-        
-        if (abs(erreur_a)<ERR_STATIQUE)
-            validation_position_a++;
-        else
-            validation_position_a = 0;
-            
-        if (abs(erreur_b)<ERR_STATIQUE)
-            validation_position_b++;
-        else
-            validation_position_b = 0;
-        
-        wait(FREQ_CORRECTION);
-                
-    } while (validation_position_a<LIMITE_TEMPS || validation_position_b<LIMITE_TEMPS);
-    moteur_a(0);
-    moteur_b(0);
-    
-    encodeur_a.reset();
-    encodeur_b.reset();
-}
+        }
+        moteur_a(0);
+        moteur_b(0);
+        compteur=0;
+        //printf("f \t");
+    }
 
 
-
+void experience()
+{
+    uint8_t buf = 42;
+    for  (int i=0; i<10; i++) {
+        wait_ms(500);
+        led.setData(RemoteIR::SONY,&buf,8);
+    }
 
-
-*/
\ No newline at end of file
+}
\ No newline at end of file