diff --git a/CMakeLists.txt b/CMakeLists.txt index 55b31a2..8b5c4b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,7 @@ Servomoteur.c Strategie.c Strategie_deplacement.c Strategie_prise_cerises.c +Strategie_pousse_gateau.c Temps.c Test.c Test_strategie.c diff --git a/Strategie.c b/Strategie.c index 29f5228..0c6af46 100644 --- a/Strategie.c +++ b/Strategie.c @@ -97,7 +97,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ if(couleur == COULEUR_BLEU){ Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; - struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; + struct objectif_t objectif_2 = { .priorite = 1, .etat = FAIT, .cible = CERISE_HAUT}; struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE}; objectifs[0]= objectif_1; @@ -107,8 +107,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ }else{ Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; - struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; - struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE}; + struct objectif_t objectif_2 = { .priorite = 1, .etat = FAIT, .cible = CERISE_HAUT}; + struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE + , .cible = CERISE_DROITE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; @@ -206,10 +207,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ etat_strategie = ATTRAPER_CERISE_GAUCHE; } } - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = ATTRAPER_CERISE_GAUCHE; - } break; case ATTRAPER_CERISE_GAUCHE: @@ -231,10 +228,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ etat_strategie = ATTRAPER_CERISE_DROITE; } } - - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = ATTRAPER_CERISE_GAUCHE; - } break; case ATTRAPER_CERISE_DROITE: @@ -315,9 +308,6 @@ 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; - return Gateau_pousse_proche(couleur, step_ms); /*if(couleur == COULEUR_BLEU){ diff --git a/Strategie.h b/Strategie.h index 8b23344..2e3cf30 100644 --- a/Strategie.h +++ b/Strategie.h @@ -62,6 +62,7 @@ enum etat_action_t Strategie_preparation(); enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms); +enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms); extern float distance_obstacle; diff --git a/Strategie_pousse_gateau.c b/Strategie_pousse_gateau.c index d01d54e..861d8f0 100644 --- a/Strategie_pousse_gateau.c +++ b/Strategie_pousse_gateau.c @@ -1,5 +1,6 @@ #include "i2c_annexe.h" #include "Localisation.h" +#include "Monitoring.h" #include "Strategie.h" #include "Trajectoire.h" #include "Trajet.h" @@ -7,57 +8,75 @@ 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; - + ALLER_1, + ALLER_2, + ALLER_3 + } etat=ALLER_1; + float point_x, point_y; + float angle_fin; struct trajectoire_t trajectoire; - - float angle_fin, point_x, point_y; - - switch(etat_pousse_proche){ - case GATEAU_PROCHE_PRE_POUSSE: + + + switch(etat){ + case ALLER_1: if(couleur == COULEUR_BLEU){ - angle_fin = 90. * DEGRE_EN_RADIAN; - point_x = 310; + angle_fin = 180. * DEGRE_EN_RADIAN; + point_x = 225; }else{ - angle_fin = -135. * DEGRE_EN_RADIAN; - point_x = 1655; + angle_fin = 180. * DEGRE_EN_RADIAN; + point_x = 1775; } 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); + Trajet_config(100, 250); + Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 380, + 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(); + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat=ALLER_2; } break; - - case GATEAU_PROCHE_POUSSE: + + case ALLER_2: if(couleur == COULEUR_BLEU){ - angle_fin = 90. * DEGRE_EN_RADIAN; - point_x = 390; + angle_fin = 180. * DEGRE_EN_RADIAN; + point_x = 225; }else{ - angle_fin = -135. * DEGRE_EN_RADIAN; - point_x = 1585; + angle_fin = 180. * DEGRE_EN_RADIAN; + point_x = 1775; } 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); + Trajet_config(250, 250); + Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1650, + 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(); + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat=ALLER_3; + } + break; + + case ALLER_3: + if(couleur == COULEUR_BLEU){ + angle_fin = -150. * DEGRE_EN_RADIAN; + point_x = 225; + }else{ + angle_fin = 30. * DEGRE_EN_RADIAN; + point_x = 1775; + } + 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 - 1500, + Localisation_get().angle_radian, angle_fin); + + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat=ALLER_1; + return ACTION_TERMINEE; } break; - case GATEAU_PROCHE_RELACHE: } + return ACTION_EN_COURS; } \ No newline at end of file