diff --git a/Holonome2023.c b/Holonome2023.c index 72b931e..f2795ce 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -95,6 +95,8 @@ int main() { set_position_avec_gyroscope(1); if(get_position_avec_gyroscope()){ Gyro_Init(); + }else{ + sleep_ms(5000); } diff --git a/Strategie.c b/Strategie.c index c9b5110..e54e2ec 100644 --- a/Strategie.c +++ b/Strategie.c @@ -49,6 +49,7 @@ int Robot_est_dans_quart_haut_gauche(); int Robot_est_dans_quart_haut_droit(); int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur); int Robot_est_dans_zone_cerise_gauche(); +int Robot_est_dans_zone_cerise_droite(); enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms); @@ -369,15 +370,22 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step // On va en ligne droite if(Robot_est_dans_quart_haut_droit()){ Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 2000-465, 2830, - -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN); + 2000-180, 2800, + Localisation_get().angle_radian ,Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); + }else if (Robot_est_dans_zone_cerise_droite()){ + Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, + Localisation_get().x_mm + 330, Localisation_get().y_mm - 100, + 2000 - 745, 3000 - 475, + 2000 - 180, 3000 - 200, + Localisation_get().angle_radian, + Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); }else{ // Sinon, on a une courbe de bézier Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 2000-485, Localisation_get().y_mm, 2000-465, 857, 2000-465, 2830, - -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN); + Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); } } @@ -536,7 +544,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste point_x = 1735; point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80; } - angle_depart = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 120. * DEGRE_EN_RADIAN); + angle_depart = Localisation_get().angle_radian; angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, point_y,