Demo 15 mars 2025

Demonstration.c

@@ -1,17 +1,30 @@
 #include "Holonome2023.h"
+#include "Strategie_2024.h"
 #include "Demonstration.h"
 
 #define TEST_TIMEOUT_US 10000000
-#define CAPTEUR_POUR_ATTENTE 11
+
+#define CAPTEUR_POUR_ATTENTE 6
+#define CAPTEUR_POUR_ATTENTE_DEVANT 6
+#define CAPTEUR_POUR_ATTENTE_GAUCHE 9
+#define CAPTEUR_POUR_ATTENTE_DROIT 3
+#define CAPTEUR_POUR_ATTENTE_ARRIERE 0
 
 
 int Demonstration_init(void);
 enum etat_action_t Demonstration_calage();
 enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm);
 enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees);
+enum etat_action_t Demonstration_tourne(float angle_degrees);
 enum etat_action_t Demonstration_bezier();
 enum etat_action_t Demonstration_attente();
-
+enum etat_action_t Demonstration_leve_bras(uint32_t bras);
+enum etat_action_t  Demonstration_baisse_bras(void);
+enum etat_action_t Demonstration_attrape_plante();
+enum etat_action_t Demonstration_attente_capteur(int capteur);
+void Demonstration_menu_balise(void);
+void Demonstration_prise_plante(void);
+void Demonstration_actionneurs(void);
 
 uint32_t temps_ms_demo = 0, temps_ms_old;
 
@@ -22,6 +35,7 @@ void demo_affiche_localisation(){
         printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
         printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie() / 10.);
         printf(">capteur:%d\n", Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE));
+        sleep_ms(50);
     }
 }
 
