diff --git a/Strategie.c b/Strategie.c index 05eefbc..29f5228 100644 --- a/Strategie.c +++ b/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){ diff --git a/Strategie_pousse_gateau.c b/Strategie_pousse_gateau.c new file mode 100644 index 0000000..d01d54e --- /dev/null +++ b/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: + } + +} \ No newline at end of file diff --git a/Strategie_pousse_gateau.h b/Strategie_pousse_gateau.h new file mode 100644 index 0000000..d2e7539 --- /dev/null +++ b/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); \ No newline at end of file