diff --git a/Strategie.c b/Strategie.c index 328818e..ee14712 100644 --- a/Strategie.c +++ b/Strategie.c @@ -2,6 +2,7 @@ #include "i2c_annexe.h" #include "Asser_Position.h" #include "Balise_VL53L1X.h" +#include "Commande_vitesse.h" #include "Geometrie_robot.h" #include "Localisation.h" #include "Moteurs.h" @@ -42,8 +43,8 @@ void Homologation(uint32_t step_ms){ case APPROCHE_CERISE_1_A: Trajet_config(250, 500); - Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN); - if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){ + Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, 30. * DEGREE_EN_RADIAN); + if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){ etat_strategie = ATTRAPE_CERISE_1; } break; @@ -91,7 +92,7 @@ void Homologation(uint32_t step_ms){ 180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80, 120. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN); - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_strategie = LANCE_DANS_PANIER; } break; @@ -99,14 +100,32 @@ void Homologation(uint32_t step_ms){ case LANCE_DANS_PANIER: Asser_Position_maintien(); if(lance_balles(step_ms) == ACTION_TERMINEE){ + etat_strategie = APPROCHE_CERISE_2; + } + break; + + case APPROCHE_CERISE_2: + Trajet_config(250, 500); + Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, + 830, 3000 - 156, + Localisation_get().angle_radian, 30. * DEGREE_EN_RADIAN); + + if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_strategie = STRATEGIE_FIN; } break; + case ATTRAPPE_CERISE_2: + etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms); + if(etat_action == ACTION_TERMINEE){ + etat_strategie = APPROCHE_PANIER_2; + } + break; + case STRATEGIE_FIN: - Moteur_Stop(); i2c_annexe_desactive_propulseur(); + commande_vitesse_stop(); break; } } @@ -117,7 +136,7 @@ void Homologation(uint32_t step_ms){ /// @return ACTION_EN_COURS ou ACTION_TERMINEE enum etat_action_t lance_balles(uint32_t step_ms){ enum etat_action_t etat_action = ACTION_EN_COURS; - uint32_t tempo_ms; + static uint32_t tempo_ms; static enum{ LANCE_PROPULSEUR_ON, @@ -128,14 +147,13 @@ enum etat_action_t lance_balles(uint32_t step_ms){ switch(etat_lance_balle){ case LANCE_PROPULSEUR_ON: i2c_annexe_active_propulseur(); - tempo_ms = 2000; + tempo_ms = 1000; etat_lance_balle = LANCE_TEMPO_PROP_ON; break; case LANCE_TEMPO_PROP_ON: if (tempo_ms < step_ms){ - //etat_lance_balle = LANCE_PORTE_OUVERTE; - etat_lance_balle = LANCE_TEMPO_PROP_ON; + etat_lance_balle = LANCE_PORTE_OUVERTE; i2c_annexe_ouvre_porte(); tempo_ms = 6000; }else{ @@ -152,8 +170,6 @@ enum etat_action_t lance_balles(uint32_t step_ms){ tempo_ms -= step_ms; } break; - - } return etat_action; } @@ -212,6 +228,32 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint return etat_action; } +enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){ + enum etat_action_t etat_action = ACTION_EN_COURS; + enum etat_trajet_t etat_trajet; + static enum { + PARCOURS_INIT, + PARCOURS_AVANCE, + } etat_parcourt=PARCOURS_INIT; + + switch (etat_parcourt){ + case PARCOURS_INIT: + Trajet_debut_trajectoire(trajectoire); + etat_parcourt = PARCOURS_AVANCE; + break; + + case PARCOURS_AVANCE: + etat_trajet = Trajet_avance(step_ms/1000.); + if(etat_trajet == TRAJET_TERMINE){ + etat_action = ACTION_TERMINEE; + etat_parcourt = PARCOURS_INIT; + } + break; + } + + return etat_action; +} + /// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette uint attente_tirette(void){ return !gpio_get(TIRETTE);