From 13ea0836706067fd4dc04286fd726ef033674333 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Mon, 17 Apr 2023 22:16:14 +0200
Subject: [PATCH] =?UTF-8?q?Visualisation=20des=20capteurs=20VL53L1X=20igno?=
 =?UTF-8?q?r=C3=A9s=20+=20corrections=20dans=20la=20gestion=20des=20c?=
 =?UTF-8?q?=C3=B4nes=20de=20d=C3=A9tection.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Cas de test spécifique
- La distance des cônes n'est pas encore prise en compte
---
 Balise_VL53L1X.c | 39 ++++++++++++++++++++++++++++++---------
 Balise_VL53L1X.h |  3 ++-
 Geometrie.c      |  2 +-
 Test.c           | 30 ++++++++++++++++++++++++++++++
 4 files changed, 63 insertions(+), 11 deletions(-)

diff --git a/Balise_VL53L1X.c b/Balise_VL53L1X.c
index f2e6b74..f7238d8 100644
--- a/Balise_VL53L1X.c
+++ b/Balise_VL53L1X.c
@@ -14,12 +14,12 @@ uint8_t distance_capteur_cm[NB_CAPTEURS];
 
 struct capteur_VL53L1X_t{
     uint8_t distance_lue_cm; // Distance entre le capteur et l'obstacle.
-    uint8_t distance_obstacle_robot_mm; // Distance entre l'obstacle et le centre du robot.
+    double distance_obstacle_robot_mm; // Distance entre l'obstacle et le centre du robot.
     double angle_ref_robot; // Orientation du capteur  dans le référentiel du robot
     double angle_ref_terrain; // Orientation du capteur dans le référentiel du terrain
     double angle_ref_terrain_min; // Cone de détection du capteur (min)
     double angle_ref_terrain_max; // Cone de détection du capteur (max)
-    uint donnee_valide;
+    uint donnee_valide; // L'obstacle détecté est dans le terrain et n'est pas dans le robot
     uint donnee_ignore; // Le capteur est ignoré car derrière le robot.
 }capteurs_VL53L1X[NB_CAPTEURS];
 
@@ -42,14 +42,14 @@ void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t dista
     while(capteur_VL53L1X->angle_ref_terrain > M_PI){
         capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
     }
-    while(capteur_VL53L1X->angle_ref_terrain < 2* -M_PI){
+    while(capteur_VL53L1X->angle_ref_terrain < -M_PI){
         capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
     }
 
     capteur_VL53L1X->distance_lue_cm = distance_capteur_cm;
     capteur_VL53L1X->distance_obstacle_robot_mm = 10 * (distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM);
-    capteur_VL53L1X->angle_ref_terrain_min = capteur_VL53L1X->angle_ref_terrain - DEMI_CONE_CAPTEUR_RADIAN;
-    capteur_VL53L1X->angle_ref_terrain_max = capteur_VL53L1X->angle_ref_terrain + DEMI_CONE_CAPTEUR_RADIAN;
+    capteur_VL53L1X->angle_ref_terrain_min = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_terrain - DEMI_CONE_CAPTEUR_RADIAN);
+    capteur_VL53L1X->angle_ref_terrain_max = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_terrain + DEMI_CONE_CAPTEUR_RADIAN);
 
     invalide_obstacle(capteur_VL53L1X, position_robot);
 
@@ -61,8 +61,8 @@ void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct positio
     // Positionne l'obstacle sur le terrain
     struct position_t position_obstacle;
     //printf("Angle:%.1f\n",capteur_VL53L1X->angle_ref_terrain);
-    position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_obstacle_robot_mm);
-    position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_obstacle_robot_mm * 10);
+    position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
+    position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
 
     capteur_VL53L1X->donnee_valide=1;
     // Si la distance vaut 0, à invalider
@@ -118,6 +118,7 @@ uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
 /// @return 
 double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
     const uint8_t NB_CONE=3;
