Travail en cours

Strategie.c

@@ -69,19 +69,68 @@ void Homologation(uint32_t step_ms){
             break;
 
         case APPROCHE_PANIER_2:
+            if( depose_cerises(step_ms) == ACTION_TERMINEE){
+                etat_strategie = APPROCHE_CERISE_2;
+            }
+            break;
+
+        case APPROCHE_CERISE_2:
+            Trajet_config(250, 500);
             Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
-                                265,2830,
+                                857, 3000 - 156,
-                                +120. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
+                                +270. * DEGREE_EN_RADIAN, 300. * DEGREE_EN_RADIAN);
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat_strategie = ATTRAPPE_CERISE_2;
+            }
+            break;
+        
+        case ATTRAPPE_CERISE_2:
+            etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms);
+            if(etat_action == ACTION_TERMINEE){
+                etat_strategie = APPROCHE_PANIER_3;
+            }
+            break;
+
+        case APPROCHE_PANIER_3:
+            if( depose_cerises(step_ms) == ACTION_TERMINEE){
+                etat_strategie = STRATEGIE_FIN;
+            }
+            break;
+
+        case STRATEGIE_FIN:
+            Moteur_Stop();
+            i2c_annexe_desactive_propulseur();
+            break;
+    }
+}
+
+enum etat_action_t depose_cerises(uint32_t step_ms){
+    static enum {
+        APPROCHE_ANGLE,
+        CALAGE_PANIER_1,
+        RECULE_PANIER,
+        LANCE_DANS_PANIER,
+    }etat_depose = APPROCHE_ANGLE;
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+
+    struct trajectoire_t trajectoire;
+
+    switch(etat_depose){
+
+        case APPROCHE_ANGLE:
+            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                                265, 2830,
+                                Localisation_get().angle_radian, +120. * DEGREE_EN_RADIAN);
 
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_strategie = CALAGE_PANIER_1;
+                etat_depose = CALAGE_PANIER_1;
             }
 
             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){
-                etat_strategie = RECULE_PANIER;
+                etat_depose = RECULE_PANIER;
             }
             break;
         
@@ -92,23 +141,20 @@ void Homologation(uint32_t step_ms){
                                 120. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN);
 
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_strategie = LANCE_DANS_PANIER;
+                etat_depose = LANCE_DANS_PANIER;
             }
             break;
 
         case LANCE_DANS_PANIER:
             Asser_Position_maintien();
             if(lance_balles(step_ms) == ACTION_TERMINEE){
-                etat_strategie = STRATEGIE_FIN;
+                etat_depose = APPROCHE_ANGLE;
+                etat_action = ACTION_TERMINEE;
             }
             break;
-
-
-        case STRATEGIE_FIN:
-            Moteur_Stop();
-            i2c_annexe_desactive_propulseur();
-            break;
     }
+    return etat_action;
+
 }
 
 
@@ -212,6 +258,32 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
     return etat_action;
 }
 
+enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    enum etat_trajet_t etat_trajet;
+    static enum {
+        PARCOURS_INIT,
+        PARCOURS_AVANCE,
+    } etat_parcourt=PARCOURS_INIT;
+
+    switch (etat_parcourt){
+        case PARCOURS_INIT:
+            Trajet_debut_trajectoire(trajectoire);
+            etat_parcourt = PARCOURS_AVANCE;
+            break;
+        
+        case PARCOURS_AVANCE:
+            etat_trajet = Trajet_avance(step_ms/1000.);
+            if(etat_trajet == TRAJET_TERMINE){
+                etat_action = ACTION_TERMINEE;
+                etat_parcourt = PARCOURS_INIT;
+            }
+            break;
+    }
+
+    return etat_action;
+}
+
 /// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
 uint attente_tirette(void){
     return !gpio_get(TIRETTE);

Strategie.h

@@ -35,12 +35,18 @@ extern enum etat_strategie_t{
     RECULE_PANIER,
     TOURNE_PANIER,
     LANCE_DANS_PANIER,
+    APPROCHE_CERISE_2,
+    ATTRAPPE_CERISE_2,
+    APPROCHE_PANIER_3,
     STRATEGIE_FIN,
+    
 }etat_strategie;
 
 enum etat_action_t cerise_accostage(void);
 enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
 enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
+enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms);
+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);

Strategie_prise_cerises.c

@@ -13,6 +13,7 @@
 
 // Distance la plus éloignée du bord où le robot est capable d'aspirer les cerises en longeant la bodure.
 #define MAX_LONGE_MM 240
+#define MAX_ASPIRE_CERISE_MM 320
 
 // Translation en mm/s pour aspirer les cerises
 #define TRANSLATION_CERISE 70
@@ -40,7 +41,8 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
         TURBINE_DEMARRAGE,
         TURBINE_DEMARRAGE_TEMPO,
         ASPIRE_LONGE,
-        ASPIRE_LIBRE
+        ASPIRE_LIBRE,
+        ASPIRE_FIN
     } etat_attrape=ATTRAPE_INIT;
     
     switch(etat_attrape){
@@ -68,7 +70,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
         case TURBINE_DEMARRAGE_TEMPO:
             if(tempo_ms < step_ms){
                 etat_attrape = ASPIRE_LONGE;
-                printf(">e:ASPIRE_LONGE|t\n");
             }else{
                 tempo_ms -= step_ms;
             }
@@ -83,11 +84,23 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
             if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) ||
                 ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){
                 etat_attrape = ASPIRE_LIBRE;
-                printf(">e:ASPIRE_LIBRE|t\n");
             }
             break;
 
         case ASPIRE_LIBRE:
+            longer_direction_aspire = inverser_longe_direction(longer_direction);
+            if(longer_direction_aspire == LONGER_VERS_A){
+                commande_translation_longer_vers_A();
+            }else{
+                commande_translation_longer_vers_C();
+            }
+            if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_ASPIRE_CERISE_MM) )) ||
+                ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_ASPIRE_CERISE_MM))) ){
+                etat_attrape = ASPIRE_FIN;
+            }
+            break;
+            
+        case ASPIRE_FIN:
             commande_vitesse_stop();
             i2c_annexe_desactive_turbine();
             etat_action = ACTION_TERMINEE;

Test_strategie.c

@@ -106,6 +106,7 @@ int test_strategie(){
     }
 }
 
+
 int test_homologation(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
     printf("Homologation\n");