Homologation : Approche panier + correction bug "longe cerise"

Strategie.c

@@ -13,6 +13,9 @@ void Homologation(uint32_t step_ms){
         APPROCHE_CERISE_1_A,
         APPROCHE_CERISE_1_B,
         ATTRAPE_CERISE_1,
+        APPROCHE_PANIER_1_A,
+        APPROCHE_PANIER_1_B,
+        CALAGE_PANIER_1,
         STRATEGIE_FIN
     }etat_strategie=STRATEGIE_INIT;
 
@@ -44,10 +47,29 @@ void Homologation(uint32_t step_ms){
         case ATTRAPE_CERISE_1:
             etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
             if(etat_action == ACTION_TERMINEE){
-                etat_strategie = STRATEGIE_FIN;
+                etat_strategie = APPROCHE_PANIER_1_A;
             }
             break;
 
+        case APPROCHE_PANIER_1_A:
+            Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                                485, Localisation_get().y_mm,
+                                465, 857,
+                                465,2830,
+                                +30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
+            Trajet_debut_trajectoire(trajectoire);
+            etat_strategie = APPROCHE_PANIER_1_B;
+            break;
+        
+        case APPROCHE_PANIER_1_B:
+            etat_trajet = Trajet_avance(step_ms/1000.);
+            if(etat_trajet == TRAJET_TERMINE){
+                etat_strategie = ATTRAPE_CERISE_1;
+            }
+            break;
+
+
+
         case STRATEGIE_FIN:
             break;
         

Strategie_prise_cerises.c

@@ -46,7 +46,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
     switch(etat_attrape){
         case ATTRAPE_INIT:
             etat_attrape=ATTRAPE_VERS_BORDURE;
-            printf("ATTRAPE_VERS_BORDURE\n");
             break;
         
         case ATTRAPE_VERS_BORDURE:
@@ -54,7 +53,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
             if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ||
                 (longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
                     etat_attrape = TURBINE_DEMARRAGE;
-                    printf("TURBINE_DEMARRAGE\n");
             }
             break;
         
@@ -64,27 +62,28 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
             commande_vitesse_stop();
             tempo_ms = 2000;
             etat_attrape = TURBINE_DEMARRAGE_TEMPO;
-            printf("TURBINE_DEMARRAGE_TEMPO\n");
+
             break;
 
         case TURBINE_DEMARRAGE_TEMPO:
             if(tempo_ms < step_ms){
                 etat_attrape = ASPIRE_LONGE;
-                printf("ASPIRE_LONGE\n");
+                printf(">e:ASPIRE_LONGE|t\n");
-            }
+            }else{
                 tempo_ms -= step_ms;
+            }
             break;
 
         case ASPIRE_LONGE:
             longer_direction_aspire = inverser_longe_direction(longer_direction);
-            cerise_longer_bord(longer_direction_aspire);
+            cerise_longer_bord(LONGER_VERS_A);
             // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support
             // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
             // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Z pour respecter cette condition
             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("ASPIRE_LIBRE\n");
+                printf(">e:ASPIRE_LIBRE|t\n");
             }
             break;
 
@@ -104,43 +103,37 @@ enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
 
     static enum {
         LONGE_INIT,
-        LONGE_VERS_A,
+        LONGE_NORMAL,
-        LONGE_VERS_C,
         LONGE_COLLE
     } etat_longer_bord=LONGE_INIT;
 
+    
+
     switch  (etat_longer_bord){
         case LONGE_INIT:
-            if(longer_direction==LONGER_VERS_A){
+            etat_longer_bord=LONGE_NORMAL;
-                etat_longer_bord=LONGE_VERS_A;
-            }else{
-                // longer_direction==LONGER_VERS_C
-                etat_longer_bord=LONGE_VERS_C;
-            }
             break;
 
-        case LONGE_VERS_A:
+        case LONGE_NORMAL:
+            if(longer_direction==LONGER_VERS_A){
                 commande_translation_longer_vers_A();
                 if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || 
                     (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
                         etat_longer_bord = LONGE_COLLE;
-                    printf("Longer colle\n");
                 }
-            break;
+            }else{
-        
-        case LONGE_VERS_C:
                 commande_translation_longer_vers_C();
                 if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || 
                     (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
                         etat_longer_bord = LONGE_COLLE;
-                    printf("Longer colle\n");
+                }
+                
             }
             break;
 
         case LONGE_COLLE:
             if(cerise_accostage() == ACTION_TERMINEE){
-                etat_longer_bord = LONGE_INIT;
+                etat_longer_bord = LONGE_NORMAL;
-                printf("Longer INIT\n");
             }
     }
     return ACTION_EN_COURS;