On attrape une plante et on la dépose dans le pot

Strategie_2024_plante.c

@@ -1,6 +1,7 @@
 #include "Commande_vitesse.h"
 #include "Strategie_2024_plante.h"
 #include "Geometrie_robot.h"
+#include "Localisation.h"
 #include "i2c_annexe.h"
 #include "math.h"
 #include <stdio.h>
@@ -8,12 +9,10 @@
 #define ASSER_ANGLE_GAIN_PLANTE_P 1.5
 #define ASSER_DISTANCE_GAIN_PLANTE_P 10
 
-enum etat_action_t Strat_2024_aller_a_plante(){
+enum etat_action_t Strat_2024_aller_a_plante(void){
     static enum {
         SAAP_INIT_DETECTION,
-        SAAP_ASSERV,
+        SAAP_ASSERV
-        SAAP_ATTRAPE,
-        SAAP_ATTRAPE_TEMPO,
     } etat_aller_a_plante = SAAP_INIT_DETECTION;
     enum validite_vl53l8_t validite;
     float angle, distance, commande_vitesse_plante;
@@ -35,8 +34,8 @@ enum etat_action_t Strat_2024_aller_a_plante(){
                 if(commande_vitesse_plante <= 0){
                     commande_vitesse_stop();
                     i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
-                    etat_aller_a_plante = SAAP_ATTRAPE;
+                    etat_aller_a_plante = SAAP_INIT_DETECTION;
-                    break;
+                    return ACTION_TERMINEE;
                 }
                 
                 commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
@@ -44,23 +43,64 @@ enum etat_action_t Strat_2024_aller_a_plante(){
             }else{
             }
             break;
-        
-        case SAAP_ATTRAPE:
-            i2c_annexe_attrape_plante(PLANTE_BRAS_1);
-            etat_aller_a_plante = SAAP_ATTRAPE_TEMPO;
-            tempo = 2500;
-            break;
-
-        case SAAP_ATTRAPE_TEMPO:
-            tempo--;
-            if(tempo <= 0){
-                etat_aller_a_plante = SAAP_INIT_DETECTION;
-                return ACTION_TERMINEE;
-            }
-            break;
-
-
     }
 
+    return ACTION_EN_COURS;
+}
+
+/// @brief S'approche d'une plante et la dépose dans un pot déjà attrapé
+/// @param step_ms 
+/// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6
+/// @return ACTION_EN_COURS ou ACTION_TERMINEE
+enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot){
+    static enum{
+        PDP_ALLER_PLANTE,
+        PDP_ATTRAPE_PLANTE,
+        PDP_RECULE,
+        PDP_TEMPO,
+    } etat_plante_dans_pot = PDP_ALLER_PLANTE;
+    static int tempo_ms;
+    struct position_t position_recule, position_initiale;
+
+    switch (etat_plante_dans_pot)
+    {
+        case PDP_ALLER_PLANTE:
+            if (Strat_2024_aller_a_plante() == ACTION_TERMINEE){
+                i2c_annexe_attrape_plante(bras_depose_pot);
+                tempo_ms = 2000;
+
+                etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
+            }
+            break;
+
+        case PDP_ATTRAPE_PLANTE:
+            tempo_ms--;
+            if(tempo_ms <= 0){
+                etat_plante_dans_pot=PDP_RECULE;
+                tempo_ms = 2000;
+            }
+            break;
+
+        case PDP_RECULE:
+            tempo_ms--;
+            position_initiale = Localisation_get();
+            position_initiale.angle_radian += ANGLE_PINCE;
+            position_recule = Geometrie_deplace(position_initiale, -100);
+            Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
+            if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){
+                commande_vitesse_stop();
+                etat_plante_dans_pot=PDP_TEMPO;
+            }
+            break;
+
+        case PDP_TEMPO:
+            tempo_ms--;
+            if(tempo_ms <= 0){
+                etat_plante_dans_pot=PDP_ALLER_PLANTE;
+                return ACTION_TERMINEE;
+            }            
+        default:
+            break;
+    }
     return ACTION_EN_COURS;
 }

Strategie_2024_plante.h

@@ -1,3 +1,4 @@
 #include "Strategie.h"
 
-enum etat_action_t Strat_2024_aller_a_plante();
+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);

Test_strategie_2024.c

@@ -23,6 +23,7 @@
 int test_calcul_position_pot(void);
 int test_calage_debut(void);
 int test_attrape_pot(void);
+int test_aller_a_plante(void);
 int test_attrape_plante(void);
 void affichage_test_strategie_2024(void);
 
@@ -30,7 +31,8 @@ int test_strategie_2024(){
     printf("A - Position groupes pot.\n");
     printf("B - Calage debut.\n");
     printf("C - Attrape pot.\n");
-    printf("D - Attrape plante.\n");
+    printf("D - Aller  a plante.\n");
+    printf("E - Attrape plante.\n");
     
     int lettre;
     do{
@@ -54,6 +56,11 @@ int test_strategie_2024(){
 
         case 'd':
         case 'D':
+            while(test_aller_a_plante());
+            break;
+
+        case 'e':
+        case 'E':
             while(test_attrape_plante());
             break;
 
@@ -174,7 +181,66 @@ int test_attrape_plante(){
     enum evitement_t evitement;
     enum etat_action_t etat_action=ACTION_EN_COURS;
 
-    printf("test_attrape_plante\n");
+    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);
+
+
+    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_plante_dans_pot(_step_ms, PLANTE_BRAS_1);
+
+
+        }
+        lettre = getchar_timeout_us(0);
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(etat_action == ACTION_EN_COURS);
+    Moteur_Stop();
+    
+    return 0;
+
+}
+
+int test_aller_a_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();