Debut du programme principal + detection obtacle pour chercher les plantes

CMakeLists.txt

@@ -34,6 +34,7 @@ Strategie.c
 Strategie_deplacement.c
 Strategie_2024_plante.c
 Strategie_2024_pots.c
+Strategie_2024.c
 Temps.c
 Test.c
 Test_gyro.c

Holonome2023.c

@@ -44,8 +44,8 @@ int main() {
     stdio_init_all();
     
     //Demonstration_init();Demonstration_auto();
-    while(mode_test());
+    //while(mode_test());
-    test_pseudo_homologation();
+    //test_pseudo_homologation();
     Holonome2023_init();
 
     multicore_launch_core1(Monitoring_display);
@@ -113,7 +113,7 @@ int main() {
                         printf("MATCH_ARRET_EN_COURS\n");
                         statu_match = MATCH_ARRET_EN_COURS;
                     }
-                    Strategie(couleur, _step_ms, timer_match_ms);
+                    Strategie_2024(couleur, _step_ms, timer_match_ms);
                     break;
                 
                 case MATCH_ARRET_EN_COURS:

Strategie.c

@@ -248,7 +248,7 @@ uint attente_tirette(void){
     return !gpio_get(TIRETTE);
 }
 
-/// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU
+/// @brief Renvoi COULEUR_JAUNE ou COULEUR_BLEU
 enum couleur_t lire_couleur(void){
     if (gpio_get(COULEUR))
         return COULEUR_JAUNE;

Strategie_2024.c

@@ -0,0 +1,174 @@
+#include "Strategie_2024.h"
+#include "pico/stdlib.h"
+#include "Balise_VL53L1X.h"
+#include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+#include "i2c_annexe.h"
+
+enum etat_groupe_pot_t{
+    GROUPE_POT_DISPO,
+    GROUPE_POT_ECHEC
+};
+
+enum etat_groupe_plante_t{
+    GROUPE_PLANTE_DISPO,
+    GROUPE_PLANTE_ECHEC
+};
+
+enum etat_groupe_pot_t etat_groupe_pot[6]={
+    GROUPE_POT_DISPO,
+    GROUPE_POT_DISPO,
+    GROUPE_POT_DISPO,
+    GROUPE_POT_DISPO,
+    GROUPE_POT_DISPO,
+    GROUPE_POT_DISPO
+};
+
+enum etat_groupe_plante_t etat_groupe_plante[6]={
+    GROUPE_PLANTE_DISPO,
+    GROUPE_PLANTE_DISPO,
+    GROUPE_PLANTE_DISPO,
+    GROUPE_PLANTE_DISPO,
+    GROUPE_PLANTE_DISPO,
+    GROUPE_PLANTE_DISPO
+};
+
+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_B2;
+}
+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;
+    int i;
+    if(couleur == COULEUR_BLEU){
+        ordre_groupe_plante = ordre_groupe_plante_bleu;
+    }else{
+        ordre_groupe_plante = ordre_groupe_plante_jaune;
+    }
+    for(i=0; i<6; i++){
+        if(etat_groupe_plante[ordre_groupe_plante[i]] == GROUPE_PLANTE_DISPO){
+            return ordre_groupe_plante[i];
+        }
+    }
+    return ZONE_PLANTE_AUCUNE;
+}
+
+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;
+    struct trajectoire_t trajectoire;
+    enum evitement_t evitement;
+    enum etat_action_t etat_action=ACTION_EN_COURS;
+    static int tempo_ms;
+
+    static enum {
+        TAP_CALAGE,
+        TAP_POT,
+        TAP_PLANTE_ORIENTATION,
+        TAP_PLANTE_ATTRAPE_1,
+        TAP_PLANTE_ATTRAPE_2,
+        TAP_ECHANGE_POT,
+        TAP_PLANTE_ATTRAPE_3,
+        TAP_PLANTE_ATTRAPE_4,
+        TAP_RENTRE,
+        TAP_DEPOSE
+    } etat_test = TAP_CALAGE;
+
+
+    switch(etat_test){
+        case TAP_CALAGE:
+            if(Strategie_calage_debut_manuel(couleur, _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_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;
+            }
+            break;
+
+        case TAP_PLANTE_ATTRAPE_1:
+            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_2;
+                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_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;
+            }
+            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){
+                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_RENTRE:
+            float angle_destination;
+            angle_destination = 15 * DEGRE_EN_RADIAN;
+            Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+            if(Strategie_tourner_et_aller_a(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
+                etat_test=TAP_DEPOSE;
+                i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
+                i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
+                tempo_ms=500;
+            }
+            break;
+        
+        case TAP_DEPOSE:
+            tempo_ms--;
+            commande_vitesse_stop();
+            if(tempo_ms<= 0){
+                i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE);
+                i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE);
+            }
+            break;
+
+    }
+}

Strategie_2024.h

@@ -0,0 +1,17 @@
+#include "pico/stdlib.h"
+#include "Balise_VL53L1X.h"
+#include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+#include "Strategie.h"
+#include "Strategie_2024_plante.h"
+#include "Strategie_2024_pots.h"
+#include "Commande_vitesse.h"
+
+#ifndef STRATEGIE_2024_H
+#define STRATEGIE_2024_H
+
+void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms);
+
+// STRATEGIE_H
+#endif

Strategie_2024_plante.c

@@ -60,7 +60,9 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
         SAAP_ASSERV
     } etat_aller_a_plante = SAAP_INIT_DETECTION;
     enum validite_vl53l8_t validite;
-    float angle_rad, distance_mm, commande_vitesse_plante;
+    float angle_rad, distance_mm, distance_obstacle, commande_vitesse_plante;
+    float distance_contrainte_obstacle, vitesse_max_contrainte_obstacle;
+    const float acceleration_mm_ss_obstacle=500;
     static float distance_min_mm;
     static int tempo_ms, tempo_asserv;
     static bool entree_dans_zone;
@@ -106,6 +108,10 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
                 }
 
                 // 2 on s'assure qu'il n'y a pas de robot en face (TODO)
+                distance_contrainte_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(ANGLE_PINCE);
+                if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
+                    vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
+                }
 
                 // 2 bis, on s'assure que le robot se rapproche de la plante. Si ce n'est pas le cas, on arrête
                 if(distance_mm < distance_min_mm){
@@ -130,6 +136,10 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
                     etat_aller_a_plante = SAAP_INIT_DETECTION;
                     return ACTION_TERMINEE;
                 }
+                // On limite la vitesse avec l'obstacle
+                if (commande_vitesse_plante > vitesse_max_contrainte_obstacle){
+                    commande_vitesse_plante = vitesse_max_contrainte_obstacle;
+                }
                 
                 commande_vitesse(cosf(ANGLE_PINCE + 0.04) * commande_vitesse_plante ,
                     sinf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , (angle_rad - 0.04) * ASSER_ANGLE_GAIN_PLANTE_P);

Strategie_2024_pots.c

@@ -18,19 +18,10 @@ float angle_bras[6] =
     -120 * DEGRE_EN_RADIAN
 };
 
-float angle_bras_correction[6] =
-{
-    0 * DEGRE_EN_RADIAN,
-    0 * DEGRE_EN_RADIAN,
-    0 * DEGRE_EN_RADIAN,
-    0,
-    0 * DEGRE_EN_RADIAN,
-    7 * DEGRE_EN_RADIAN
-};
 
 float distance_bras_correction_mm[6] =
 {
-    0,
+    -5,
     0,
     -10,
     -15,
@@ -125,7 +116,6 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
 
     static enum {
         AP_ALLER_VERS_GROUPE_POT,
-        AP_RECALE,
         AP_ORIENTE,
         AP_APPROCHE_POT,
         AP_ATTRAPE_POT,
@@ -153,28 +143,10 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
                 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),
-                EVITEMENT_SANS_EVITEMENT, step_ms);
+                EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
             if (etat_action == ACTION_TERMINEE){
-                etat_attrape_pot = AP_RECALE;
-                i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
-            }
-            break;
-
-        case AP_RECALE:
-            i2c_annexe_get_VL53L8(&validite, &angle, &distance);
-            if(validite == VL53L8_DISTANCE_LOIN){
-                if(fabs(distance + DISTANCE_CENTRE_CAPTEUR - Localisation_get().x_mm) < 25){
-                    i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
-                    commande_vitesse_stop();
-                    //if(couleur == COULEUR_BLEU){
-                    //    Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
-                    /*}else{
-                        Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
-                    }*/
                 etat_attrape_pot = AP_ORIENTE;
-                }else{
+                i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
-                    //printf("Erreur - recalage trop loin\n");
-                }
             }
             break;
 
@@ -193,7 +165,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
                 SANS_EVITEMENT, step_ms);*/
             etat_action = Strategie_aller_a_puis_tourner(
                 position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras],
-                EVITEMENT_SANS_EVITEMENT, step_ms);
+                EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
 
             if (etat_action == ACTION_TERMINEE){
                 position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras]));
@@ -222,7 +194,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
             }
             etat_action = Strategie_tourner_et_aller_a(
                 position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
-                EVITEMENT_SANS_EVITEMENT, step_ms);
+                EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
             if (etat_action == ACTION_TERMINEE){
                 etat_attrape_pot = AP_APPROCHE_POT;
                 pot = get_pot_suivant(pot);
@@ -245,7 +217,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
             }
             etat_action = Strategie_tourner_et_aller_a(
                 position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras],
-                EVITEMENT_SANS_EVITEMENT, step_ms);
+                EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms);
             if (etat_action == ACTION_TERMINEE){
                 etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT;
                 return ACTION_TERMINEE;
@@ -261,11 +233,14 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
 
 }
 
-
+/// @brief Echange les pots avant avec les pots arrière
+/// @param step_ms Le pas de temps
+/// @return ACTION_EN_COURS ou ACTION_TERMINEE
 enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms){
     static struct position_t position_initiale;
     struct position_t position_but, position_tmp;
     enum etat_action_t etat_action;
+    struct trajectoire_t trajectoire;
     
     static int tempo_ms;
 
@@ -294,9 +269,13 @@ enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms){
         break;
 
     case EPAA_TOURNE:
-        if(Strategie_tourner_a(position_initiale.angle_radian + M_PI, step_ms) == ACTION_TERMINEE){
+        Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
+        Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
+        Localisation_get().angle_radian, position_initiale.angle_radian - M_PI);
+
+        if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
             etat_echange_pot = EPAA_ATTRAPE_POT;
-            position_initiale.angle_radian += M_PI;
+            position_initiale.angle_radian -= M_PI;
             i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
             i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT);
             i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT);
@@ -397,6 +376,7 @@ enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){
         DP_BAISSE_BRAS,
         DP_BAISSE_BRAS_TEMPO,
         DP_RANGE_DOIGT,
+        DP_RANGE_BRAS_TEMPO,
     } etat_depose_pot = DP_BAISSE_BRAS;
     switch (etat_depose_pot)
     {
@@ -430,7 +410,22 @@ enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){
     case DP_RANGE_DOIGT:
         tempo_ms--;
         if(tempo_ms < 0){
-            etat_depose_pot = DP_BAISSE_BRAS;
+            for (int i=0; i< NB_BRAS; i++){
+                masque =1;
+                masque = masque << i;
+                if(masque_pot & masque){
+                    i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE);
+                    tempo_ms=250;
+                }
+            }
+            etat_depose_pot = DP_RANGE_BRAS_TEMPO;
+            
+        }
+        break;
+
+    case DP_RANGE_BRAS_TEMPO:
+        tempo_ms--;
+        if(tempo_ms < 0){
             return ACTION_TERMINEE;
         }
         break;

Test_strategie_2024.c

@@ -480,6 +480,9 @@ int test_pseudo_homologation(){
         TAP_PLANTE_ORIENTATION,
         TAP_PLANTE_ATTRAPE_1,
         TAP_PLANTE_ATTRAPE_2,
+        TAP_ECHANGE_POT,
+        TAP_PLANTE_ATTRAPE_3,
+        TAP_PLANTE_ATTRAPE_4,
         TAP_RENTRE,
         TAP_DEPOSE
     } etat_test = TAP_CALAGE;
@@ -557,6 +560,34 @@ int test_pseudo_homologation(){
                 
                 case TAP_PLANTE_ATTRAPE_2:
                     etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);
+                    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_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, ZONE_PLANTE_3);
+                    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;
+                    }
+                    break;
+
+                case TAP_PLANTE_ATTRAPE_4:
+                    etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
                     if( etat_action == ACTION_TERMINEE){
                         etat_test=TAP_RENTRE;
                         etat_action = ACTION_EN_COURS;