From 04b02a98ab114eeef5989b4065d7093aa44e0cbd Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sat, 4 Mar 2023 13:23:32 +0100
Subject: [PATCH] Couleur en fonction des distances

---
 main.c   | 47 ++++++++++++++++++++++++++++++++++++++++++-----
 ws2812.c |  9 ++++++---
 ws2812.h |  1 -
 3 files changed, 48 insertions(+), 9 deletions(-)

diff --git a/main.c b/main.c
index 95da796..27023f9 100644
--- a/main.c
+++ b/main.c
@@ -12,6 +12,19 @@
 #define I2C_SCL_PIN 1
 
 
+#define DISTANCE_TRES_LOIN_CM 120
+#define DISTANCE_LOIN_CM 50
+#define DISTANCE_PROCHE_CM 14
+
+// Couleur quand la distance est trop loin pour s'en servir avec fiabilité
+#define COULEUR_TRES_LOIN 0x00, 0x00, 0x01
+// Couleur quand la distance fiable mais loin
+#define COULEUR_LOIN 0x00, 0x01, 0x00
+// Couleur quand la distance fiable et que le robot doit ralentir
+#define COULEUR_PROCHE 0x01, 0x01, 0x00
+// Couleur trop proche : rien ne devrait être si près
+#define COULEUR_TROP_PROCHE 0x01, 0x00, 0x01
+
 #define TEST_TIMEOUT_US 10000000
 
 void i2c_master_init(void);
@@ -20,9 +33,12 @@ int calibration(uint8_t device);
 int change_address(uint8_t * device, uint8_t new_i2c_7bits_address);
 void initialise_adresses(void);
 int continuous_multiple_reading(void);
+void affiche_distance_sur_led();
 void init_sensors(void);
 void display_menu();
 
+// Stock les valeurs lues des capteurs
+uint8_t distance_capteur_cm[12];
 
 uint8_t statu_capteurs[13];
 
@@ -128,8 +144,8 @@ void initialise_adresses(void){
                 statu_capteurs[capteur]=1;
                 int status;
                 status = VL53L1X_SetDistanceMode (VL53L1X_device, 1); // Short mode
-                status |= VL53L1X_SetInterMeasurementInMs(VL53L1X_device, 200); 
-                status |= VL53L1X_SetTimingBudgetInMs(VL53L1X_device, 200);
+                status |= VL53L1X_SetInterMeasurementInMs(VL53L1X_device, 100); 
+                status |= VL53L1X_SetTimingBudgetInMs(VL53L1X_device, 100);
                 if(status){
                     printf("Custom config KO, error %d\n", status);
                     ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
@@ -162,7 +178,7 @@ int continuous_multiple_reading(){
         }
         int status;
         uint8_t data_ready = 0;
-        uint16_t distance;
+        uint16_t distance_mm;
         while(!data_ready){
             status=VL53L1X_CheckForDataReady(device, &data_ready);
             if(status){
@@ -170,12 +186,13 @@ int continuous_multiple_reading(){
             }
         }
         
-        status=VL53L1X_GetDistance(device, &distance);
+        status=VL53L1X_GetDistance(device, &distance_mm);
         if(status){
             printf("GetDistance KO, error %d, capteur:%x\n", status, device);
             return 0;
         }else{
-            printf(">distance%x:%d\n", device, distance);
+            printf(">distance%x:%d\n", device, distance_mm);
+            distance_capteur_cm[device-0x31] = distance_mm / 10;
         }
         
         status=VL53L1X_ClearInterrupt(device);
@@ -189,9 +206,29 @@ int continuous_multiple_reading(){
             //return 0;
         }
     }
+    affiche_distance_sur_led();
     return 1;
 }
 
+void affiche_distance_sur_led(){
+    uint8_t distance_cm;
+    uint32_t couleur;
+    for(uint8_t capteur=0; capteur<12; capteur++){
+        distance_cm = distance_capteur_cm[capteur];
+        if(distance_cm == 0 ||distance_cm > DISTANCE_TRES_LOIN_CM){
+            ws2812_set_buffer_rgb(COULEUR_TRES_LOIN, capteur);
+        }else if(distance_cm > DISTANCE_LOIN_CM){
+            ws2812_set_buffer_rgb(COULEUR_LOIN, capteur);
+        }else if(distance_cm > DISTANCE_PROCHE_CM){
+            ws2812_set_buffer_rgb(COULEUR_PROCHE, capteur);
+        }else{
+            ws2812_set_buffer_rgb(COULEUR_TROP_PROCHE, capteur);
+        }
+        
+    }
+    ws2812_affiche_buffer();
+}
+
 
 void display_menu(){
     printf("Select action :\n");
diff --git a/ws2812.c b/ws2812.c
index 4f4abd9..67dc0e3 100644
--- a/ws2812.c
+++ b/ws2812.c
@@ -10,6 +10,8 @@
 uint32_t couleur[13];
 uint32_t buffer_couleur[12];
 
+void ws2812_set_buffer(uint32_t couleur, uint8_t index_led);
+
 void ws2812_init(){
     /*couleur[0]=0x000200;
     couleur[1]=0x010200;
@@ -105,11 +107,12 @@ void ws2812_affiche_buffer(){
 }
 
 void ws2812_set_buffer_rgb(uint8_t rouge, uint8_t vert, uint8_t bleu, uint8_t index_led){
-    if(index_led <12){
-        buffer_couleur[index_led] = urgb_u32(rouge, vert,bleu);
-    }
+        ws2812_set_buffer(urgb_u32(rouge, vert,bleu), index_led);
 }
 
+/// @brief Rempli le buffer à envoyer au LED, necessite d'appeler la fonction ws2812_affiche_buffer() ensuite
+/// @param couleur : couleur en RVB
+/// @param index_led : index entre 0 et 11
 void ws2812_set_buffer(uint32_t couleur, uint8_t index_led){
     if(index_led <12){
         buffer_couleur[index_led] = couleur;
diff --git a/ws2812.h b/ws2812.h
index dca15e4..b6ab15d 100644
--- a/ws2812.h
+++ b/ws2812.h
@@ -3,5 +3,4 @@
 void ws2812_init(void);
 void ws2812_affiche_buffer(void);
 void ws2812_set_buffer_rgb(uint8_t rouge, uint8_t vert, uint8_t bleu, uint8_t index_led);
-void ws2812_set_buffer(uint32_t couleur, uint8_t index_led);
 void ws2812_arc_en_ciel(void);
\ No newline at end of file