@@ -29,6 +43,7 @@ int Demonstration_menu(void){
     static int iteration = 2;
     int rep;
     printf("Mode demo - init\n");
+    set_position_avec_gyroscope(false);
     Demonstration_init();
     while(1){
         do{
@@ -40,6 +55,10 @@ int Demonstration_menu(void){
             /*printf("C - Trajets enchaines - manuels\n");
             printf("D - Trajets enchaines - auto\n");
             printf("E - Asservissement angulaire\n");*/
+            printf("F - Rotation et actionneurs\n");
+            printf("G - Prise plante\n");
+            printf("M - Menu Balise\n");
+            printf("Z - Sem-automatique\n");
             printf("Q - Quitter\n");
             rep = getchar_timeout_us(TEST_TIMEOUT_US);
         }while(rep == 0 || rep == PICO_ERROR_TIMEOUT);
@@ -69,12 +88,35 @@ int Demonstration_menu(void){
             Demonstration_bezier();
             break;
 
+        case 'e':
         case 'E':
             printf("Début attente\n");
             Demonstration_attente();
             printf("Fin attente\n");
             break;
 
+        case 'f':
+        case 'F':
+            while(1){
+                printf("Demo actionneur\n");
+                Demonstration_actionneurs();
+            }
+            break;
+
+        case 'g':
+        case 'G':
+            printf("Début prise plante\n");
+            Demonstration_prise_plante();
+            printf("Fin prise plante\n");
+            break;
+
+        case 'm':
+        case 'M':
+            printf("Menu balise\n");
+            Demonstration_menu_balise();
+            printf("Fin menu balise\n");
+            break;
+
         case 'q':
         case 'Q':
             return 0;
@@ -82,6 +124,7 @@ int Demonstration_menu(void){
 
         case 'z':
         case 'Z':
+            printf("Demo semi-auto\n");
             Demonstration_semiauto();
             break;
 
@@ -135,6 +178,258 @@ void Demonstration_auto(){
     }
 }
 
+void Demonstration_actionneurs(){
+    Demonstration_attente();
+    Demonstration_attrape_plante();
+    Demonstration_tourne(30);
+    Demonstration_leve_bras(0);
+    Demonstration_tourne(-60);
+    Demonstration_leve_bras(5);
+    Demonstration_tourne(120);
+    Demonstration_leve_bras(1);
+    Demonstration_tourne(-180);
+    Demonstration_leve_bras(4);
+    Demonstration_tourne(240);
+    Demonstration_leve_bras(2);
+    Demonstration_tourne(-300);
+    Demonstration_leve_bras(3);
+    Demonstration_tourne(150);
+    Demonstration_baisse_bras();
+}
+
+void Demonstration_prise_plante(){
+    static int32_t m_temps_ms, m_timer;
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    enum etat_action_t etat_action_step;
+    static enum {
+        INIT,
+        TIENT_POT,
+        LEVE_POT,
+        ATTRAPE_PLANTE_1,
+        ATTRAPE_PLANTE_2,
+        RETOUR_MAISON
+    } etat_prise_plante = INIT;
+    while(etat_action == ACTION_EN_COURS){
+        Holonome_cyclique(PARAM_DEFAULT);
+        if(m_temps_ms != Temps_get_temps_ms()){
+            m_temps_ms = Temps_get_temps_ms();
+
+            switch (etat_prise_plante){
+                case INIT:
+                    m_timer = 3000;
+                    etat_prise_plante = TIENT_POT;
+                    etat_action = ACTION_EN_COURS;
+
+                case TIENT_POT:
+                    m_timer--;
+                    i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
+                    if(m_timer <= 0){
+                        etat_prise_plante = LEVE_POT;
+                        m_timer = 1000;
+                    }
+                    break;
+                case LEVE_POT:
+                    m_timer--;
+                    i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
+                    if(m_timer <= 0){
+                        etat_prise_plante = ATTRAPE_PLANTE_1;
+                    }
+                    break;
+                case ATTRAPE_PLANTE_1:
+                    etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_1, ZONE_PLANTE_1);
+                    if(etat_action_step == ACTION_TERMINEE){
+                        etat_prise_plante = ATTRAPE_PLANTE_2;
+                    }
+                    break;
+                case ATTRAPE_PLANTE_2:
+                    etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_6, ZONE_PLANTE_1);
+                    if(etat_action_step == ACTION_TERMINEE){
+                        etat_prise_plante = RETOUR_MAISON;
+                    }
+                    break;
+                case RETOUR_MAISON:{
+                    etat_action = Strategie_aller_a(0, 0, EVITEMENT_SANS_EVITEMENT, 1);
+                    if(etat_action == ACTION_TERMINEE){
+                        etat_prise_plante = INIT;
+                    }
+                }
+
+            }
+        }
+    }
+
+    Moteur_Stop();
+}
+
+void Balise_cli_orange_maintenance(int nb_cli){
+    int32_t temps_court_ms = 150, temps_long_ms = 700;
+    static int32_t temps_ms_led_cli, timer_led_ms = 0;
+    static int32_t m_nb_cli;
+    static enum etat_led_cli_orange_maintenance_t{
+        CLI_ON_COURT,
+        CLI_OFF_COURT,
+        CLI_OFF_LONG,
+    }etat_led_cli_orange_maintenance=CLI_OFF_LONG;
+    // Toutes les 1 ms.
+    if(temps_ms_led_cli != Temps_get_temps_ms()){
+        temps_ms_led_cli = Temps_get_temps_ms();
+
+        timer_led_ms--;
+
+        switch(etat_led_cli_orange_maintenance){
+            case CLI_ON_COURT:
+                if(timer_led_ms<= 0){
+                    i2c_annexe_couleur_balise(0, 0xFFFF);
+                    timer_led_ms = temps_court_ms;
+                    m_nb_cli--;
+                    if(m_nb_cli){
+                        etat_led_cli_orange_maintenance=CLI_OFF_COURT;
+                        timer_led_ms = temps_court_ms;
+                    }else{
+                        etat_led_cli_orange_maintenance=CLI_OFF_LONG;
+                        timer_led_ms = temps_long_ms;
+                    }
+                }
+                break;
+            case CLI_OFF_COURT:
+                if(timer_led_ms<= 0){
+                    i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
+                    timer_led_ms = temps_court_ms;
+                    etat_led_cli_orange_maintenance=CLI_ON_COURT;
+                }
+                break;
+            case CLI_OFF_LONG:
+                m_nb_cli = nb_cli;
+                if(timer_led_ms<= 0){
+                    i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
+                    timer_led_ms = temps_court_ms;
+                    etat_led_cli_orange_maintenance=CLI_ON_COURT;
+                }
+                break;
+        }
+   
+    }
+}
+
+void Balise_pulse_vert(){
+    static int32_t temps_ms_led_cli, timer_led_ms = 800;
+    int32_t timer_led_max_ms =800;
+    int32_t vert_led, vert_led_max = 0b111;
+    static enum etat_led_pulse_vert_t{
+        PULSE_RISE,
+        PULSE_FALL,
+    }etat_led_pulse_vert=PULSE_FALL;
+    // Toutes les 1 ms.
+    if(temps_ms_led_cli != Temps_get_temps_ms()){
+        temps_ms_led_cli = Temps_get_temps_ms();
+
+        timer_led_ms--;
+
+        switch(etat_led_pulse_vert){
+            case PULSE_RISE:
+                vert_led = (vert_led_max-1)  - (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
+                vert_led = vert_led & 0b111;
+                i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
+                if(timer_led_ms<= 0){
+                    i2c_annexe_couleur_balise(0, 0xFFFF);
+                    timer_led_ms = timer_led_max_ms;
+                    etat_led_pulse_vert=PULSE_FALL;
+                }
+                break;
+            case PULSE_FALL:
+                vert_led = (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
+                vert_led = vert_led & 0b111;
+                i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
+                if(timer_led_ms<= 0){
+                    timer_led_ms = timer_led_max_ms;
+                    etat_led_pulse_vert=PULSE_RISE;
+                }
+                break;
+        }
+   
+    }
+}
+
+void Demonstration_menu_balise(void){
+    int nb_cli=2;
+    while(1){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        Balise_cli_orange_maintenance(nb_cli);
+        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_DROIT) == ACTION_TERMINEE){
+            nb_cli++;
+            printf(">nb_cli%d\n",nb_cli);
+        }
+        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_GAUCHE) == ACTION_TERMINEE){
+            nb_cli--;
+            printf(">nb_cli%d\n",nb_cli);
+        }
+        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_DEVANT) == ACTION_TERMINEE){
+            return;
+        }
+        if(Demonstration_attente_capteur(CAPTEUR_POUR_ATTENTE_ARRIERE) == ACTION_TERMINEE){
+            switch(nb_cli){
+                case 2:
+                    Demonstration_actionneurs();
+                    break;
+                case 3:
+                    Demonstration_prise_plante();
+                    break;
+            }
+        }
+
+    }
+}
+
+/// @brief Comme la fonction demonstration attente, mais en choisissant le capteur. Fonction non-bloquante
+/// @param capteur: numéro du capteur, 0 à l'arrière, 3 à droite, 6 devant, 9, à gauche
+/// @return ACTION_TERMINEE si nous avons un signal pour ce capteur
+enum etat_action_t Demonstration_attente_capteur(int capteur){
+    static enum {
+        ATTENTE_DETECTION,
+        DETECTION_PROCHE,
+        FIN_ATTENTE
+    } etat_attente[12] = {ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,
+    ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION,ATTENTE_DETECTION};
+    static uint32_t temps_debut_tempo[12];
+    uint32_t duree_tempo_ms = 50;
+    
+    switch(etat_attente[capteur]){
+        case ATTENTE_DETECTION:
+            if(Balise_VL53L1X_get_capteur_cm(capteur) < 15 && Balise_VL53L1X_get_capteur_cm(capteur) > 1){
+                /// Sans obstacle, le capteur peut renvoyer 0;
+                etat_attente[capteur]=DETECTION_PROCHE;
+                temps_debut_tempo[capteur] = time_us_32();
+            }
+            break;
+        
+        case DETECTION_PROCHE:
+            if(Balise_VL53L1X_get_capteur_cm(capteur) > 15 || Balise_VL53L1X_get_capteur_cm(capteur) < 1){
+                // On a perdu la detection avant le temps écoulé
+                etat_attente[capteur]=ATTENTE_DETECTION;
+            }
+            if((temps_debut_tempo[capteur] + (duree_tempo_ms * 1000)) < time_us_32()){
+                // temps écoulé
+                etat_attente[capteur]=FIN_ATTENTE;
+            }
+            break;
+
+        case FIN_ATTENTE:
+            if(Balise_VL53L1X_get_capteur_cm(capteur) > 15 || Balise_VL53L1X_get_capteur_cm(capteur) < 1){
+                // On a perdu la detection après le temps écoulé
+                etat_attente[capteur]=ATTENTE_DETECTION;
+                return ACTION_TERMINEE;
+            }
+            break;
+            
+    }
+    return ACTION_EN_COURS;
+    //sleep_ms(20);
+    
+
+}
+
 enum etat_action_t Demonstration_attente(){
     enum {
         ATTENTE_DETECTION,
@@ -145,6 +440,7 @@ enum etat_action_t Demonstration_attente(){
 
     while(true){
         Holonome_cyclique(PARAM_NO_MOTORS);
+        Balise_cli_orange_maintenance(2);
         
         switch(etat_attente){
             case ATTENTE_DETECTION:
@@ -376,6 +672,52 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_
     return ACTION_ECHEC;
 }
 
+/// @brief Rotation du robot sur lui-même du robot 
+/// @param angle_degrees : Rotation du robot sur lui-même
+/// @return ACTION_TERMINEE
+enum etat_action_t Demonstration_tourne(float angle_degrees){
+    enum {
+        DEMO_TOURNE,
+        DEMO_TOURNE_TERMINE
+    } etat_avance_puis_tourne = DEMO_TOURNE;
+    int pos_x_init_mm, pos_y_init_mm;
+    while(true){
+        struct trajectoire_t trajectoire;
+
+        Holonome_cyclique(PARAM_DEFAULT);
+
+        // Toutes les 1 ms.
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+
+        
+            switch (etat_avance_puis_tourne)
+            {
+            
+            case DEMO_TOURNE:
+                Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
+                Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
+                if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_avance_puis_tourne = DEMO_TOURNE_TERMINE;
+                    Trajet_config(TRAJECT_CONFIG_STD);
+                }
+                break;
+            
+            case DEMO_TOURNE_TERMINE:
+                etat_avance_puis_tourne = DEMO_TOURNE;
+                Moteur_Stop();
+                return ACTION_TERMINEE;
+            
+            default:
+                break;
+            }
+        }
+        
+    }
+    return ACTION_ECHEC;
+}
+
 /// @brief Déplacement suivant deux courbes de Bézier. Recommandé pour une démo sur une planche de 1m x 1,5m
 /// @return ACTION_TERMINEE
 enum etat_action_t Demonstration_bezier(){
@@ -434,3 +776,128 @@ enum etat_action_t Demonstration_bezier(){
     }
     return ACTION_ECHEC;
 }
+
+/// @brief Leve le bras, et agite le doigt
+/// @param bras 
+enum etat_action_t  Demonstration_leve_bras(uint32_t bras){
+    static enum{
+        LB_LEVE_BRAS,
+        LB_TEMPO_BRAS,
+        LB_DOIGT_ATTRAPE_1,
+        LB_DOIGT_LACHE,
+        LB_DOIGT_ATTRAPE_2,
+    } DLB_status;
+    int timer_ms;
+    while(true){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+            timer_ms--;
+
+            switch(DLB_status){
+                case LB_LEVE_BRAS:
+                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
+                    DLB_status = LB_TEMPO_BRAS;
+                    timer_ms = 250;
+                    break;
+
+                case LB_TEMPO_BRAS:
+                    if(timer_ms <= 0){
+                        DLB_status = LB_DOIGT_ATTRAPE_1;
+                        timer_ms=150;
+                    }
+                    break;
+
+                case LB_DOIGT_ATTRAPE_1:
+                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
+                    if(timer_ms <= 0){
+                        DLB_status = LB_DOIGT_LACHE;
+                        timer_ms=200;
+                    }
+                    break;
+
+                case LB_DOIGT_LACHE:
+                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
+                    if(timer_ms <= 0){
+                        DLB_status = LB_DOIGT_ATTRAPE_2;
+                        timer_ms=200;
+                    }
+                    break;
+
+                case LB_DOIGT_ATTRAPE_2:
+                    i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
+                    if(timer_ms <= 0){
+                        DLB_status = LB_LEVE_BRAS;
+                        return ACTION_TERMINEE;
+                    }
+                    break;
+
+            }
+        }
+        
+        
+    }
+}
+enum etat_action_t  Demonstration_attrape_plante(void){
+    int32_t timer_ms;
+    enum {
+        DAP_ENVOI,
+        DAP_ATTENTE
+    } dap_status=DAP_ENVOI;
+    while(true){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+            timer_ms--;
+
+            switch(dap_status){
+                case DAP_ENVOI:
+                    i2c_annexe_attrape_plante(PLANTE_BRAS_1);
+                    dap_status = DAP_ATTENTE;
+                    timer_ms=5000;
+                    break;
+
+                case DAP_ATTENTE:
+                    if (timer_ms <= 0){
+                        dap_status=DAP_ENVOI;
+                        return ACTION_TERMINEE;
+                    }
+                    break;            
+            }
+            
+        }
+    }
+}
+
+enum etat_action_t  Demonstration_baisse_bras(void){
+    static int32_t timer_ms;
+    enum {
+        DBB_ENVOI,
+        DBB_ATTENTE
+    } dbb_status=DBB_ENVOI;
+    while(true){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+            timer_ms--;
+
+            switch(dbb_status){
+                case DBB_ENVOI:
+                    for(int bras=0; bras < 6; bras++){
+                        i2c_annexe_actionneur_pot(bras, BRAS_PLIE, DOIGT_LACHE);
+                    }
+                    dbb_status = DBB_ATTENTE;
+                    timer_ms=500;
+                    break;
+
+                case DBB_ATTENTE:
+                    if (timer_ms <= 0){
+                        dbb_status = DBB_ENVOI;
+                        return ACTION_TERMINEE;
+                    }
+                    break;            
+            }
+            
+        }
+    }
+}

Demonstration.h

@@ -1,4 +1,5 @@
  int Demonstration_menu(void);
  void Demonstration_semiauto(void);
  void Demonstration_auto(void);
- int Demonstration_init(void);
+ int Demonstration_init(void);
+ void Demonstration_prise_plante(void);

Holonome2023.c

@@ -44,8 +44,9 @@ int main() {
 
     stdio_init_all();
     
-    //Demonstration_init();Demonstration_auto();
-    //while(mode_test());
+    //Demonstration_init();Demonstration_prise_plante();
+    //while(1);
+    while(mode_test());
     //test_pseudo_homologation();
     Holonome2023_init();
 
@@ -110,7 +111,7 @@ int main() {
                     break;
 
                 case MATCH_EN_COURS:
-                    if (timer_match_ms > 98000){ // 98 secondes
+                    if (timer_match_ms > 100000){ // 90 secondes
                         printf("MATCH_ARRET_EN_COURS\n");
                         statu_match = MATCH_ARRET_EN_COURS;
                     }
@@ -119,9 +120,6 @@ int main() {
                 
                 case MATCH_ARRET_EN_COURS:
                     commande_vitesse_stop();
-                    if(Robot_est_dans_zone_depose(couleur)){
-                        Score_set_pieds_dans_plat();
-                    } 
 
                     if (timer_match_ms > 100000){ // 100 secondes
                         statu_match = MATCH_TERMINEE;

Strategie.c

@@ -94,7 +94,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
 
     switch(etat_strategie){
         case STRATEGIE_INIT:
-            if(Strategie_calage_debut(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){
+            if(Strategie_calage_bas(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){
                 etat_strategie = STRATEGIE_FIN;
             }
             break;
@@ -455,7 +455,7 @@ enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_
     }
 }
 
-enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms){
+enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms){
     // 1 Envoyer la commande pour détecter la bordure
     // 2 Si la valeur de la bordure est valide, lire l'angle et la distance.
     //      Recaler la distance Y
@@ -473,25 +473,31 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
     enum validite_vl53l8_t validite;
     struct trajectoire_t trajectoire;
     enum etat_action_t etat_action;
+    static int tempo_ms;
 
     float angle, distance;
 
     switch(etat_calage_debut){
         case CD_ENVOI_CDE_BORDURE:
-            i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
+            i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
+            tempo_ms = 2000;
             etat_calage_debut = CD_LECTURE_BORDURE_Y;
             break;
         
         case CD_LECTURE_BORDURE_Y:
-            
+            tempo_ms--;
             i2c_annexe_get_VL53L8(&validite, &angle, &distance);
-            if(validite == VL53L8_BORDURE){
+            if(validite == VL53L8_DISTANCE_LOIN){
                 i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                 commande_vitesse_stop();
                 Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR);
                 Localisation_set_angle((-90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                 etat_calage_debut = CD_ROTATION_VERS_X;
             }
+            if(tempo_ms <= 0){
+                etat_calage_debut=CD_ENVOI_CDE_BORDURE;
+                return ACTION_ECHEC;
+            }
             break;
 
         case CD_ROTATION_VERS_X:
@@ -504,14 +510,16 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
                     (0 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
             }
             if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-                i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
+                i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
                 etat_calage_debut = CD_LECTURE_BORDURE_X;
+                tempo_ms = 2000;
             }
             break;
 
         case CD_LECTURE_BORDURE_X:
+            tempo_ms--;
             i2c_annexe_get_VL53L8(&validite, &angle, &distance);
-            if(validite == VL53L8_BORDURE){
+            if(validite == VL53L8_DISTANCE_LOIN){
                 i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                 commande_vitesse_stop();
                 if(couleur == COULEUR_BLEU){
@@ -521,30 +529,14 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_
                     Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
                     Localisation_set_angle((0. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
                 }
-                
-                etat_calage_debut = CD_ALLER_POSITION_INIT;
-            }
-            break;
-
-        case CD_ALLER_POSITION_INIT:
-            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            if(couleur == COULEUR_BLEU){
-                etat_action = Strategie_tourner_et_aller_a(225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms);
-            }else{
-                etat_action = Strategie_tourner_et_aller_a(3000 - 225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms);
-            }
-            if(etat_action == ACTION_TERMINEE){
                 etat_calage_debut = CD_ROTATION_POSITION_INIT;
+                return ACTION_TERMINEE;
+            }
+            if(tempo_ms <= 0){
+                etat_calage_debut=CD_ENVOI_CDE_BORDURE;
+                return ACTION_ECHEC;
             }
             break;
-        case CD_ROTATION_POSITION_INIT:
-            Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
-            /*Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
-                (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);*/
-            Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
-                0);
-            return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
-            //etat_calage_debut = CD_ENVOI_CDE_BORDURE;
     }
 
     return ACTION_EN_COURS;
@@ -572,22 +564,24 @@ enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_
     switch(etat_calage_debut){
         case CD_ENVOI_CDE_BORDURE:
             i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
-            etat_calage_debut = CD_LECTURE_BORDURE_X;
+            etat_calage_debut = CD_LECTURE_BORDURE_Y;
             break;
         
-        case CD_LECTURE_BORDURE_X:
+        case CD_LECTURE_BORDURE_Y:
             i2c_annexe_get_VL53L8(&validite, &angle, &distance);
             if(validite == VL53L8_BORDURE){
                 i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                 commande_vitesse_stop();
-                Localisation_set_y(215.);
-                if(angle < 0){
-                    Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
-                    Localisation_set_angle((-180. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
-                    Localisation_set_angle((-60. * DEGRE_EN_RADIAN));
+                
+                if(couleur == COULEUR_BLEU){
+                    Localisation_set_x(215.);
+                    Localisation_set_y(2000 - (distance + DISTANCE_CENTRE_CAPTEUR));
+                    //Localisation_set_angle((90. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
+                    Localisation_set_angle((210. * DEGRE_EN_RADIAN));
                 }else{
-                    Localisation_set_x(3025 - (distance + DISTANCE_CENTRE_CAPTEUR));
-                    Localisation_set_angle((-3. * DEGRE_EN_RADIAN) - ANGLE_PINCE + angle);
+                    Localisation_set_x(3000 - 215.);
+                    Localisation_set_y(2000 - (distance + DISTANCE_CENTRE_CAPTEUR));
+                    Localisation_set_angle((270. * DEGRE_EN_RADIAN));
                 }
                 
                 etat_calage_debut = CD_ALLER_POSITION_INIT;
@@ -597,4 +591,5 @@ enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_
     }
 
     return ACTION_EN_COURS;
-}
+}
+

Strategie.h

@@ -65,7 +65,7 @@ enum etat_action_t Strategie_preparation();
 enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms);
 enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms);
 
-enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms);
+enum etat_action_t Strategie_calage_bas(enum couleur_t couleur, uint32_t step_ms);
 enum etat_action_t Strategie_calage_debut_manuel(enum couleur_t couleur, uint32_t step_ms);
 enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, enum evitement_t evitement, uint32_t step_ms);
 enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms);

Strategie_2024.c

@@ -1,4 +1,6 @@
 #include "Strategie_2024.h"
+#include "Localisation.h"
+#include "Geometrie_robot.h"
 #include "pico/stdlib.h"
 #include "Balise_VL53L1X.h"
 #include "Temps.h"
@@ -34,17 +36,36 @@ enum etat_groupe_plante_t etat_groupe_plante[6]={
     GROUPE_PLANTE_DISPO
 };
 
+struct position_t approche_groupe_plante_bleu[6]={
+    {.x_mm= 0, .y_mm=0, .angle_radian = 0},
+    {.x_mm= 580, .y_mm=1530, .angle_radian = 0},
+    {.x_mm= 580, .y_mm=530, .angle_radian = 0},
+    {.x_mm= 1220, .y_mm=360, .angle_radian = 0}, 
+    {.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0},
+    {.x_mm= 3000 - 580, .y_mm=1530, .angle_radian = 0},
+};
+
+struct position_t approche_groupe_plante_jaune[6]={
+    {.x_mm= 0, .y_mm=0, .angle_radian = 0},
+    {.x_mm= 580, .y_mm=1530, .angle_radian = 0},
+    {.x_mm= 580, .y_mm=530, .angle_radian = 0},
+    {.x_mm= 3000 - 1220, .y_mm=360, .angle_radian = 0}, 
+    {.x_mm= 3000 - 580, .y_mm=530, .angle_radian = 0},
+    {.x_mm= 3000 - 580, .y_mm=1530, .angle_radian = 0},
+};
+
+
 int ordre_groupe_pot[6];
 
 unsigned int get_groupe_pot(enum couleur_t couleur){
     if(couleur == COULEUR_BLEU){
-        return GROUPE_POT_B1;
+        return GROUPE_POT_L1;
     }
-    return GROUPE_POT_B2;
+    return GROUPE_POT_R1;
 }
 enum zone_plante_t get_zone_plante(enum couleur_t couleur){
-    enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_4, ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_1};
-    enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_5, ZONE_PLANTE_6, ZONE_PLANTE_4, ZONE_PLANTE_3, ZONE_PLANTE_2, ZONE_PLANTE_1};
+    enum zone_plante_t ordre_groupe_plante_bleu[6] = { ZONE_PLANTE_2, ZONE_PLANTE_3, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
+    enum zone_plante_t ordre_groupe_plante_jaune[6] = { ZONE_PLANTE_6, ZONE_PLANTE_5, ZONE_PLANTE_4, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE, ZONE_PLANTE_AUCUNE};
     enum zone_plante_t *ordre_groupe_plante;
     int i;
     if(couleur == COULEUR_BLEU){
@@ -60,155 +81,282 @@ enum zone_plante_t get_zone_plante(enum couleur_t couleur){
     return ZONE_PLANTE_AUCUNE;
 }
 
+struct position_t get_position_approche_zone_plante(enum couleur_t couleur, enum zone_plante_t zone_plante){
+    if(couleur == COULEUR_BLEU){
+        return approche_groupe_plante_bleu[zone_plante];
+    }
+    return approche_groupe_plante_jaune[zone_plante];
+}
+
 void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
     int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init;
-    float angle_destination;
+    float angle_destination, pos_x;
     struct trajectoire_t trajectoire;
+    struct position_t position;
     enum evitement_t evitement;
     enum etat_action_t etat_action=ACTION_EN_COURS;
     static int tempo_ms;
+    static int nb_plante_ok = 0;
+    static int bras_depose = PLANTE_BRAS_1;
+    static int depose_en_cours = 0;
+    static int pre_fin_match=0;
 
     static enum {
         TAP_CALAGE,
+        TAP_PASSAGE_JAUNE,
         TAP_POT,
         TAP_PANNEAUX_SOLAIRES,
         TAP_ALLER_PLANTE,
         TAP_PLANTE_ORIENTATION,
-        TAP_PLANTE_ATTRAPE_1,
-        TAP_PLANTE_ATTRAPE_2,
+        TAP_PLANTE_ATTRAPE,
         TAP_ECHANGE_POT,
-        TAP_PLANTE_ATTRAPE_3,
-        TAP_PLANTE_ATTRAPE_4,
+        TAP_PLANTE_TERMINEE_ECHEC,
         TAP_RENTRE,
+        TAP_RENTRE_RECALE,
+        TAP_DEPOSE_0,
         TAP_DEPOSE_1,
+        TAP_PANNEAU_SOLAIRE_1,
         TAP_DEPOSE_2,
         TAP_DEPOSE_3,
+        TAP_RECHARGE,
         TAP_FINI
     } etat_test = TAP_CALAGE;
+    
+    if(temps_ms > 80000 && depose_en_cours == 0){
+        etat_test = TAP_RENTRE;
+        pre_fin_match = 1;
+    }
 
 
     switch(etat_test){
         case TAP_CALAGE:
             if(Strategie_calage_debut_manuel(couleur, _step_ms) == ACTION_TERMINEE){
+                if(couleur == COULEUR_BLEU){
+                    etat_test=TAP_POT;
+                }else{
+                    etat_test=TAP_PASSAGE_JAUNE;
+                }    
+            }
+            break;
+        
+        case TAP_PASSAGE_JAUNE:
+            if(Strategie_aller_a(2600, 1600, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                 etat_test=TAP_POT;
             }
             break;
+
         case TAP_POT:
             if(Strat_2024_attrape_pot(get_groupe_pot(couleur), _step_ms) == ACTION_TERMINEE){
-                etat_test=TAP_PANNEAUX_SOLAIRES;
-            }
-            break;
-
-        case TAP_PANNEAUX_SOLAIRES:
-            if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){
                 etat_test=TAP_ALLER_PLANTE;
             }
             break;
 
         case TAP_ALLER_PLANTE:
-            if(Strategie_aller_a(940, 300, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
+            position = get_position_approche_zone_plante(couleur, get_zone_plante(couleur));
+            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
+            if(Strategie_aller_a(position.x_mm, position.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
                 etat_test=TAP_PLANTE_ORIENTATION;
             }
             break;
 
         case TAP_PLANTE_ORIENTATION:
             if(Strat_2024_aller_zone_plante(get_zone_plante(couleur), _step_ms) == ACTION_TERMINEE){
-                etat_test=TAP_PLANTE_ATTRAPE_1;
+                etat_test=TAP_PLANTE_ATTRAPE;
             }
             break;
 
-        case TAP_PLANTE_ATTRAPE_1:
-            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
+        case TAP_PLANTE_ATTRAPE:
+            etat_action = Strat_2024_plante_dans_pot(_step_ms, bras_depose, get_zone_plante(couleur));
             if( etat_action == ACTION_TERMINEE){
-                etat_test=TAP_PLANTE_ATTRAPE_2;
+                etat_test=TAP_PLANTE_ATTRAPE;
+                nb_plante_ok++;
+                if(bras_depose == PLANTE_BRAS_1){
+                    bras_depose = PLANTE_BRAS_6;
+                }else{
+                    bras_depose = PLANTE_BRAS_1;
+                }
+                if(nb_plante_ok == 2){
+                    etat_test = TAP_ECHANGE_POT;
+                }
+                if(nb_plante_ok == 4){
+                    etat_test = TAP_RENTRE;
+                }
                 etat_action = ACTION_EN_COURS;
             }else if( etat_action == ACTION_ECHEC){
-                etat_test=TAP_RENTRE;
-                etat_action = ACTION_EN_COURS;
-            }
-            break;
-        
-        case TAP_PLANTE_ATTRAPE_2:
-            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, get_zone_plante(couleur));
-            if( etat_action == ACTION_TERMINEE){
-                etat_test=TAP_ECHANGE_POT;
-                etat_action = ACTION_EN_COURS;
-            }else if( etat_action == ACTION_ECHEC){
-                etat_test=TAP_RENTRE;
+                etat_test=TAP_PLANTE_TERMINEE_ECHEC;
                 etat_action = ACTION_EN_COURS;
             }
             break;
 
         case TAP_ECHANGE_POT:
             if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
-                etat_test=TAP_PLANTE_ATTRAPE_3;
-            }
-            break;
-        
-        case TAP_PLANTE_ATTRAPE_3:
-            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
-            if( etat_action == ACTION_TERMINEE){
-                etat_test=TAP_PLANTE_ATTRAPE_4;
-                etat_action = ACTION_EN_COURS;
-            }else if( etat_action == ACTION_ECHEC){
-                etat_test=TAP_RENTRE;
-                etat_action = ACTION_EN_COURS;
+                etat_test=TAP_PLANTE_ATTRAPE;
             }
             break;
 
-        case TAP_PLANTE_ATTRAPE_4:
-            etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, get_zone_plante(couleur));
-            if( etat_action == ACTION_TERMINEE){
+        case TAP_PLANTE_TERMINEE_ECHEC:
+            // On note que la zone plante actuelle est "occupée/epuisée"
+            etat_groupe_plante[get_zone_plante(couleur)] = GROUPE_PLANTE_ECHEC;
+            // On va à la zone suivante ou on rentre
+            if(get_zone_plante(couleur) == ZONE_PLANTE_AUCUNE){
                 etat_test=TAP_RENTRE;
-                etat_action = ACTION_EN_COURS;
-            }else if( etat_action == ACTION_ECHEC){
-                etat_test=TAP_RENTRE;
-                etat_action = ACTION_EN_COURS;
+            }else{
+                etat_test=TAP_ALLER_PLANTE;
             }
             break;
 
         case TAP_RENTRE:
+            depose_en_cours = 1;
             if(couleur == COULEUR_BLEU){
-                angle_destination = 15 * DEGRE_EN_RADIAN;
+                angle_destination = 60 * DEGRE_EN_RADIAN;
+                pos_x = 450;
             }else{
-                angle_destination = (90-15) * DEGRE_EN_RADIAN;
+                angle_destination = 60 * DEGRE_EN_RADIAN;
+                pos_x = 3000-450;
             }
             
             Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            if(Strategie_tourner_et_aller_a(300, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
+            if(Strategie_tourner_et_aller_a(pos_x, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
+                etat_test=TAP_RENTRE_RECALE;
+            }
+            break;
+
+        case TAP_RENTRE_RECALE:
+            etat_action = Strategie_calage_bas(couleur, step_ms);
+            if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
+                etat_test=TAP_DEPOSE_0;
+            }
+            break;
+
+        case TAP_DEPOSE_0:
+            angle_destination = -30 * DEGRE_EN_RADIAN;
+            if(couleur == COULEUR_BLEU){
+                pos_x = 300;
+                
+            }else{
+                pos_x = 3000-300;
+            }
+            
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+            if(Strategie_aller_a_puis_tourner(pos_x, 280, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
+                i2c_annexe_actionneur_pot(POT_5, BRAS_LEVITE, DOIGT_TIENT);
                 etat_test=TAP_DEPOSE_1;
             }
             break;
         
         case TAP_DEPOSE_1:
             commande_vitesse_stop();
-            if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
+            if(couleur == COULEUR_BLEU){
+                if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_2 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
+                    etat_test=TAP_PANNEAUX_SOLAIRES;
+                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+                }
+            }else{
+                if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_2 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
+                    etat_test=TAP_PANNEAUX_SOLAIRES;
+                    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+                }
+            }
+            break;
+
+        case TAP_PANNEAUX_SOLAIRES:
+            if(Strat_2024_tourner_panneaux(couleur, step_ms) == ACTION_TERMINEE){
                 etat_test=TAP_DEPOSE_2;
             }
             break;
 
         case TAP_DEPOSE_2:
+            angle_destination = 150 * DEGRE_EN_RADIAN;
             if(couleur == COULEUR_BLEU){
-                angle_destination = (180+15) * DEGRE_EN_RADIAN;
+                pos_x = 550;
             }else{
-                angle_destination = (90-(180+15)) * DEGRE_EN_RADIAN;
+                pos_x = 3000-550;
             }
             Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            if(Strategie_aller_a_puis_tourner(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
+            if(Strategie_aller_a_puis_tourner(pos_x, 280, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
                 etat_test=TAP_DEPOSE_3;
             }
             break;
 
         case TAP_DEPOSE_3:
             commande_vitesse_stop();
-            if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
+            if(couleur == COULEUR_BLEU){
+                if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){
+                    etat_test=TAP_RECHARGE;
+                }
+            }else{
+                if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){
+                    etat_test=TAP_RECHARGE;
+                }
+            }
+            break;
+        
+        case TAP_RECHARGE:
+            if(rentre_recharge(couleur, step_ms) == ACTION_TERMINEE){
                 etat_test=TAP_FINI;
             }
             break;
 
         case TAP_FINI:
+            commande_vitesse_stop();
             break;
 
 
     }
-}
+}
+
+
+enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms){
+    struct trajectoire_t trajectoire;
+    enum etat_action_t etat_action;
+    enum validite_vl53l8_t validite;
+    float angle, distance, pos_x;
+
+    static int tempo_ms;
+
+    static enum {
+        RR_ORIENTE,
+        RR_RECALE,
+        RR_DEPLACE,
+    } etat_rentre_charge = RR_DEPLACE;
+
+    Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+
+    switch (etat_rentre_charge){
+        case RR_ORIENTE:
+            if(Strategie_tourner_a(60 * DEGRE_EN_RADIAN, step_ms) == ACTION_TERMINEE){
+                i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
+                commande_vitesse_stop();
+                tempo_ms = 2000;
+                etat_rentre_charge = RR_RECALE;
+            }
+            break;
+
+        case RR_RECALE:
+            tempo_ms--;
+            i2c_annexe_get_VL53L8(&validite, &angle, &distance);
+            if(validite == VL53L8_BORDURE){
+                i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
+                commande_vitesse_stop();
+                Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR);
+                etat_rentre_charge = RR_DEPLACE;
+            }
+            if(tempo_ms <= 0){
+                etat_rentre_charge =RR_DEPLACE;
+            }
+            break;
+
+        case RR_DEPLACE:
+            if(couleur == COULEUR_BLEU){
+                pos_x = Localisation_get().x_mm + 30;
+            }else{
+                pos_x = Localisation_get().x_mm - 30;
+            }
+            if( Strategie_aller_a(pos_x , 150, EVITEMENT_SANS_EVITEMENT, step_ms) == ACTION_TERMINEE){
+                etat_rentre_charge =RR_ORIENTE;
+                return ACTION_TERMINEE;
+            }
+            break;
+    }
+    return ACTION_EN_COURS;
+}

Strategie_2024.h

@@ -13,6 +13,7 @@
 #define STRATEGIE_2024_H
 
 void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms);
+enum etat_action_t rentre_recharge(enum couleur_t couleur, int step_ms);
 
 // STRATEGIE_H
 #endif

Strategie_2024_panneaux.c

@@ -7,6 +7,8 @@
 #include "Localisation.h"
 #include "Score.h"
 
+enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms);
+
 /// @brief Fonction demande au robot d'aller activer les panneaux solaires
 enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms){
     enum etat_action_t etat_action;
@@ -23,53 +25,30 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
         TP_TOURNE_PANNEAU_6,
         TP_DEGAGEMENT,
         TP_DEGAGEMENT_ACTIF
-    } etat_tourne_panneaux = TP_APPROCHE_PANNEAU_1;
+    } etat_tourne_panneaux = TP_TOURNE_PANNEAU_1;
 
 
     switch (etat_tourne_panneaux)
     {
-        case TP_APPROCHE_PANNEAU_1:
-            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            if(couleur == COULEUR_BLEU){
-                etat_action = Strategie_tourner_et_aller_a(
-                    323, 300, (150. *DEGRE_EN_RADIAN),
-                    EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
-            }else{
-                etat_action = Strategie_tourner_et_aller_a(
-                    3000 - 323, 300, (150. *DEGRE_EN_RADIAN),
-                    EVITEMENT_ARRET_DEVANT_OBSTACLE, step_ms);
-            }
-            if (etat_action == ACTION_TERMINEE){
-                etat_tourne_panneaux = TP_TOURNE_PANNEAU_1;
-            }else if(etat_action == ACTION_ECHEC){
-                return ACTION_TERMINEE;
-            }
-            break;
-
         case TP_TOURNE_PANNEAU_1:
             if(couleur == COULEUR_BLEU){
-                etat_action = Strategie_tourner_et_aller_a(
-                    323, 200, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
+                etat_action = Strategie_aller_a(315, 170, EVITEMENT_SANS_EVITEMENT, step_ms);
             }else{
-                etat_action = Strategie_tourner_et_aller_a(
-                    3000 - 323, 300, (150. *DEGRE_EN_RADIAN), EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
+                etat_action = Strategie_aller_a(3000 - 305, 170, EVITEMENT_SANS_EVITEMENT, step_ms);
             }
             if (etat_action == ACTION_TERMINEE){
-                Score_ajout_panneau(1);
                 etat_tourne_panneaux = TP_TOURNE_PANNEAU_2;
             }
             break;
 
         case TP_TOURNE_PANNEAU_2:
             if(couleur == COULEUR_BLEU){
-                Trajectoire_bezier(&trajectoire, 323, 200, 405, 310, 548, 400, 548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+                etat_action = active_panneau_solaire(562, step_ms);
             }else{
-                Trajectoire_bezier(&trajectoire, 3000-323, 200, 3000-405, 310, 3000-548, 400, 3000-548, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+                etat_action = active_panneau_solaire(3000 - 558, step_ms);
             }
-            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
             if (etat_action == ACTION_TERMINEE){
-                Score_ajout_panneau(1);
-                etat_tourne_panneaux = TP_DEGAGEMENT;
+                etat_tourne_panneaux = TP_TOURNE_PANNEAU_3;
             }else if(etat_action == ACTION_ECHEC){
                 etat_tourne_panneaux = TP_DEGAGEMENT;
             }
@@ -77,66 +56,19 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
 
         case TP_TOURNE_PANNEAU_3:
             if(couleur == COULEUR_BLEU){
-                Trajectoire_bezier(&trajectoire, 548, 200, 630, 310, 773, 400, 773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+                etat_action = active_panneau_solaire(795, step_ms);
             }else{
-                Trajectoire_bezier(&trajectoire, 3000-548, 200, 3000-630, 310, 3000-773, 400, 3000-773, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
+                etat_action = active_panneau_solaire(3000 - 802, step_ms);
             }
-            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
             if (etat_action == ACTION_TERMINEE){
-                Score_ajout_panneau(1);
-                etat_tourne_panneaux = TP_TOURNE_PANNEAU_4;
-            }else if(etat_action == ACTION_ECHEC){
                 etat_tourne_panneaux = TP_DEGAGEMENT;
-            }
-            break;
-
-        case TP_TOURNE_PANNEAU_4:
-            if(couleur == COULEUR_BLEU){
-                Trajectoire_bezier(&trajectoire, 773, 200, 855, 310, 1323, 400, 1323, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
-            }else{
-                Trajectoire_bezier(&trajectoire, 3000-773, 200, 3000-855, 310, 3000-1323, 400, 3000-1323, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
-            }
-            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
-            if (etat_action == ACTION_TERMINEE){
-                Score_ajout_panneau(1);
-                etat_tourne_panneaux = TP_TOURNE_PANNEAU_5;
-            }else if(etat_action == ACTION_ECHEC){
-                etat_tourne_panneaux = TP_DEGAGEMENT;
-            }
-            break;
-
-        case TP_TOURNE_PANNEAU_5:
-            if(couleur == COULEUR_BLEU){
-                Trajectoire_bezier(&trajectoire, 1323, 200, 1410, 310, 1547, 400, 1547, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
-            }else{
-                Trajectoire_bezier(&trajectoire, 3000-1323, 200, 3000-1410, 310, 3000-1547, 400, 3000-1547, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
-            }
-            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
-            if (etat_action == ACTION_TERMINEE){
-                Score_ajout_panneau(1);
-                etat_tourne_panneaux = TP_TOURNE_PANNEAU_6;
-            }else if(etat_action == ACTION_ECHEC){
-                etat_tourne_panneaux = TP_DEGAGEMENT;
-            }
-            break;
-        
-        case TP_TOURNE_PANNEAU_6:
-            if(couleur == COULEUR_BLEU){
-                Trajectoire_bezier(&trajectoire, 1547, 200, 1630, 310, 1771, 400, 1771, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
-            }else{
-                Trajectoire_bezier(&trajectoire, 3000-1547, 200, 3000-1630, 310, 3000-1771, 400, 3000-1771, 200, (150. *DEGRE_EN_RADIAN), (150. *DEGRE_EN_RADIAN));
-            }
-            etat_action =Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
-            if (etat_action == ACTION_TERMINEE){
-                Score_ajout_panneau(1);
-                etat_tourne_panneaux = TP_TOURNE_PANNEAU_6;
             }else if(etat_action == ACTION_ECHEC){
                 etat_tourne_panneaux = TP_DEGAGEMENT;
             }
             break;
 
         case TP_DEGAGEMENT:
-            if(Localisation_get().y_mm < 250){
+            if(Localisation_get().y_mm < 360){
                 etat_tourne_panneaux = TP_DEGAGEMENT_ACTIF;
             }else{
                 commande_vitesse_stop();
@@ -145,8 +77,21 @@ enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t
             break;
 
         case TP_DEGAGEMENT_ACTIF:
-                return Strategie_aller_a(Localisation_get().x_mm, 250, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
+                return Strategie_aller_a(Localisation_get().x_mm, 360, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
             break;
     }
     return ACTION_EN_COURS;
+}
+
+
+enum etat_action_t active_panneau_solaire(float pos_x, uint32_t step_ms){
+    struct trajectoire_t trajectoire;
+    enum etat_action_t etat_action;
+    Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
+         Localisation_get().x_mm, 400, pos_x, 400, pos_x, 140, 
+         Localisation_get().angle_radian, 
+         Geometrie_get_angle_optimal(Localisation_get().angle_radian, -30 * DEGRE_EN_RADIAN));
+
+    etat_action = Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE);
+    return etat_action;
 }

Strategie_2024_panneaux.h

@@ -1,3 +1,3 @@
 #include "Strategie.h"
 
-enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms);
+enum etat_action_t Strat_2024_tourner_panneaux(enum couleur_t couleur, uint32_t step_ms);

Strategie_2024_plante.c

@@ -93,12 +93,14 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
                     position_robot.angle_radian += ANGLE_PINCE + angle_rad;
                     position_plante = Geometrie_deplace(position_robot, distance_mm);
                     if( !est_dans_zone(position_plante, zone_plante)){
+                        etat_aller_a_plante = SAAP_INIT_DETECTION;
                         return ACTION_ECHEC;
                     }
                     robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante);
                     if(entree_dans_zone == true){
                         if(robot_dans_zone == false){
                             // Le robot est sorti de la zone
+                            etat_aller_a_plante = SAAP_INIT_DETECTION;
                             return ACTION_ECHEC;
                         }
                     }
@@ -121,14 +123,15 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
                     tempo_asserv--;
                     if(tempo_asserv <= 0){
                         commande_vitesse(0, 0, 0);
+                        etat_aller_a_plante = SAAP_INIT_DETECTION;
                         return ACTION_ECHEC;
                     }
                 }
 
                 // 3 on asservi
                 commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P;
-                if(commande_vitesse_plante > 300){
-                    commande_vitesse_plante = 300;
+                if(commande_vitesse_plante > 150){
+                    commande_vitesse_plante = 150;
                 }
                 if(commande_vitesse_plante <= 0){
                     commande_vitesse_stop();
@@ -146,6 +149,7 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
             }else{
                 tempo_ms--;
                 if(tempo_ms <= 0){
+                    etat_aller_a_plante = SAAP_INIT_DETECTION;
                     return ACTION_ECHEC;
                 }
             }
@@ -187,6 +191,7 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
             if (etat_action == ACTION_TERMINEE){
                 etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
             }else if (etat_action == ACTION_ECHEC){
+                i2c_annexe_ouvre_doigt_plante();
                 return ACTION_ECHEC;
             }
             break;

Strategie_2024_pots.c

@@ -22,11 +22,11 @@ float angle_bras[6] =
 float distance_bras_correction_mm[6] =
 {
     -12,
-    0,
+    -7,
     -5,
     -15,
     0,
-    0
+    -25
 };
 
 enum etat_bras_t{
@@ -47,10 +47,10 @@ struct position_t position_groupe_pot[6] =
     {
         {.x_mm = 36.1, .y_mm = 1386.8, .angle_radian = -90 * DEGRE_EN_RADIAN}, 
         {.x_mm = 36.1, .y_mm = 616.2, .angle_radian = -90 * DEGRE_EN_RADIAN}, 
-        {.x_mm = 1020, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, 
-        {.x_mm = 2000, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, 
+        {.x_mm = 1020, .y_mm = 36.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, // 1020 : bidouille
+        {.x_mm = 1980, .y_mm = 51.4, .angle_radian = 0 * DEGRE_EN_RADIAN}, // Attention bidouille !!!
         {.x_mm = 2963.9, .y_mm = 616.2, .angle_radian = 90 * DEGRE_EN_RADIAN}, 
-        {.x_mm = 2963.9, .y_mm = 1386.8, .angle_radian = 90 * DEGRE_EN_RADIAN}
+        {.x_mm = 2970, .y_mm = 1340.8, .angle_radian = 90 * DEGRE_EN_RADIAN} // bidouille : 1386.8 => 1326.8 , 2963.9 => 2970
     };
 
 
@@ -138,15 +138,12 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
             position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
             position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
             Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
-            /*etat_action = Strategie_tourner_et_aller_a(
-                position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[0],
-                SANS_EVITEMENT, step_ms);*/
-            etat_action = Strategie_tourner_et_aller_a(
-                position_approche_pot.x_mm, position_approche_pot.y_mm, (-30. *DEGRE_EN_RADIAN),
+
+            etat_action = Strategie_aller_a(
+                position_approche_pot.x_mm, position_approche_pot.y_mm,
                 EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
             if (etat_action == ACTION_TERMINEE){
                 etat_attrape_pot = AP_ORIENTE;
-                i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
             }
             break;
 

Strategie_2024_pots.h

@@ -31,7 +31,7 @@
 #define DISTANCE_APPROCHE_POT_MM 300. 
 #define DISTANCE_ATTRAPE_POT_MM 200.
 
-#define DISTANCE_ECHANGE_POT 50.
+#define DISTANCE_ECHANGE_POT 65.
 
 struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot);
 enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms);

Test_strategie_2024.c

@@ -42,6 +42,7 @@ int test_strategie_2024(){
     printf("G - Pseudo homologation.\n");
     printf("H - reglage pots.\n");
     printf("I - echange pots.\n");
+    printf("J - Calage fin.\n");
     
     int lettre;
     do{
@@ -92,6 +93,8 @@ int test_strategie_2024(){
         case 'I':
             while(test_echange_pot());
             break;
+
+            
             
 
         case 'q':
@@ -193,7 +196,7 @@ int test_calage_debut(){
                 }
             }
 
-            etat_action = Strategie_calage_debut(COULEUR_BLEU, _step_ms);
+            etat_action = Strategie_calage_bas(COULEUR_BLEU, _step_ms);
 
         }
         lettre = getchar_timeout_us(0);
@@ -768,7 +771,7 @@ int test_attrape_1_pot(void){
             }
             switch(etat_test){
                 case TAP_CALAGE:
-                    if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
+                    if(Strategie_calage_bas(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
                         etat_test=TAP_APPROCHE_POT;
                     }
                     break;

Tests_deplacement.c

@@ -345,8 +345,8 @@ int test_aller_retour(){
     switch(lettre){
         case 'b':
         case 'B':
-            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
-            Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+            Trajectoire_bezier(&trajectoire, 0, 0, 2500, 0, 250, 1300, 0, 0, 0, 0);
             printf("Trajectoire de Bézier\n");
             break;