Diff: main.cpp

Revision:

8:2ce9493549e8

Parent:

7:dc7e66870bd0

Child:

9:a15fc52284ff

--- a/main.cpp	Fri May 31 12:05:59 2019 +0000
+++ b/main.cpp	Tue Jun 04 16:04:52 2019 +0000
@@ -39,26 +39,28 @@
     avg_y = 0;
     
     // Calcul de la direction à prende en fonction des charges fictives
-    for (i=0; i<360; i++)
+    for (i=1; i<361; i++)
         {
         int theta;
         float r, x, y;
         theta = i;
-        r = list_lidar[theta];
-        
-        if (r == 0) break; // non calcul en cas de distance nul (donnée non captée) 
-        
+        r = list_lidar[360-theta];
+        //theta = 360 - theta;
+        //pc.printf("Salut 1\n\r");
+        //pc.printf("%f\n\r",r);
+        if (r == 0 && theta != 180) break; // non calcul en cas de distance nul (donnée non captée) 
+        //pc.printf("Salut 2\n\r");
         //x = 0;
         //y = 0; 
-        y = r*cosf(theta);
-        x = r*sinf(theta);
+        x = r*cosf(theta);
+        y = r*sinf(theta);
         sum_inv_dist += 1/pow(r, 2);
-        avg_x -= x/sum_inv_dist;
-        avg_y -= y/sum_inv_dist;
+        avg_x -= x/pow(r,2);
+        avg_y -= y/pow(r,2);
         }
     
-    avg_x /= sum_inv_dist;
-    avg_y /= sum_inv_dist;
+    //avg_x /= sum_inv_dist;
+    //avg_y /= sum_inv_dist;
     direction[0] = avg_x;
     direction[1] = avg_y;
     
@@ -137,11 +139,14 @@
     //printf("pwm_moteur = %f, pwm_direction = %f", pwm_moteur, pwm_direction);
     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
     
     pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur
@@ -181,6 +186,7 @@
         Distance_d = Distance>>2; // in mm
 
         // 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;