diff --git a/Strategie.c b/Strategie.c index 9fac9c6..f5be02c 100644 --- a/Strategie.c +++ b/Strategie.c @@ -53,7 +53,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ - const uint32_t temps_pre_fin_match = (97000 - 15000); + const uint32_t temps_pre_fin_match = (97000 - 10000); static bool pre_fin_match_active=false; float angle_fin; float recal_pos_x, recal_pos_y; @@ -96,9 +96,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ case STRATEGIE_INIT: 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_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; + struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; + struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; + struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; @@ -106,17 +106,16 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ objectifs[3]= objectif_4; }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_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; + struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE}; + struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; objectifs[2]= objectif_3; objectifs[3]= objectif_4; } - objectif_courant = &objectifs[0]; + objectif_courant = &objectifs[4]; Strategie_set_cerise_dans_robot(10); etat_strategie = LANCER_PANIER; break; @@ -125,18 +124,24 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ if(couleur == COULEUR_BLEU){ angle_fin = 30. * DEGRE_EN_RADIAN; - point_x = 857; + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); + Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + 650, Localisation_get().y_mm, + 490, 0, + 857, 0, + Localisation_get().angle_radian, angle_fin); }else{ angle_fin = -150. * DEGRE_EN_RADIAN; - point_x = 2000 - 857; + angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); + Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, + 2000 - 650, Localisation_get().y_mm, + 2000 - 490, 0, + 2000 - 857, 0, + Localisation_get().angle_radian, angle_fin); } - angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); + Trajet_config(500,250); - Trajet_config(250, 250); - Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156, - Localisation_get().angle_radian, angle_fin); - - if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ + if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == TRAJET_TERMINE){ etat_strategie = ATTRAPER_CERISE_BAS; } break; @@ -169,7 +174,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ } angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); - Trajet_config(250, 250); + Trajet_config(500, 250); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, Localisation_get().angle_radian, angle_fin); @@ -289,6 +294,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ case RETOUR_MAISON: i2c_annexe_plie_bras(); + i2c_annexe_desactive_turbine(); if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){ // Si le robot est dans la zone du panier, jeter les cerises s'il en a if(Strategie_get_cerise_dans_robot() > 0 && Robot_est_dans_zone_depose_panier(couleur)){ @@ -358,7 +364,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){ enum etat_action_t etat_action = ACTION_EN_COURS; struct trajectoire_t trajectoire; - Trajet_config(250, 250); + Trajet_config(TRAJECT_CONFIG_STD); // Definition des trajectoires if(couleur == COULEUR_BLEU){ @@ -378,6 +384,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN)); }else{ // Sinon, on a une courbe de bézier + Trajet_config(500,250); Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 485, Localisation_get().y_mm, 465, 857, @@ -401,6 +408,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN)); }else{ // Sinon, on a une courbe de bézier + Trajet_config(500,250); Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 2000-485, Localisation_get().y_mm, 2000-465, 857, @@ -411,7 +419,7 @@ enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step } // parcours du trajet - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == ACTION_TERMINEE){ etat_action = ACTION_TERMINEE; } return etat_action; diff --git a/Strategie_deplacement.c b/Strategie_deplacement.c index a3c5317..bfd9779 100644 --- a/Strategie_deplacement.c +++ b/Strategie_deplacement.c @@ -8,6 +8,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, enum etat_action_t etat_action = ACTION_EN_COURS; enum etat_trajet_t etat_trajet; float angle_avancement; + static bool trajet_inverse = false; static enum { PARCOURS_INIT, @@ -41,6 +42,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, return ACTION_ECHEC; case RETOUR_SI_OBSTABLE: + trajet_inverse = !trajet_inverse; Trajet_inverse(); break; @@ -53,7 +55,11 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, etat_trajet = Trajet_avance(step_ms/1000.); if(etat_trajet == TRAJET_TERMINE){ - etat_action = ACTION_TERMINEE; + if(trajet_inverse){ + etat_action = ACTION_ECHEC; + }else{ + etat_action = ACTION_TERMINEE; + } etat_parcourt = PARCOURS_INIT; } break; diff --git a/Strategie_pousse_gateau.c b/Strategie_pousse_gateau.c index f39fed1..66caa11 100644 --- a/Strategie_pousse_gateau.c +++ b/Strategie_pousse_gateau.c @@ -48,7 +48,7 @@ enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms } angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin); - Trajet_config(250, 250); + Trajet_config(500,250); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1700, Localisation_get().angle_radian, angle_fin); diff --git a/Trajet.h b/Trajet.h index b93d1ff..2887a3e 100644 --- a/Trajet.h +++ b/Trajet.h @@ -10,6 +10,8 @@ enum etat_trajet_t{ #define TRAJECT_CONFIG_AVANCE_DROIT 1000, 500 // Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²) #define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500 +// Vitesse et acceleration - standard (en mm et mm/s²) +#define TRAJECT_CONFIG_STD 500, 500 // Vitesse et acceleration pour une rotation (rad/s et rad/s²) #define TRAJECT_CONFIG_ROTATION_PURE 2, 2