From 4e898151f57104f9c41d36db0266810129ea0794 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Thu, 13 Apr 2023 19:43:35 +0200
Subject: [PATCH] =?UTF-8?q?Re-modification=20de=20Trajet=5Fterminee,=20rec?=
 =?UTF-8?q?alage=20X=20lorsqu'on=20attrappe=20les=20cerise,=20Gestion=20de?=
 =?UTF-8?q?=20l'=C3=A9vitement=20:=20fonction=20geom=C3=A9triques=20+=20lo?=
 =?UTF-8?q?gique=20=C3=A0=20base=20de=20c=C3=B4nes=20de=20d=C3=A9tection?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .vscode/settings.json     |   4 +-
 Balise_VL53L1X.c          |  89 ++++++++++++++++++++++++-----
 CMakeLists.txt            |   1 +
 Geometrie.c               |  19 +++++++
 Geometrie.h               |   1 +
 Geometrie_robot.h         |   5 +-
 Holonome2023.c            |   5 +-
 Strategie.c               |  35 ++++++++----
 Strategie.h               |   6 +-
 Strategie_prise_cerises.c |   3 +-
 Strategie_prise_cerises.h |   2 +-
 Test_strategie.c          | 115 ++++++++++++++++++++++++++++++++++++--
 Trajet.c                  |   8 +--
 13 files changed, 252 insertions(+), 41 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index ff50aa4..1d9f24e 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -6,6 +6,8 @@
         "strategie.h": "c",
         "trajectoire.h": "c",
         "trajet.h": "c",
-        "asser_position.h": "c"
+        "asser_position.h": "c",
+        "strategie_prise_cerises.h": "c",
+        "geometrie.h": "c"
     }
 }
\ No newline at end of file
diff --git a/Balise_VL53L1X.c b/Balise_VL53L1X.c
index fd6a5a1..d7ed7bc 100644
--- a/Balise_VL53L1X.c
+++ b/Balise_VL53L1X.c
@@ -1,23 +1,29 @@
 #include <stdio.h>
+#include "Geometrie_robot.h"
 #include "i2c_annexe.h"
 #include "Localisation.h"
 #include "math.h"
 
 #define NB_CAPTEURS 12
-#define DISTANCE_OBSTACLE_IGNORE 200
+#define DISTANCE_OBSTACLE_IGNORE_MM 2000
 #define DISTANCE_CAPTEUR_CENTRE_ROBOT_CM 4
+#define DISTANCE_TROP_PRES_CM 3
+#define DEMI_CONE_CAPTEUR_RADIAN 0.2225
 
 uint8_t distance_capteur_cm[NB_CAPTEURS];
 
 struct capteur_VL53L1X_t{
-    uint8_t distance_cm;
-    double angle_ref_robot;
-    double angle_ref_terrain;
+    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 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;
 }capteurs_VL53L1X[NB_CAPTEURS];
 
 void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm);
-void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct position_t position_robot);
+void invalide_obstacle(struct capteur_VL53L1X_t * capteur_VL53L1X, struct position_t position_robot);
 
 void Balise_VL53L1X_gestion(){
     i2c_annexe_get_distances(distance_capteur_cm);
@@ -32,13 +38,16 @@ void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t dista
     // Actualisation de l'angle du capteur
     capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian;
     // Maintien de l'angle entre -PI et 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;
     }
     while(capteur_VL53L1X->angle_ref_terrain < 2* -M_PI){
         capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
     }
-    capteur_VL53L1X->distance_cm = distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_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;
 
     invalide_obstacle(capteur_VL53L1X, position_robot);
 
@@ -50,12 +59,12 @@ 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_cm * 10);
-    position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_cm * 10);
+    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);
 
     capteur_VL53L1X->donnee_valide=1;
     // Si la distance vaut 0, à invalider
-    if(capteur_VL53L1X->distance_cm <= DISTANCE_CAPTEUR_CENTRE_ROBOT_CM){
+    if(capteur_VL53L1X->distance_lue_cm <= DISTANCE_TROP_PRES_CM){
         capteur_VL53L1X->donnee_valide=0;
     }
     // Si l'obstacle est à l'extérieur du terrain (on prend 50 mm de marge à l'intérieur du terrain, la balise faisant 100 mm)
@@ -83,17 +92,67 @@ void Balise_VL53L1X_init(){
 }
 
 uint8_t Balise_VL53L1X_get_min_distance(void){
-    uint8_t min_distance = DISTANCE_OBSTACLE_IGNORE;
+    uint8_t min_distance_cm = DISTANCE_OBSTACLE_IGNORE_MM / 10;
     for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
         if(capteurs_VL53L1X[capteur].donnee_valide){
-            if(min_distance> capteurs_VL53L1X[capteur].distance_cm){
-                min_distance = capteurs_VL53L1X[capteur].distance_cm;
+            if(min_distance_cm> capteurs_VL53L1X[capteur].distance_lue_cm){
+                min_distance_cm = capteurs_VL53L1X[capteur].distance_lue_cm;
             }
         }
     }
-    return min_distance;
+    return min_distance_cm;
 }
 
 uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
-    return capteurs_VL53L1X[capteur].distance_cm;
+    return capteurs_VL53L1X[capteur].distance_lue_cm;
+}
+
+/// @brief Renvoi l'obstacle le plus proche en tenant compte de la direction d'avancement du robot dans le référentiel du terrain.
+/// La fonction utilise 3 cônes de détection : 
+/// * +/- 22°, à 800 mm
+/// * +/- 50°, à 580 mm
+/// * +/- 90°, à 350 mm
+/// @param angle_avancement_radiant : angle d'avancement du robot entre -PI et PI
+/// @return 
+double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
+    const uint8_t NB_CONE=3;
+    struct cone_t{
+        double distance_mm;
+        double angle;
+    } cone[NB_CONE];
+    cone[0].angle = 22 * DEGRE_EN_RADIAN;
+    cone[0].distance_mm = 800;
+
+    cone[1].angle = 50 * DEGRE_EN_RADIAN;
+    cone[1].distance_mm = 580;
+
+    cone[2].angle = 90 * DEGRE_EN_RADIAN;
+    cone[2].distance_mm = 350;
+
+    double angle_min, angle_max;
+    double distance_minimale = DISTANCE_OBSTACLE_IGNORE_MM;
+    
+    for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
+        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,
+                    capteurs_VL53L1X[capteur].angle_ref_terrain_min, capteurs_VL53L1X[capteur].angle_ref_terrain_max)){
+                if(capteurs_VL53L1X[capteur].donnee_valide){
+                    if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale){
+                        distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm;
+                    }
+                }
+            }
+        }
+    }
+
+    // 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);
+        if(distance_minimale < 0){
+            distance_minimale = 0;
+        }
+    }
+
+    return distance_minimale;
 }
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 044c30a..9bdce67 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,6 +15,7 @@ Asser_Position.c
 Balise_VL53L1X.c
 Commande_vitesse.c
 QEI.c
+Geometrie.c
 gyro.c
 gyro_L3GD20H.c
 gyro_ADXRS453.c
diff --git a/Geometrie.c b/Geometrie.c
index ee5d2a2..bb8414c 100644
--- a/Geometrie.c
+++ b/Geometrie.c
@@ -37,4 +37,23 @@ unsigned int Geometrie_compare_angle(double angle, double angle_min, double angl
         }
         return 0;
     }
+}
+
+/// @brief Indique si les deux plages d'angle se recoupent
+/// @param angle1_min Début de la première plage
+/// @param angle1_max Fin de la première plage
+/// @param angle2_min Début de la seconde plage
+/// @param angle2_max Fin de la seconde plage
+/// @return 1 si les deux plages s'intersectent, 0 sinon
+unsigned int Geometrie_intersecte_plage_angle(double angle1_min, double angle1_max, double angle2_min, double angle2_max){
+    // Pour que les plages s'intersectent, soit :
+    // * angle1_min est compris entre angle2_min et angle2_max
+    // * angle1_max est compris entre angle2_min et angle2_max
+    // * angle2_min et angle2_max sont compris entre angle1_min et angle1_max (tester angle2_min ou angle2_max est suffisant)
+    if(Geometrie_compare_angle(angle1_min, angle2_min, angle2_max) ||
+        Geometrie_compare_angle(angle1_max, angle2_min, angle2_max) ||
+        Geometrie_compare_angle(angle2_min, angle1_min, angle1_max)){
+            return 1;
+    }
+    return 0;
 }
\ No newline at end of file
diff --git a/Geometrie.h b/Geometrie.h
index 97f7de4..34feeee 100644
--- a/Geometrie.h
+++ b/Geometrie.h
@@ -14,5 +14,6 @@ struct position_t{
 
 double Geometrie_get_angle_normalisee(double angle);
 unsigned int Geometrie_compare_angle(double angle, double angle_min, double angle_max);
+unsigned int Geometrie_intersecte_plage_angle(double angle1_min, double angle1_max, double angle2_min, double angle2_max);
 
 #endif
diff --git a/Geometrie_robot.h b/Geometrie_robot.h
index 7c11d91..44b6076 100644
--- a/Geometrie_robot.h
+++ b/Geometrie_robot.h
@@ -5,4 +5,7 @@
 
 // Distance entre le centre du robot et un bord du robot
 #define PETIT_RAYON_ROBOT_MM 108.2
-#define RACINE_DE_3 1.73205081
\ No newline at end of file
+#define RACINE_DE_3 1.73205081
+
+// Géométrie liée à l'adversaire
+#define RAYON_ROBOT_ADVERSE_MM 240
diff --git a/Holonome2023.c b/Holonome2023.c
index 536b6fc..c721f1c 100644
--- a/Holonome2023.c
+++ b/Holonome2023.c
@@ -75,8 +75,9 @@ int main() {
     AsserMoteur_Init();
     Localisation_init();
 
-    //while(mode_test());
-    test_homologation();
+    while(mode_test());
+    //test_panier();
+    //test_homologation();
 
     Gyro_Init();
 
diff --git a/Strategie.c b/Strategie.c
index ee14712..2c8562b 100644
--- a/Strategie.c
+++ b/Strategie.c
@@ -12,7 +12,7 @@
 #include "math.h"
 
 #define SEUIL_RECAL_DIST_MM 75
-#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN)
+#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGRE_EN_RADIAN)
 #define DISTANCE_OBSTACLE_CM 50
 #define DISTANCE_PAS_OBSTACLE_MM 2000
 
@@ -31,7 +31,7 @@ void Homologation(uint32_t step_ms){
 
     switch(etat_strategie){
         case STRATEGIE_INIT:
-            Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN);
+            Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
             etat_strategie = ATTENTE_TIRETTE;
             break;
 
@@ -43,14 +43,14 @@ void Homologation(uint32_t step_ms){
 
         case APPROCHE_CERISE_1_A:
             Trajet_config(250, 500);
-            Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, 30. * DEGREE_EN_RADIAN);
+            Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGRE_EN_RADIAN, 30. * DEGRE_EN_RADIAN);
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
                 etat_strategie = ATTRAPE_CERISE_1;
             }
             break;
         
         case ATTRAPE_CERISE_1:
-            etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
+            etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
             if(etat_action == ACTION_TERMINEE){
                 etat_strategie = APPROCHE_PANIER_1;
             }
@@ -62,7 +62,7 @@ void Homologation(uint32_t step_ms){
                                 485, Localisation_get().y_mm,
                                 465, 857,
                                 465,2830,
-                                +30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
+                                +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
 
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_strategie = CALAGE_PANIER_1;
@@ -72,7 +72,7 @@ void Homologation(uint32_t step_ms){
         case APPROCHE_PANIER_2:
             Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                 265,2830,
-                                +120. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
+                                +120. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
 
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_strategie = CALAGE_PANIER_1;
@@ -81,7 +81,7 @@ void Homologation(uint32_t step_ms){
             break;
 
         case CALAGE_PANIER_1:
-            if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){
+            if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
                 etat_strategie = RECULE_PANIER;
             }
             break;
@@ -90,7 +90,7 @@ void Homologation(uint32_t step_ms){
             Trajet_config(250, 500);
             Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                 180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80,
-                                120. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN);
+                                120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
 
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_strategie = LANCE_DANS_PANIER;
@@ -100,7 +100,7 @@ void Homologation(uint32_t step_ms){
         case LANCE_DANS_PANIER:
             Asser_Position_maintien();
             if(lance_balles(step_ms) == ACTION_TERMINEE){
-                etat_strategie = APPROCHE_CERISE_2;
+                etat_strategie = STRATEGIE_FIN;
             }
             break;
 
@@ -108,7 +108,7 @@ void Homologation(uint32_t step_ms){
             Trajet_config(250, 500);
             Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                 830, 3000 - 156,
-                                Localisation_get().angle_radian, 30. * DEGREE_EN_RADIAN);
+                                Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
 
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_strategie = STRATEGIE_FIN;
@@ -116,7 +116,7 @@ void Homologation(uint32_t step_ms){
             break;
 
         case ATTRAPPE_CERISE_2:
-            etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms);
+            etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
             if(etat_action == ACTION_TERMINEE){
                 etat_strategie = APPROCHE_PANIER_2;
             }
@@ -265,4 +265,17 @@ enum couleur_t lire_couleur(void){
         return COULEUR_VERT;
     return COULEUR_BLEU;
 
+}
+
+/// @brief Décremente la temps de step_ms, renvoie 1 si la temporisation est écoulée
+/// @param tempo_ms 
+/// @param step_ms 
+/// @return 1 si la temporisation est écoulée, 0 sinon.
+int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
+    if(*tempo_ms < step_ms){
+        return 1;
+    }else{
+        *tempo_ms -= step_ms;
+        return 0;
+    }
 }
\ No newline at end of file
diff --git a/Strategie.h b/Strategie.h
index 4284fdf..ceb0d2b 100644
--- a/Strategie.h
+++ b/Strategie.h
@@ -6,7 +6,7 @@
 
 #define COULEUR 15
 #define TIRETTE 14
-#define DEGREE_EN_RADIAN  (M_PI / 180.)
+#define CORR_ANGLE_DEPART_DEGREE  (-1.145)
 
 enum etat_action_t{
     ACTION_EN_COURS,
@@ -50,6 +50,10 @@ enum etat_action_t depose_cerises(uint32_t step_ms);
 void Homologation(uint32_t step_ms);
 enum couleur_t lire_couleur(void);
 uint attente_tirette(void);
+enum etat_action_t lance_balles(uint32_t step_ms);
+int test_panier(void);
+
+int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
 
 
 
diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c
index becafc0..544400d 100644
--- a/Strategie_prise_cerises.c
+++ b/Strategie_prise_cerises.c
@@ -31,7 +31,7 @@ double vitesse_accostage_mm_s=100;
 /// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout.
 /// @param longer_direction : direction dans laquelle se trouve la bordure
 /// @return ACTION_EN_COURS ou ACTION_TERMINEE
-enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms){
+enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, double pos_recalage_x_mm){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     enum longer_direction_t longer_direction_aspire;
     static uint32_t tempo_ms = 0;
@@ -54,6 +54,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
             avance_puis_longe_bordure(longer_direction);
             if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ||
                 (longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
+                    Localisation_set_x(pos_recalage_x_mm);
                     etat_attrape = TURBINE_DEMARRAGE;
             }
             break;
diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h
index 5ef63bc..0221d08 100644
--- a/Strategie_prise_cerises.h
+++ b/Strategie_prise_cerises.h
@@ -1,2 +1,2 @@
 #include "Strategie.h"
-enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms);
+enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, double pos_x_mm);
diff --git a/Test_strategie.c b/Test_strategie.c
index 2e5341e..baefde7 100644
--- a/Test_strategie.c
+++ b/Test_strategie.c
@@ -19,6 +19,7 @@
 
 int test_accostage(void);
 int test_longe(void);
+int test_panier(void);
 int test_homologation(void);
 int test_evitement(void);
 int test_tirette_et_couleur();
@@ -64,10 +65,12 @@ void affichage_test_strategie(){
 
 
 int test_strategie(){
-    printf("L - longer.\n");
     printf("A - Accoster.\n");
-    printf("H - Homologation.\n");
     printf("C - Couleur et tirette.\n");
+    printf("H - Homologation.\n");
+    printf("L - Longer.\n");
+    printf("P - Panier.\n");
+    
     int lettre;
     do{
         lettre = getchar_timeout_us(0);
@@ -98,6 +101,11 @@ int test_strategie(){
             while(test_longe());
             break;
 
+        case 'p':
+        case 'P':
+            while(test_panier());
+            break;
+
         case 'q':
         case 'Q':
             return 0;
@@ -121,8 +129,6 @@ int test_homologation(){
 
     stdio_flush();
 
-    
-
     multicore_launch_core1(affichage_test_strategie);
 
     temps_ms = Temps_get_temps_ms();
@@ -234,6 +240,107 @@ int test_evitement(){
 }
 
 
+int test_panier(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+    printf("Test panier\n");
+
+    static enum{
+        TEST_PANIER_INIT,
+        TEST_PANIER_ASPIRE,
+        TEST_PANIER_TEMPO,
+        TEST_PANIER_LANCE_BALLES,
+        TEST_PANIER_PORTE_OUVERTE,
+    } etat_test_panier=TEST_PANIER_INIT;
+    static uint32_t tempo_ms;
+
+    i2c_maitre_init();
+    Trajet_init();
+    //printf("Init gyroscope\n");
+    set_position_avec_gyroscope(0);
+    if(get_position_avec_gyroscope()){
+        Gyro_Init();
+    }
+
+    stdio_flush();
+
+    multicore_launch_core1(affichage_test_strategie);
+
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_init = temps_ms;
+    do{
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        Balise_VL53L1X_gestion();
+        // Routines à 1 ms
+        if(temps_ms != Temps_get_temps_ms()){
+            temps_ms = Temps_get_temps_ms();
+            QEI_update();
+            Localisation_gestion();
+            AsserMoteur_Gestion(_step_ms);
+
+            // Routine à 2 ms
+            if(temps_ms % _step_ms_gyro == 0){
+                if(get_position_avec_gyroscope()){
+                    Gyro_Read(_step_ms_gyro);
+                }
+            }
+
+            switch(etat_test_panier){
+                case TEST_PANIER_INIT:
+                    if(lire_couleur()== COULEUR_VERT){
+                        i2c_annexe_ferme_porte();
+                        i2c_annexe_active_turbine();
+                        etat_test_panier=TEST_PANIER_ASPIRE;
+                    }
+                    break;
+
+                case TEST_PANIER_ASPIRE:
+                    if(lire_couleur()== COULEUR_BLEU){
+                        i2c_annexe_desactive_turbine();
+                        tempo_ms = 3000;
+                        etat_test_panier=TEST_PANIER_TEMPO;
+                    }
+                    break;
+                case TEST_PANIER_TEMPO:
+                    if(temporisation_terminee(&tempo_ms, _step_ms)){
+                        etat_test_panier=TEST_PANIER_LANCE_BALLES;
+                    }
+                    break;
+
+                case TEST_PANIER_LANCE_BALLES:
+                    if(lance_balles(_step_ms) == ACTION_TERMINEE){
+                        etat_test_panier=TEST_PANIER_PORTE_OUVERTE;
+                    }
+                    break;
+                
+
+                case TEST_PANIER_PORTE_OUVERTE:
+                    if(temporisation_terminee(&tempo_ms, _step_ms)){
+                        i2c_annexe_ouvre_porte();
+                    }
+                    if(lire_couleur()== COULEUR_BLEU){
+                        etat_test_panier=TEST_PANIER_INIT;
+                    }
+                    break;
+
+                
+            }
+
+        }
+        lettre = getchar_timeout_us(0);
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(1);
+    printf("STRATEGIE_LOOP_2\n");
+    printf("Lettre : %d; %c\n", lettre, lettre);
+    
+    if(lettre == 'q' && lettre == 'Q'){
+        return 0;
+    }
+    return 0;
+
+}
+
+
 int test_longe(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
 
diff --git a/Trajet.c b/Trajet.c
index ef3ebe9..7f84424 100644
--- a/Trajet.c
+++ b/Trajet.c
@@ -86,10 +86,10 @@ void Trajet_stop(double pas_de_temps_s){
 /// @param abscisse : abscisse sur la trajectoire
 /// @return 1 si le trajet est terminé, 0 sinon
 int Trajet_terminee(double abscisse){
-    if(abscisse >= 0.99 ){
+    /*if(abscisse >= 0.99 ){
         return 1;
-    }
-    /*
+    }*/
+    
     if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){
         if(abscisse >= 1 ){
             return 1;
@@ -98,7 +98,7 @@ int Trajet_terminee(double abscisse){
         if(abscisse >= 0.99 ){
             return 1;
         }
-    }*/
+    }
     return 0;
 }