Diff: main.cpp

Revision:

11:e227edfced99

Parent:

10:c8d93dc5993c

Child:

12:594a1b936f4b

--- a/main.cpp	Wed Jun 05 12:29:09 2019 +0000
+++ b/main.cpp	Thu Jun 06 10:25:41 2019 +0000
@@ -3,12 +3,14 @@
 
 // Définition des ports séries
 Serial pc(USBTX, USBRX, 115200);
+//Serial pc(PC_10, PC_11, 115200);
 Serial lidar(PC_6, PC_7, 115200);
 
 // Définition des variables globales
 float tableau_distance[360] = {};
 int compteur_tours_lidar = 0;
 int affiche_lidar = 0;
+bool run = false;
 
 // Défintion des pwm
 PwmOut pwm_lidar(PB_15); // pwm du Lidar
@@ -28,8 +30,17 @@
    
 void update_direction(float* list_lidar, 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;
     float avg_x, avg_y, sum_inv_dist;
@@ -39,27 +50,28 @@
     for (int i=0; i<360; i++){
         liste_fictifs[i] = 0;
         }
-    for (int i=90; i<271; i++){
+    for (int i=135; i<225; i++){
+    //for (int i=0; i<180; i++){ //test
         liste_fictifs[i] = 1;
         }
-    int i;
+
     avg_x = 0;
     avg_y = 0;
     
     // Calcul de la direction à prende en fonction des charges fictives
-    for (i=1; i<361; i++)
+    for (i=0; i<360; i++)
         {
         int theta;
         float r, x, y;
         theta = i;
-        if (liste_fictifs[360-theta] == 1){
+        if (liste_fictifs[theta] == 1){
             //pc.printf("Angle,%i\n\r",theta);
-            r = 0.01;
+            r = 50;
             }
         else{
             //pc.printf("Angle,%i\n\r",theta);
             //r = 0; //test
-            r = list_lidar[360-theta];
+            r = list_lidar[359-theta];
             //pc.printf("r,%f\n\r",r);
             }
         //pc.printf("Salut 1\n\r");
@@ -70,19 +82,24 @@
         //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);
+        //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);
+        
         }
     
     //avg_x /= sum_inv_dist;
     //avg_y /= sum_inv_dist;
     direction[0] = avg_x;
     direction[1] = avg_y;
-    
+    //pc.printf("Update termine\n\r");
     // mise à jour de la direction
     for(i=0; i<2; i++)
         vecteur[i] = direction[i];
+
 }
     
 float angle_servo(float *direction)
@@ -143,10 +160,8 @@
     pwm_lidar.period_us(40);
     pwm_lidar.pulsewidth_us(40); // vitesse fixe
     
-    // pwm du Moteur
+    //pwm moteur
     pwm_moteur.period_ms(20);
-    pwm_moteur.pulsewidth_us(1440); // correspond à une vitesse nulle
-    // Gaspard : 1450, Solal : 1480. Tester les deux
 
     // pwm de la direction
     pwm_direction.period_ms(20);
@@ -164,6 +179,19 @@
     
     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(1){
         afficher_lidar(tableau_distance);
         
@@ -171,16 +199,21 @@
         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
+    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)
 {