Pousse gateau - non testé

Strategie.c

@@ -8,6 +8,7 @@
 #include "Moteurs.h"
 #include "Score.h"
 #include "Strategie_prise_cerises.h"
+#include "Strategie_pousse_gateau.h"
 #include "Strategie.h"
 #include "Trajet.h"
 #include "math.h"
@@ -316,7 +317,10 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
 enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){
     struct trajectoire_t trajectoire;
     float angle_fin;
-    if(couleur == COULEUR_BLEU){
+
+    return Gateau_pousse_proche(couleur, step_ms);
+
+    /*if(couleur == COULEUR_BLEU){
         angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
         Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
                             740, 3000 - 550,
@@ -332,7 +336,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t coul
                             Localisation_get().angle_radian, angle_fin);
 
     }
-    return parcourt_trajet_simple(trajectoire, step_ms);
+    return parcourt_trajet_simple(trajectoire, step_ms);*/
 }
 
 enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms){

Strategie_pousse_gateau.c

@@ -0,0 +1,63 @@
+#include "i2c_annexe.h"
+#include "Localisation.h"
+#include "Strategie.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+#include "Geometrie.h"
+
+enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){
+    static enum {
+        GATEAU_PROCHE_PRE_POUSSE,
+        GATEAU_PROCHE_POUSSE,
+        GATEAU_PROCHE_RELACHE,
+    } etat_pousse_proche;
+
+    struct trajectoire_t trajectoire;
+
+    float angle_fin, point_x, point_y;
+
+    switch(etat_pousse_proche){
+        case GATEAU_PROCHE_PRE_POUSSE:
+            if(couleur == COULEUR_BLEU){
+                angle_fin = 90. * DEGRE_EN_RADIAN;
+                point_x = 310;
+            }else{
+                angle_fin = -135. * DEGRE_EN_RADIAN;
+                point_x = 1655;
+            }
+            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
+
+            Trajet_config(250, 250);            
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 360,
+                     Localisation_get().angle_radian, angle_fin);
+
+            if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
+                etat_pousse_proche = GATEAU_PROCHE_POUSSE;
+                i2c_annexe_deplie_bras();
+            }
+            break;
+
+        case GATEAU_PROCHE_POUSSE:
+            if(couleur == COULEUR_BLEU){
+                angle_fin = 90. * DEGRE_EN_RADIAN;
+                point_x = 390;
+            }else{
+                angle_fin = -135. * DEGRE_EN_RADIAN;
+                point_x = 1585;
+            }
+            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
+
+            Trajet_config(250, 250);            
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1665,
+                     Localisation_get().angle_radian, angle_fin);
+
+            if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
+                etat_pousse_proche = GATEAU_PROCHE_POUSSE;
+                i2c_annexe_plie_bras();
+            }
+            break;
+
+        case GATEAU_PROCHE_RELACHE:
+    }
+
+}

Strategie_pousse_gateau.h

@@ -0,0 +1,3 @@
+#include "pico/stdlib.h"
+
+enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms);