Le robot part de la zone de départ, prend les pots, attrape une plante et la met dans un pot

Strategie_2024_plante.c

@@ -21,15 +21,14 @@ struct position_t liste_zone_plante[]=
 
 
 
-enum etat_action_t Strat_2024_aller_zone_plante(uint32_t step_ms){
+enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms){
-    int zone_plante = 2;
     struct position_t position_robot, position_zone_plante;
     float angle;
 
     position_zone_plante = liste_zone_plante[zone_plante];
     position_robot = Localisation_get(); 
-    angle = atan2f(position_zone_plante.x_mm - position_robot.x_mm, position_zone_plante.y_mm - position_robot.y_mm);
+    angle = atan2f(position_zone_plante.y_mm - position_robot.y_mm, position_zone_plante.x_mm - position_robot.x_mm);
-    return Strategie_tourner_a(angle, step_ms);
+    return Strategie_tourner_a(angle - ANGLE_PINCE, step_ms);
 }
 
 enum etat_action_t Strat_2024_aller_a_plante(void){
@@ -115,6 +114,11 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
             position_recule = Geometrie_deplace(position_initiale, -50);
             Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
             if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){
+                if(bras_depose_pot == PLANTE_BRAS_1){
+                    i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
+                }else{
+                    i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
+                }
                 commande_vitesse_stop();
                 i2c_annexe_ouvre_doigt_plante();
                 i2c_annexe_attrape_plante(bras_depose_pot);

Strategie_2024_plante.h

@@ -1,4 +1,14 @@
 #include "Strategie.h"
 
+enum zone_plante_t{
+    ZONE_PLANTE_1=0,
+    ZONE_PLANTE_2=1,
+    ZONE_PLANTE_3=2,
+    ZONE_PLANTE_4=3,
+    ZONE_PLANTE_5=4,
+    ZONE_PLANTE_6=5,
+};
+
+enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms);
 enum etat_action_t Strat_2024_aller_a_plante();
 enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot);

Strategie_2024_pots.c

@@ -7,7 +7,7 @@
 #include "Localisation.h"
 
 #define DISTANCE_APPROCHE_POT_MM 300. 
-#define DISTANCE_ATTRAPE_POT_MM 180.
+#define DISTANCE_ATTRAPE_POT_MM 185.
 
 float angle_bras[6] =
 {

Test_strategie_2024.c

@@ -25,6 +25,8 @@ int test_calage_debut(void);
 int test_attrape_pot(void);
 int test_aller_a_plante(void);
 int test_attrape_plante(void);
+int test_aller_zone_plante();
+int test_pseudo_homologation(void);
 void affichage_test_strategie_2024(void);
 
 int test_strategie_2024(){
@@ -33,6 +35,8 @@ int test_strategie_2024(){
     printf("C - Attrape pot.\n");
     printf("D - Aller  a plante.\n");
     printf("E - Attrape plante.\n");
+    printf("F - Aller zone plante.\n");
+    printf("G - Pseudo homologation.\n");
     
     int lettre;
     do{
@@ -64,6 +68,16 @@ int test_strategie_2024(){
             while(test_attrape_plante());
             break;
 
+        case 'f':
+        case 'F':
+            while(test_aller_zone_plante());
+            break;
+
+        case 'g':
+        case 'G':
+            while(test_pseudo_homologation());
+            break;
+
         case 'q':
         case 'Q':
             return 0;
@@ -293,6 +307,70 @@ int test_aller_a_plante(){
 
 }
 
+
+
+int test_aller_zone_plante(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+    struct trajectoire_t trajectoire;
+    enum evitement_t evitement;
+    enum etat_action_t etat_action=ACTION_EN_COURS;
+
+    printf("test_aller_a_plante\n");
+
+    i2c_maitre_init();
+    Trajet_init();
+    Balise_VL53L1X_init();
+
+    
+    set_position_avec_gyroscope(0);
+    if(get_position_avec_gyroscope()){
+        printf("Init gyroscope\n");
+        Gyro_Init();
+    }
+
+    stdio_flush();
+
+    multicore_launch_core1(affichage_test_strategie_2024);
+
+    Localisation_set(800, 200, 0);
+
+
+    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);
+            Evitement_gestion(_step_ms);
+
+            // Routine à 2 ms
+            if(temps_ms % _step_ms_gyro == 0){
+                if(get_position_avec_gyroscope()){
+                    Gyro_Read(_step_ms_gyro);
+                }
+            }
+
+            etat_action = Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms);
+
+
+        }
+        lettre = getchar_timeout_us(0);
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(etat_action == ACTION_EN_COURS);
+    Moteur_Stop();
+    
+    return 0;
+
+}
+
+
 int test_attrape_pot(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
     struct trajectoire_t trajectoire;
@@ -373,6 +451,102 @@ int test_attrape_pot(){
 
 }
 
+int test_pseudo_homologation(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+    struct trajectoire_t trajectoire;
+    enum evitement_t evitement;
+    enum etat_action_t etat_action=ACTION_EN_COURS;
+
+    enum {
+        TAP_CALAGE,
+        TAP_POT,
+        TAP_PLANTE_ORIENTATION,
+        TAP_PLANTE_ATTRAPE
+    } etat_test = TAP_CALAGE;
+
+    printf("test_attrape_pot\n");
+
+
+    i2c_maitre_init();
+    Trajet_init();
+    Balise_VL53L1X_init();
+    Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);
+
+    
+    set_position_avec_gyroscope(1);
+    if(get_position_avec_gyroscope()){
+        printf("Init gyroscope\n");
+        Gyro_Init();
+    }
+
+    stdio_flush();
+    Trajet_config(TRAJECT_CONFIG_STD);
+
+    multicore_launch_core1(affichage_test_strategie_2024);
+
+
+    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);
+            Evitement_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){
+                case TAP_CALAGE:
+                    if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
+                        etat_test=TAP_POT;
+                    }
+                    break;
+                case TAP_POT:
+                    if(Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms) == ACTION_TERMINEE){
+                        etat_test=TAP_PLANTE_ORIENTATION;
+                    }
+                    break;
+
+                case TAP_PLANTE_ORIENTATION:
+                    if(Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms) == ACTION_TERMINEE){
+                        etat_test=TAP_PLANTE_ATTRAPE;
+                    }
+                    break;
+
+                case TAP_PLANTE_ATTRAPE:
+                    if(Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1) == ACTION_TERMINEE){
+                        etat_action= ACTION_TERMINEE;
+                    }
+                    break;
+            }
+
+        }
+        lettre = getchar_timeout_us(0);
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(etat_action == ACTION_EN_COURS);
+    printf("STRATEGIE_LOOP_2\n");
+    printf("Lettre : %d; %c\n", lettre, lettre);
+
+    while(1){Moteur_Stop();}
+    
+    if(lettre == 'q' && lettre == 'Q'){
+        return 0;
+    }
+    return 0;
+
+}
+
 
 void affichage_test_strategie_2024(){
     uint32_t temps;

i2c_annexe.c

@@ -173,7 +173,7 @@ void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){
 }
 
 /// @brief Envoie la consigne de position du servomoteur à la carte des servomoteurs
-/// @param actionneur de 1 à 6, pour le "bras" correspondant".
+/// @param actionneur de 0 à 5, pour le "bras" correspondant".
 /// @param pos_bras Code de position du bras, voir les #define BRAS_PLIE, ... définis plus haut ou dans le .h
 /// @param pos_doigt Code de position du doigt, voir les #define DOIGT_LACHE, ... définis plus haut ou dans le .h
 void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt){