From c66f1011bcd1632cfbe22179891c73fb63268e1c Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sun, 7 May 2023 00:12:29 +0200
Subject: [PATCH] =?UTF-8?q?Ajout=20d'un=20fonction=20d'init=20oubli=C3=A9e?=
 =?UTF-8?q?=20dans=20la=20strat=C3=A9gie=20+=20test=20compl=C3=A9mentaire?=
 =?UTF-8?q?=20pour=20LEDs?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Holonome2023.c |  1 +
 Test.c         | 56 ++++++++++++++++++++++++++++++++++++++++++++++----
 2 files changed, 53 insertions(+), 4 deletions(-)

diff --git a/Holonome2023.c b/Holonome2023.c
index 0c674cf..8c80ee0 100644
--- a/Holonome2023.c
+++ b/Holonome2023.c
@@ -85,6 +85,7 @@ int main() {
     //while(mode_test());
     i2c_maitre_init();
     Trajet_init();
+    Balise_VL53L1X_init();
 
     set_position_avec_gyroscope(1);
     if(get_position_avec_gyroscope()){
diff --git a/Test.c b/Test.c
index 18c0893..f837af2 100644
--- a/Test.c
+++ b/Test.c
@@ -1423,11 +1423,24 @@ int test_angle_balise(void){
     int lettre;
     float distance, angle=3.1281;
 
+    enum {
+        TEST_BLEU,
+        ATTENTE1,
+        TEST_VERT,
+        ATTENTE2,
+        TEST_RESET,
+        ATTENTE3,
+        TEST_ANGLE
+    }etat_test_led=TEST_BLEU;
+
     i2c_maitre_init();
     Balise_VL53L1X_init();
     Localisation_set(1000,1500,0);
 
     multicore_launch_core1(affiche_monitoring);
+    uint temps_ms, timer_ms=1000;
+
+    temps_ms = Temps_get_temps_ms();
     
     do{
         temps_cycle_check();
@@ -1435,13 +1448,48 @@ int test_angle_balise(void){
         i2c_gestion(i2c0);
         i2c_annexe_gestion();
         Balise_VL53L1X_gestion();
+        if(temps_ms != Temps_get_temps_ms()){
+            temps_ms = Temps_get_temps_ms();
 
-        distance = Balise_VL53L1X_get_distance_obstacle_mm(angle);
-        Trajet_set_obstacle_mm(distance);
+            switch(etat_test_led){
+                case TEST_BLEU:
+                    i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
+                    timer_ms--;
+                    if(timer_ms<2){
+                        etat_test_led=TEST_VERT;
+                        timer_ms=1000;
+                    }
+                    break;
+                
+                case TEST_VERT:
+                    i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
+                    timer_ms--;
+                    if(timer_ms<2){
+                        etat_test_led=TEST_RESET;
+                        timer_ms=10000;
+                    }
+                    break;
+
+                case TEST_RESET:
+                    i2c_annexe_couleur_balise(0, 0x00);
+                    timer_ms--;
+                    if(timer_ms<2){
+                        etat_test_led=TEST_ANGLE;
+                        timer_ms=0;
+                    }
+                    break;
+
+                case TEST_ANGLE:
+                    timer_ms++;
+                    angle=(float)timer_ms / 1000.;
+                    Trajet_set_obstacle_mm(Balise_VL53L1X_get_distance_obstacle_mm(angle));
+                    break;
+                
+            }
+        }
 
         lettre = getchar_timeout_us(0);
-    }while(1);
-    //}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
         
     return 0;