+    uint16_t masque_led=0;
     struct cone_t{
         double distance_mm;
         double angle;
@@ -137,11 +138,23 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
     for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
         capteurs_VL53L1X[capteur].donnee_ignore = 1;
         for(uint8_t cone_index=0; cone_index<NB_CONE; cone_index++){
+            /*printf("capteur:%d\n", capteur);
+            printf("capteur_angle_min:%f\n", capteurs_VL53L1X[capteur].angle_ref_terrain_min);
+            printf("capteur_angle_max:%f\n", capteurs_VL53L1X[capteur].angle_ref_terrain_max);
+            printf("cone angel min:%f\n", angle_avancement_radiant - cone[cone_index].angle);
+            printf("cone angel max:%f\n", angle_avancement_radiant + cone[cone_index].angle);*/
+
+            //On test si le capteur détecte dans la plage du cône
             if(Geometrie_intersecte_plage_angle(
                     angle_avancement_radiant - cone[cone_index].angle, angle_avancement_radiant + cone[cone_index].angle,
                     capteurs_VL53L1X[capteur].angle_ref_terrain_min, capteurs_VL53L1X[capteur].angle_ref_terrain_max)){
+                // Si l'obstacle est sur le terrain
                 if(capteurs_VL53L1X[capteur].donnee_valide){
-                    if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale){
+                    // Si la distance est plus petite que la distance minimale actuelle
+                    // Si la distance est plus petite que le cône en question...
+                    if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale /*&& 
+                            capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < cone[cone_index].distance_mm*/){
+                        // On retient cette distance comme étant notre distance minimale
                         distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm;
                     }
                 }
@@ -151,6 +164,14 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
         }
     }
 
+    // On éteint les LEDs qui correspondent aux capteurs ignorés
+    for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
+        if(capteurs_VL53L1X[capteur].donnee_ignore == 1){
+            masque_led |= 1 << capteur;
+        }
+    }
+    i2c_annexe_couleur_balise(0, masque_led);
+
     // On enlève le rayon du robot et la taille du robot adverse
     if(distance_minimale  < DISTANCE_OBSTACLE_IGNORE_MM){
         distance_minimale -= (RAYON_ROBOT + RAYON_ROBOT_ADVERSE_MM);
@@ -160,4 +181,4 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
     }
 
     return distance_minimale;
-}
\ No newline at end of file
+}
diff --git a/Balise_VL53L1X.h b/Balise_VL53L1X.h
index 4e9a5b6..0dae84c 100644
--- a/Balise_VL53L1X.h
+++ b/Balise_VL53L1X.h
@@ -2,4 +2,5 @@
 void Balise_VL53L1X_init(void);
 void Balise_VL53L1X_gestion(void);
 uint8_t Balise_VL53L1X_get_min_distance(void);
-uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur);
\ No newline at end of file
+uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur);
+double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant);
\ No newline at end of file
diff --git a/Geometrie.c b/Geometrie.c
index bb8414c..203daaa 100644
--- a/Geometrie.c
+++ b/Geometrie.c
@@ -8,7 +8,7 @@ double Geometrie_get_angle_normalisee(double angle){
     while(angle > M_PI){
         angle -= 2* M_PI;
     }
-    while(angle < 2* -M_PI){
+    while(angle < -M_PI){
         angle += 2* M_PI;
     }
     return angle;
diff --git a/Test.c b/Test.c
index a909e9e..79de106 100644
--- a/Test.c
+++ b/Test.c
@@ -60,6 +60,7 @@ int test_aller_retour();
 void test_trajectoire_teleplot();
 int test_capteurs_balise(void);
 int test_geometrie(void);
+int test_angle_balise(void);
 
 
 // Mode test : renvoie 0 pour quitter le mode test
@@ -161,6 +162,11 @@ int mode_test(){
         while(test_geometrie());
         break;
 
+    case 'O':
+    case 'o':
+        while(test_angle_balise());
+        break;
+
     case 'T':
     case 't':
         while(test_trajectoire());
@@ -1368,4 +1374,28 @@ int test_geometrie(){
         Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
 
     return 0;
+}
+
+
+int test_angle_balise(void){
+    
+    i2c_maitre_init();
+    Balise_VL53L1X_init();
+    Localisation_set(1000,1500,0);
+    int lettre;
+    double distance, angle=0;
+    do{
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        Balise_VL53L1X_gestion();
+
+        distance = Balise_VL53L1X_get_distance_obstacle_mm(angle);
+        printf(">distance_obstacle:%3.0f\n", distance);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
+        
+    return 0;
+
+
 }
\ No newline at end of file