Revision 18:daba0b3777c0, committed 2019-06-07

Comitter:

SolalNathan

Date:

Fri Jun 07 01:11:01 2019 +0000

Parent:

17:4a65cce4ac4f

Commit message:

formatage du code en differents modules

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Direction.cpp	Fri Jun 07 01:11:01 2019 +0000
@@ -0,0 +1,69 @@
+#include "SaphTeamRacing.h"
+
+void points_fictifs(uint8_t* liste_fictifs, int zero_min, int zero_max, 
+                     int one_min, int one_max){
+    /*
+    Fonction définissant les points fictifs poussant la voiture.
+    zero_min, zero_max, one_min et one_max doivent êtres compris entre 0
+    et 360°. Il faut zero_min < zero_max et one_min < one_max.
+    */
+    for (int i = zero_min; i < zero_max; i++) {
+        liste_fictifs[i] = 0;
+    }
+    for (int i = one_max; i < one_max; i++) {
+        liste_fictifs[i] = 1;
+    }
+}
+
+void update_direction(float* list_lidar_var, float* vecteur)
+{
+    /*
+    Fonction de mise à jour de la direction grâce à l'algorithme des charges 
+    fictives.
+    */
+    
+    float direction[2], list_lidar[360];
+    uint8_t liste_fictifs[360];
+    int i;
+    for(i=0; i<360; i++) list_lidar[i]=list_lidar_var[i];
+
+    direction[0] = 1;
+    direction[1] = 0;
+    float avg_x = 0; 
+    float avg_y = 0;
+
+    points_fictifs(liste_fictifs, 0, 360, 90, 270);
+
+    // Calcul de la direction à prende en fonction des charges fictives
+    for (i=0; i<360; i++) {
+        int theta = i;
+        float r;
+        double x, y;
+        if (liste_fictifs[theta] == 1) {
+            r = 500;
+        } else {
+            //r = 0; //test
+            if(theta != 0) r = list_lidar[360-theta];
+            else r = list_lidar[0];
+
+        }
+
+        if (r > 0) {
+            double theta_d = theta*3.141592/180;
+            x = r*cosf(theta_d);
+            y = r*sinf(theta_d);
+            double puissance = 0.5*(double)cosf(2*theta_d) + 1.5;
+            double r_pow = pow((double) r, (double) puissance);
+            avg_x = avg_x - x/r_pow;
+            avg_y = avg_y - y/r_pow;
+
+        }
+    }
+
+    direction[0] = avg_x;
+    direction[1] = avg_y;
+
+    // mise à jour de la direction
+    for(i=0; i<2; i++)
+        vecteur[i] = direction[i];
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Lidar.cpp	Fri Jun 07 01:11:01 2019 +0000
@@ -0,0 +1,17 @@
+#include "SaphTeamRacing.h"
+
+
+void afficher_lidar(float *tableau_distances_var, Serial pc)
+{
+    /*
+    Affiche les données du lidar dans la liaison série.
+    */
+    int 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);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SaphTeamRacing.h	Fri Jun 07 01:11:01 2019 +0000
@@ -0,0 +1,21 @@
+#ifndef STR_H
+#define STR_H
+
+#include "mbed.h"
+#include <math.h>
+
+// importer toutes les fonctions ici
+
+// Servo
+float angle_servo(float *direction);
+// Lidar
+void interrupt_lidar_rx(Serial lidar, bool *tableau_en_cours,
+                        uint8_t *flag_fin_tour_lidar, float* tableau_distance0,
+                        float* tableau_distance1);
+void afficher_lidar(float *tableau_distances_var, Serial pc);
+// Utils
+float distance(float x_1, float x_2, float y_1, float y_2);
+// Direction 
+void update_direction(float* list_lidar_var, float* vecteur);
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.cpp	Fri Jun 07 01:11:01 2019 +0000
@@ -0,0 +1,17 @@
+#include "SaphTeamRacing.h"
+
+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 x, y;
+    double angle, pwm;
+    x = direction[0];
+    y = direction[1];
+    angle = atan2(y, x); // récupration de l'angle en radian
+    angle = angle*180/3.14; // conversion en degrés
+    pwm = -13.30*angle + 1376.75; // Formule issue de la régression linéaire
+
+    return pwm;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Utils.cpp	Fri Jun 07 01:11:01 2019 +0000
@@ -0,0 +1,12 @@
+#include "SaphTeamRacing.h"
+
+float distance(float x_1, float x_2, float y_1, float y_2)
+{
+    /*
+    Fonction qui renvoie la distance entre deux points en norme 2.
+    */
+    
+    float norm2;
+    norm2 = sqrt((x_1 - x_2)*(x_1 - x_2) + (y_1 - y_2)*(y_1 - y_2));
+    return norm2;
+}

--- a/main.cpp	Thu Jun 06 15:40:53 2019 +0000
+++ b/main.cpp	Fri Jun 07 01:11:01 2019 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include <math.h>
+#include "SaphTeamRacing.h"
 
 // Définition des ports séries
 //Serial pc(USBTX, USBRX, 115200);
@@ -9,10 +10,9 @@
 // Définition des variables globales
 float tableau_distance0[360] = {};
 float tableau_distance1[360] = {};
-uint8_t tableau_en_cours = 0; //tableau en cours de remplissage
+bool tableau_en_cours = false; //tableau en cours de remplissage
 uint8_t flag_fin_tour_lidar=0;
 int compteur_tours_lidar = 0;
-int affiche_lidar = 0;
 bool run = false;
 bool stop = true;
 
@@ -23,145 +23,6 @@
 
 void interrupt_lidar_rx(void);
 
-
-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;
-    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_var, float* vecteur)
-{
-    pc.printf("Update commence\n\r");
-    // Fonction de mise à jour de la direction
-    float direction[2];
-    int i;
-    float list_lidar[360];
-    uint8_t liste_fictifs[360];
-    for(i=0; i<360; i++) list_lidar[i]=list_lidar_var[i];
-
-    //pour les essais
-    //for(i=0; i<360; i++)
-    //    list_lidar[i]=00;
-
-    //list_lidar[0]=100;
-    //list_lidar[90]=20.0;
-    //list_lidar[270]=100.0;
-    //list_lidar[45]=20.0;
-    //list_lidar[315]=100;
-    ///////////////////
-
-
-    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
-
-    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;
-    }
-    //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++) {
-        int theta;
-        float r, x, y;
-        theta = i;
-        if (liste_fictifs[theta] == 1) {
-            r = 500;
-        } else {
-            //r = 0; //test
-            if(theta != 0) r = list_lidar[360-theta];
-            else r = list_lidar[0];
-
-        }
-
-        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 = 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);
-
-            //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;
-    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)
-{
-    // 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;
-    x = direction[0];
-    y = direction[1];
-    //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");
-    //if (angle > 5*3.14/180){
-    //    pwm = 1745;
-    //}
-    //else{
-    //    if (angle < -5*3.14/180){
-    //        pwm = 1080;
-    //        }
-    //    else{
-    //        pwm = 1453;
-    //        }
-    //}
-
-    return pwm;
-}
-
-void afficher_lidar(float *tableau_distances_var)
-{
-    //Affiche les données du lidar dans la liaison série
-    int 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()
 {
 
@@ -169,14 +30,7 @@
 
     float dir[2]; // direction
     float pwm_direction_value;
-
-
-    int i;
-
-
-
-
-
+    
     // pwm du LIDAR
     pwm_lidar.period_us(40);
     pwm_lidar.pulsewidth_us(40); // vitesse fixe
@@ -191,7 +45,7 @@
     // 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(int i=0; i<7; i++)
         lidar.getc();
 
     pc.printf("FIN intit \n\r");
@@ -215,16 +69,15 @@
 
 
         if(0) {
-            if(tableau_en_cours == 0)
-                afficher_lidar(tableau_distance1);
-            else afficher_lidar(tableau_distance0);
+            if(!tableau_en_cours)
+                afficher_lidar(tableau_distance1, pc);
+            else afficher_lidar(tableau_distance0, pc);
 
-            affiche_lidar = 0;
         }
 
         if(flag_fin_tour_lidar==1) {
             flag_fin_tour_lidar=0;
-            if(tableau_en_cours == 0)
+            if(!tableau_en_cours)
                 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]);
@@ -251,14 +104,8 @@
 
     int SEUIL = 0; // Seuil de qualité
 
-    static uint8_t data[5],i=0;
-    uint16_t Quality;
-    uint16_t Angle;
-    uint16_t Distance;
-    uint16_t Angle_d;
-    static uint16_t Angle_d_old=0;
-    uint16_t Distance_d;
-    affiche_lidar ++;
+    static uint8_t data[5], i = 0, Angle_d_old = 0;
+    uint16_t Quality, Angle, Distance, Angle_d, Distance_d;
     data[i] = lidar.getc();
     i++;
     if(i==5) {
@@ -280,16 +127,16 @@
         if(Angle_d>359) Angle_d=359;
 
         if(Angle < (Angle_d_old - 180)) {
-            tableau_en_cours=1-tableau_en_cours;
+            tableau_en_cours=!tableau_en_cours;
             flag_fin_tour_lidar=1;
         }
 
         if (Quality < SEUIL) {
             // Fiabilisation des données du LIDAR naïve
-            if(tableau_en_cours==0)
+            if(!tableau_en_cours)
                 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)
+        } else if(!tableau_en_cours)
             tableau_distance0[Angle_d] = Distance_d;
         else tableau_distance1[Angle_d] = Distance_d;
 
@@ -297,4 +144,4 @@
 
         //tableau_distance[Angle_d] = Distance_d;
     }
-}
+}
\ No newline at end of file