Diff: main.cpp

Revision:

18:6986eb7282ee

Parent:

15:f8c5007343f9

--- a/main.cpp	Thu Jun 06 14:38:58 2019 +0000
+++ b/main.cpp	Fri Jun 07 01:12:00 2019 +0000
@@ -14,6 +14,9 @@
 int compteur_tours_lidar = 0;
 int affiche_lidar = 0;
 bool run = false;
+Timer T;
+bool first_update = false;
+
 
 // Défintion des pwm
 PwmOut pwm_lidar(PB_15); // pwm du Lidar
@@ -22,6 +25,8 @@
 
 void interrupt_lidar_rx(void);
 
+void interrupt_bt_rx(void);
+
 
 float distance(float x_1, float x_2, float y_1, float y_2)
 {
@@ -33,7 +38,19 @@
 
 void update_direction(float* list_lidar_var, float* vecteur)
 {
-    pc.printf("Update commence\n\r");
+    float t = 0;
+    //pc.printf("Update commence\n\r");
+    if (first_update==true) {
+        //T.start();
+        first_update=false;
+    } else {
+        t = T.read();
+        //pc.printf("%f\n\r",t);
+        if(t>1) {
+            pc.printf("Premier update depuis %f secondes\n\r",t);
+        }
+        //T.reset();
+    }
     // Fonction de mise à jour de la direction
     float direction[2];
     int i;
@@ -62,10 +79,10 @@
     for (i=0; i<360; i++) {
         liste_fictifs[i] = 0;
     }
-     for (i=90; i<=270; i++) {
-         //for (int i=0; i<180; i++){ //test
-         liste_fictifs[i] = 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;
@@ -83,7 +100,7 @@
             if(theta != 0) r = list_lidar[360-theta];
             else r = list_lidar[0];
 
-            }
+        }
 
         if (r != 0) {
 
@@ -106,7 +123,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];
@@ -127,7 +144,7 @@
     //y = 1;
     angle = atan2(y,x);
     angle = angle*180/3.14;
-    pc.printf("Angle : %f\n\r",angle);
+    //pc.printf("Angle : %f\n\r",angle);
     //pwm = 14.662756 * angle + 1453.08; // à refaire
     pwm = -13.30 * angle + 1376.75;
 
@@ -168,6 +185,7 @@
 
     float dir[2]; // direction
     float pwm_direction_value;
+    int pwm_mot_value = 1480;
 
 
     int i;
@@ -175,13 +193,13 @@
 
 
 
-
     // pwm du LIDAR
     pwm_lidar.period_us(40);
     pwm_lidar.pulsewidth_us(40); // vitesse fixe
 
     //pwm moteur
     pwm_moteur.period_ms(20);
+    pwm_moteur.pulsewidth_us(pwm_mot_value);
 
     // pwm de la direction
     pwm_direction.period_ms(20);
@@ -196,22 +214,14 @@
     pc.printf("FIN intit \n\r");
 
     lidar.attach(&interrupt_lidar_rx, Serial::RxIrq);
+    pc.attach(&interrupt_bt_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;
-            }
-        }
-
-
+        //Test
+        float t = T.read_us();
+        pc.printf("%f\n\r",T.read_us());
+        wait(2);
         if(0) {
             if(tableau_en_cours == 0)
                 afficher_lidar(tableau_distance1);
@@ -225,19 +235,42 @@
             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]);
+            //pc.printf("direction,%f,%f\n\r",dir[0],dir[1]);
             pwm_direction_value = angle_servo(dir); // calcul du pwm
         }
 
-        if (1) {
-            // 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 (run==false && pwm_mot_value != 1480) {
+            // vitesse nulle
+            pwm_mot_value = 1480;
+            pwm_moteur.pulsewidth_us(pwm_mot_value);
+            pc.printf("pwm_mot = %i\n\r",pwm_mot_value);
+
+        }
+        if (run==true) {
+            if(pwm_mot_value != 1440) {
+                pwm_mot_value = 1440;
+                pwm_moteur.pulsewidth_us(pwm_mot_value);
+                pc.printf("pwm_mot = %i\n\r",pwm_mot_value);
+            }
+
+            pwm_direction.pulsewidth_us(pwm_direction_value); // commande du pwm de la direction
         }
+    }
+}
 
+void interrupt_bt_rx(void)
+{
+    char entree = pc.getc();
+    pc.printf("%c\n\r",entree);
+    if (entree == 'a') {
+        run = true;
+        pc.printf("c'est parti Bruno !\n\r");
     }
+    if (entree == 'z') {
+        run = false;
+        pc.printf("Hola, tout doux !\n\r");
+    }
+
 }
 
 void interrupt_lidar_rx(void)