Diff: main.cpp

Revision:

12:594a1b936f4b

Parent:

11:e227edfced99

Child:

13:bed77b03701d

--- a/main.cpp	Thu Jun 06 10:25:41 2019 +0000
+++ b/main.cpp	Thu Jun 06 12:04:13 2019 +0000
@@ -7,7 +7,10 @@
 Serial lidar(PC_6, PC_7, 115200);
 
 // Définition des variables globales
-float tableau_distance[360] = {};
+float tableau_distance0[360] = {};
+float tableau_distance1[360] = {};
+uint8_t tableau_en_cours = 0; //tableau en cours de remplissage
+uint8_t flag_fin_tour_lidar=0;
 int compteur_tours_lidar = 0;
 int affiche_lidar = 0;
 bool run = false;
@@ -23,74 +26,71 @@
 float distance(float x_1, float x_2, float y_1, float y_2)
 {
     // Fonction qui renvoie la distance entre deux points (norme 2)
-    float norm2;    
+    float norm2;
     norm2 = sqrt((x_1 - x_2)*(x_1 - x_2) + (y_1 - y_2)*(y_1 - y_2));
     return norm2;
 }
-   
-void update_direction(float* list_lidar, float* vecteur)
+
+void update_direction(float* list_lidar_var, float* vecteur)
 {
     //pc.printf("Update commence\n\r");
     // Fonction de mise à jour de la direction
     float direction[2];
-        int i;
-    
-    //pour les essais
-    for(i=0;i<360;i++)
-        list_lidar[i]=100;
-    ///////////////////
-    
-    
-    direction[0] = 0;
-    direction[1] = 1;
+    int i;
+    float list_lidar[360];
+    uint8_t liste_fictifs[360];
+    for(i=0; i<360; i++) list_lidar[i]=list_lidar_var[i];
+
+
+    direction[0] = 1;
+    direction[1] = 0;
     float avg_x, avg_y, sum_inv_dist;
     //list_lidar[180] = 50; // [mm], point fictif qui pousse la voiture
     //Définition des points fictifs poussant la voiture
-    int liste_fictifs[360];
-    for (int i=0; i<360; i++){
+
+    for (i=0; i<360; i++) {
         liste_fictifs[i] = 0;
-        }
-    for (int i=135; i<225; i++){
-    //for (int i=0; i<180; i++){ //test
-        liste_fictifs[i] = 1;
-        }
+    }
+    // for (i=135; i<=225; i++) {
+    //     //for (int i=0; i<180; i++){ //test
+    //     liste_fictifs[i] = 1;
+    // }
+    liste_fictifs[180] = 1;
 
     avg_x = 0;
     avg_y = 0;
-    
+
     // Calcul de la direction à prende en fonction des charges fictives
-    for (i=0; i<360; i++)
-        {
+    for (i=0; i<360; i++) {
         int theta;
         float r, x, y;
         theta = i;
-        if (liste_fictifs[theta] == 1){
-            //pc.printf("Angle,%i\n\r",theta);
+        if (liste_fictifs[theta] == 1) {
             r = 50;
-            }
-        else{
-            //pc.printf("Angle,%i\n\r",theta);
+        } else {
             //r = 0; //test
-            r = list_lidar[359-theta];
-            //pc.printf("r,%f\n\r",r);
+            if(theta != 0) r = list_lidar[360-theta];
+            else r = list_lidar[0];
+
             }
-        //pc.printf("Salut 1\n\r");
-        //pc.printf("%f\n\r",r);
-        if (r == 0) break; // non calcul en cas de distance nul (donnée non captée) 
-        //pc.printf("Salut 2\n\r");
-        //x = 0;
-        //y = 0; 
-        x = r*cosf(theta);
-        y = r*sinf(theta);
-        //sum_inv_dist += 1/pow(r, 2);
-        //avg_x -= x/pow(r,2);
-        //avg_y -= y/pow(r,2);
-        float puissance = 0.5*abs(cosf(2*theta)) + 1.5;
-        avg_x -= x/pow(r,puissance);
-        avg_y -= y/pow(r,puissance);
-        
+
+        if (r != 0) {
+
+            //x = 0;
+            //y = 0;
+            x = r*cosf((float)theta*3.14/180);
+            y = r*sinf((float)theta*3.14/180);
+            //sum_inv_dist += 1/pow(r, 2);
+            //avg_x -= x/pow(r,2);
+            //avg_y -= y/pow(r,2);
+            float puissance = 2;//0.5*abs(cosf(2*theta)) + 1.5;
+            avg_x = avg_x - x/pow(r,puissance);
+            avg_y = avg_y - y/pow(r,puissance);
+
+            //pc.printf("Angle:%i r:%4.2f x:%4.2f y:%4.2f %4.2f %4.2f\n\r", theta, r, x,y, avg_x,avg_y);
         }
-    
+    }
+
     //avg_x /= sum_inv_dist;
     //avg_y /= sum_inv_dist;
     direction[0] = avg_x;
@@ -101,12 +101,12 @@
         vecteur[i] = direction[i];
 
 }
-    
+
 float angle_servo(float *direction)
 {
     // Calcul basé sur la régression expérimental pour obetenir l'angle
     // le pwm à donner au moteur en fonction de l'angle voulue
-    
+
     float angle;
     double pwm;
     float x, y;
@@ -114,7 +114,7 @@
     y = direction[1];
     angle = atan(x/y);
     pwm = 14.662756 * angle*180/3.14 + 1453.08; // à refaire
-    
+
     //if (pwm < 1115) printf("trop petit\n\r");
     //if (pwm > 1625) printf("trop grand\n\r");
     //if (angle > 5*3.14/180){
@@ -128,30 +128,34 @@
     //        pwm = 1453;
     //        }
     //}
-    
+
     return pwm;
 }
 
-void afficher_lidar(float *tableau_distances)
+void afficher_lidar(float *tableau_distances_var)
 {
     //Affiche les données du lidar dans la liaison série
     int angle;
-    for(angle=0;angle<360;angle++){
+    float tableau_distances[360];
+    for(angle=0; angle<360; angle++) tableau_distances[angle]=tableau_distances_var[angle];
+
+    for(angle=0; angle<360; angle++) {
         float distance = tableau_distances[angle];
         pc.printf("%i,%f\n\r",angle,distance);
-        }
+    }
 }
 
-int main(){
-    
+int main()
+{
+
     pc.printf("\r-------------------------\n\r");
-    
+
     float dir[2]; // direction
     float pwm_direction_value;
-    
-    
+
+
     int i;
-  
+
 
 
 
@@ -159,7 +163,7 @@
     // pwm du LIDAR
     pwm_lidar.period_us(40);
     pwm_lidar.pulsewidth_us(40); // vitesse fixe
-    
+
     //pwm moteur
     pwm_moteur.period_ms(20);
 
@@ -170,92 +174,104 @@
     // récupération du premier batch de données (7 bytes) du LIDAR
     lidar.putc(0xA5);
     lidar.putc(0x20);
-    for(i=0;i<7;i++)
+    for(i=0; i<7; i++)
         lidar.getc();
 
     pc.printf("FIN intit \n\r");
 
-    lidar.attach(&interrupt_lidar_rx, Serial::RxIrq);    
-    
-    while (1){
-    //printf("pwm_moteur = %f, pwm_direction = %f", pwm_moteur, pwm_direction);
-    
-    if(pc.readable()){
-        char entree = pc.getc();
-        pc.printf("%c \n\r",entree);
-        if (entree == 'a'){
-            run = true;
+    lidar.attach(&interrupt_lidar_rx, Serial::RxIrq);
+
+    while (1) {
+        //printf("pwm_moteur = %f, pwm_direction = %f", pwm_moteur, pwm_direction);
+
+        if(pc.readable()) {
+            char entree = pc.getc();
+            pc.printf("%c \n\r",entree);
+            if (entree == 'a') {
+                run = true;
+            }
+            if (entree == 'z') {
+                run = false;
+            }
         }
-        if (entree == 'z'){
-            run = false;
+
+
+        if(1) {
+            if(tableau_en_cours == 0)
+                afficher_lidar(tableau_distance1);
+            else afficher_lidar(tableau_distance0);
+
+            affiche_lidar = 0;
         }
+
+        if(flag_fin_tour_lidar==1) {
+            flag_fin_tour_lidar=0;
+            if(tableau_en_cours == 0)
+                update_direction(tableau_distance1, dir); // mise à jour à la direction
+            else update_direction(tableau_distance0, dir); // mise à jour à la direction
+            pc.printf("direction,%f,%f\n\r",dir[0],dir[1]);
+            pwm_direction_value = angle_servo(dir); // calcul du pwm
+        }
+
+        if (run == true) {
+            // vitesse constante
+            pwm_moteur.pulsewidth_us(1440);
+            pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
+        } else {
+            pwm_moteur.pulsewidth_us(1480);
+        }
+
     }
-    
-    
-    if(1){
-        afficher_lidar(tableau_distance);
-        
-        
-        affiche_lidar = 0;    
-        }
-    
-    update_direction(tableau_distance, dir); // mise à jour à la direction
-    pc.printf("direction,%f,%f\n\r",dir[0],dir[1]);
-    pwm_direction_value = angle_servo(dir); // calcul du pwm
-    
-    if (run == true){
-        // vitesse constante
-        pwm_moteur.pulsewidth_us(1440); 
-        pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
-    }
-    else{
-        pwm_moteur.pulsewidth_us(1480);
-    }
-
-}
 }
 
 void interrupt_lidar_rx(void)
-{ 
+{
 
     int SEUIL = 0; // Seuil de qualité
-    
+
     static uint8_t data[5],i=0;
     uint16_t Quality;
     uint16_t Angle;
-    static uint16_t Angle_old=0;
     uint16_t Distance;
     uint16_t Angle_d;
+    static uint16_t Angle_d_old=0;
     uint16_t Distance_d;
     affiche_lidar ++;
     data[i] = lidar.getc();
     i++;
-    if(i==5)
-    {
+    if(i==5) {
         i=0;
         Quality = data[0] & 0xFC;
         Quality = Quality >> 2;
-        
+
         Angle = data[1] & 0xFE;
         Angle = (Angle>>1) | ((uint16_t)data[2] << 7);
-        
+
         Distance = data[3];
         Distance = Distance | ((uint16_t)data[4] << 8);
-        
+
         Angle_d = Angle/64; // in degree
         Distance_d = Distance>>2; // in mm
 
-        // On vérifie que l'on écrit pas en dehors du tableau 
+        // On vérifie que l'on écrit pas en dehors du tableau
         //Angle_d = 360 - Angle_d;
         if(Angle_d>359) Angle_d=359;
-        if(Angle_d<0) Angle_d=0;
-  
+
+        if(Angle < (Angle_d_old - 180)) {
+            tableau_en_cours=1-tableau_en_cours;
+            flag_fin_tour_lidar=1;
+        }
+
         if (Quality < SEUIL) {
             // Fiabilisation des données du LIDAR naïve
-            tableau_distance[Angle_d] = tableau_distance[Angle_d - 1];
-            }
-        else
-            tableau_distance[Angle_d] = Distance_d;
+            if(tableau_en_cours==0)
+                tableau_distance0[Angle_d] = tableau_distance0[Angle_d_old];
+            else tableau_distance1[Angle_d] = tableau_distance1[Angle_d_old];
+        } else if(tableau_en_cours==0)
+            tableau_distance0[Angle_d] = Distance_d;
+        else tableau_distance1[Angle_d] = Distance_d;
+
+        Angle_d_old = Angle;
 
         //tableau_distance[Angle_d] = Distance_d;
     }