From eb0840aa9996b56fa13f01d6635efd185d5d4249 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Mon, 17 Apr 2023 16:36:35 +0200
Subject: [PATCH] =?UTF-8?q?Fonction=20pour=20envoyer=20les=20couleurs=20au?=
 =?UTF-8?q?=20mat=20balise=20+=20fonctions=20de=20test=20associ=C3=A9es?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Balise_VL53L1X.c |  5 +++++
 Strategie.h      |  3 ++-
 i2c_annexe.c     | 14 ++++++++++++--
 i2c_annexe.h     |  3 ++-
 4 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/Balise_VL53L1X.c b/Balise_VL53L1X.c
index d7ed7bc..f2e6b74 100644
--- a/Balise_VL53L1X.c
+++ b/Balise_VL53L1X.c
@@ -20,6 +20,7 @@ struct capteur_VL53L1X_t{
     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_ignore; // Le capteur est ignoré car derrière le robot.
 }capteurs_VL53L1X[NB_CAPTEURS];
 
 void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm);
@@ -45,6 +46,7 @@ void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t dista
         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;
@@ -133,6 +135,7 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
     double distance_minimale = DISTANCE_OBSTACLE_IGNORE_MM;
     
     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++){
             if(Geometrie_intersecte_plage_angle(
                     angle_avancement_radiant - cone[cone_index].angle, angle_avancement_radiant + cone[cone_index].angle,
@@ -142,6 +145,8 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
                         distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm;
                     }
                 }
+                // Le capteur est pris en compte au moins dans un cône
+                capteurs_VL53L1X[capteur].donnee_ignore = 0;
             }
         }
     }
diff --git a/Strategie.h b/Strategie.h
index ceb0d2b..f8087d8 100644
--- a/Strategie.h
+++ b/Strategie.h
@@ -20,7 +20,8 @@ enum longer_direction_t{
 
 enum couleur_t{
     COULEUR_BLEU=0,
-    COULEUR_VERT
+    COULEUR_VERT,
+    COULEUR_INCONNUE,
 };
 
 extern enum etat_strategie_t{
diff --git a/i2c_annexe.c b/i2c_annexe.c
index d4e0fb3..c071951 100644
--- a/i2c_annexe.c
+++ b/i2c_annexe.c
@@ -3,12 +3,15 @@
 #include "stdio.h"
 
 #define ADRESSE_PICO_ANNEXE 0x17
-#define ADRESSE_DEBUT_W 0x08
+#define ADRESSE_DEBUT_W 0x05
 #define ADRESSE_DEBUT_R 0x0A
 #define ADRESSE_TURBINE_PORTE 0x0A
 #define ADRESSE_CONTACTEURS 0x0B
 #define ADRESSE_VL53L1X 0x0C
-#define TAILLE_DONNEES_EMISSION 3
+#define ADRESSE_COULEUR 0x05
+#define ADRESSE_MASQUE_LED_1 0x06
+#define ADRESSE_MASQUE_LED_2 0x07
+#define TAILLE_DONNEES_EMISSION 6
 #define TAILLE_DONNEES_RECEPTION 14
 
 
@@ -61,6 +64,13 @@ void i2c_annexe_gestion(){
 
 }
 
+void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){
+    donnees_emission[ADRESSE_COULEUR - ADRESSE_DEBUT_W] = couleur;
+    donnees_emission[ADRESSE_MASQUE_LED_1 - ADRESSE_DEBUT_W] = (masque_led >> 8) & 0xFF;
+    donnees_emission[ADRESSE_MASQUE_LED_2 - ADRESSE_DEBUT_W] = masque_led & 0xFF;
+    donnees_a_envoyer=1;
+}
+
 void i2c_annexe_active_turbine(void){
     donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
     donnees_a_envoyer=1;
diff --git a/i2c_annexe.h b/i2c_annexe.h
index e71a42d..b8af1dd 100644
--- a/i2c_annexe.h
+++ b/i2c_annexe.h
@@ -19,4 +19,5 @@ uint8_t i2c_annexe_get_contacteur_butee_C(void);
 uint8_t i2c_annexe_get_contacteur_longer_A(void);
 uint8_t i2c_annexe_get_contacteur_longer_C(void);
 
-void i2c_annexe_get_distances(uint8_t *distance_capteur_cm);
\ No newline at end of file
+void i2c_annexe_get_distances(uint8_t *distance_capteur_cm);
+void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led);
\ No newline at end of file