diff --git a/Strategie.c b/Strategie.c index d9dd6b8..328818e 100644 --- a/Strategie.c +++ b/Strategie.c @@ -69,68 +69,19 @@ void Homologation(uint32_t step_ms){ break; case APPROCHE_PANIER_2: - if( depose_cerises(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, - 857, 3000 - 156, - +270. * DEGREE_EN_RADIAN, 300. * DEGREE_EN_RADIAN); - if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = ATTRAPPE_CERISE_2; - } - break; - - case ATTRAPPE_CERISE_2: - etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms); - if(etat_action == ACTION_TERMINEE){ - etat_strategie = APPROCHE_PANIER_3; - } - break; - - case APPROCHE_PANIER_3: - if( depose_cerises(step_ms) == ACTION_TERMINEE){ - etat_strategie = STRATEGIE_FIN; - } - break; - - case STRATEGIE_FIN: - Moteur_Stop(); - i2c_annexe_desactive_propulseur(); - break; - } -} - -enum etat_action_t depose_cerises(uint32_t step_ms){ - static enum { - APPROCHE_ANGLE, - CALAGE_PANIER_1, - RECULE_PANIER, - LANCE_DANS_PANIER, - }etat_depose = APPROCHE_ANGLE; - enum etat_action_t etat_action = ACTION_EN_COURS; - - struct trajectoire_t trajectoire; - - switch(etat_depose){ - - case APPROCHE_ANGLE: - Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, - 265, 2830, - Localisation_get().angle_radian, +120. * DEGREE_EN_RADIAN); + 265,2830, + +120. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN); if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_depose = CALAGE_PANIER_1; + etat_strategie = CALAGE_PANIER_1; } break; case CALAGE_PANIER_1: if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){ - etat_depose = RECULE_PANIER; + etat_strategie = RECULE_PANIER; } break; @@ -141,20 +92,23 @@ enum etat_action_t depose_cerises(uint32_t step_ms){ 120. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN); if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_depose = LANCE_DANS_PANIER; + etat_strategie = LANCE_DANS_PANIER; } break; case LANCE_DANS_PANIER: Asser_Position_maintien(); if(lance_balles(step_ms) == ACTION_TERMINEE){ - etat_depose = APPROCHE_ANGLE; - etat_action = ACTION_TERMINEE; + etat_strategie = STRATEGIE_FIN; } break; - } - return etat_action; + + case STRATEGIE_FIN: + Moteur_Stop(); + i2c_annexe_desactive_propulseur(); + break; + } } @@ -258,32 +212,6 @@ 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);