Diff: main.cpp

Revision:

15:f8c5007343f9

Parent:

14:e1e393537fb6

Child:

16:b066e4d6e442

Child:

17:4a65cce4ac4f

--- a/main.cpp	Thu Jun 06 12:23:45 2019 +0000
+++ b/main.cpp	Thu Jun 06 14:38:58 2019 +0000
@@ -62,11 +62,11 @@
     for (i=0; i<360; i++) {
         liste_fictifs[i] = 0;
     }
-    // for (i=135; i<=225; i++) {
-    //     //for (int i=0; i<180; i++){ //test
-    //     liste_fictifs[i] = 1;
-    // }
-    liste_fictifs[180] = 1;
+     for (i=90; i<=270; i++) {
+         //for (int i=0; i<180; i++){ //test
+         liste_fictifs[i] = 1;
+     }
+    //liste_fictifs[180] = 1;
 
     avg_x = 0;
     avg_y = 0;
@@ -77,7 +77,7 @@
         float r, x, y;
         theta = i;
         if (liste_fictifs[theta] == 1) {
-            r = 50;
+            r = 500;
         } else {
             //r = 0; //test
             if(theta != 0) r = list_lidar[360-theta];
@@ -94,7 +94,7 @@
             //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;
+            float puissance = 0.5*cosf(2*theta*3.14/180) + 1.5;
             avg_x = avg_x - x/pow(r,puissance);
             avg_y = avg_y - y/pow(r,puissance);
 
@@ -106,7 +106,7 @@
     //avg_y /= sum_inv_dist;
     direction[0] = avg_x;
     direction[1] = avg_y;
-    //pc.printf("Update termine\n\r");
+    pc.printf("Update termine\n\r");
     // mise à jour de la direction
     for(i=0; i<2; i++)
         vecteur[i] = direction[i];
@@ -123,8 +123,13 @@
     float x, y;
     x = direction[0];
     y = direction[1];
-    angle = atan(x/y);
-    pwm = 14.662756 * angle*180/3.14 + 1453.08; // à refaire
+    //x = 1;
+    //y = 1;
+    angle = atan2(y,x);
+    angle = angle*180/3.14;
+    pc.printf("Angle : %f\n\r",angle);
+    //pwm = 14.662756 * angle + 1453.08; // à refaire
+    pwm = -13.30 * angle + 1376.75;
 
     //if (pwm < 1115) printf("trop petit\n\r");
     //if (pwm > 1625) printf("trop grand\n\r");
@@ -224,7 +229,7 @@
             pwm_direction_value = angle_servo(dir); // calcul du pwm
         }
 
-        if (run == true) {
+        if (1) {
             // vitesse constante
             pwm_moteur.pulsewidth_us(1440);
             pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm du moteur