diff --git a/Strategie.c b/Strategie.c index 328818e..d9dd6b8 100644 --- a/Strategie.c +++ b/Strategie.c @@ -69,19 +69,68 @@ 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, - 265,2830, - +120. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN); + 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); if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = CALAGE_PANIER_1; + etat_depose = 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_strategie = RECULE_PANIER; + etat_depose = RECULE_PANIER; } break; @@ -92,23 +141,20 @@ void Homologation(uint32_t step_ms){ 120. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN); if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ - etat_strategie = LANCE_DANS_PANIER; + etat_depose = LANCE_DANS_PANIER; } break; case LANCE_DANS_PANIER: Asser_Position_maintien(); if(lance_balles(step_ms) == ACTION_TERMINEE){ - etat_strategie = STRATEGIE_FIN; + etat_depose = APPROCHE_ANGLE; + etat_action = ACTION_TERMINEE; } break; - - - case STRATEGIE_FIN: - Moteur_Stop(); - i2c_annexe_desactive_propulseur(); - break; } + return etat_action; + } @@ -212,6 +258,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); diff --git a/Strategie.h b/Strategie.h index f328a02..4284fdf 100644 --- a/Strategie.h +++ b/Strategie.h @@ -35,12 +35,18 @@ extern enum etat_strategie_t{ RECULE_PANIER, TOURNE_PANIER, LANCE_DANS_PANIER, + APPROCHE_CERISE_2, + ATTRAPPE_CERISE_2, + APPROCHE_PANIER_3, STRATEGIE_FIN, + }etat_strategie; enum etat_action_t cerise_accostage(void); enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction); enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms); +enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms); +enum etat_action_t depose_cerises(uint32_t step_ms); void Homologation(uint32_t step_ms); enum couleur_t lire_couleur(void); uint attente_tirette(void); diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index ea5ea69..becafc0 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -13,6 +13,7 @@ // Distance la plus éloignée du bord où le robot est capable d'aspirer les cerises en longeant la bodure. #define MAX_LONGE_MM 240 +#define MAX_ASPIRE_CERISE_MM 320 // Translation en mm/s pour aspirer les cerises #define TRANSLATION_CERISE 70 @@ -40,7 +41,8 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct TURBINE_DEMARRAGE, TURBINE_DEMARRAGE_TEMPO, ASPIRE_LONGE, - ASPIRE_LIBRE + ASPIRE_LIBRE, + ASPIRE_FIN } etat_attrape=ATTRAPE_INIT; switch(etat_attrape){ @@ -68,7 +70,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct case TURBINE_DEMARRAGE_TEMPO: if(tempo_ms < step_ms){ etat_attrape = ASPIRE_LONGE; - printf(">e:ASPIRE_LONGE|t\n"); }else{ tempo_ms -= step_ms; } @@ -83,11 +84,23 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) || ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){ etat_attrape = ASPIRE_LIBRE; - printf(">e:ASPIRE_LIBRE|t\n"); } break; case ASPIRE_LIBRE: + longer_direction_aspire = inverser_longe_direction(longer_direction); + if(longer_direction_aspire == LONGER_VERS_A){ + commande_translation_longer_vers_A(); + }else{ + commande_translation_longer_vers_C(); + } + if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_ASPIRE_CERISE_MM) )) || + ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_ASPIRE_CERISE_MM))) ){ + etat_attrape = ASPIRE_FIN; + } + break; + + case ASPIRE_FIN: commande_vitesse_stop(); i2c_annexe_desactive_turbine(); etat_action = ACTION_TERMINEE; diff --git a/Test_strategie.c b/Test_strategie.c index 4d0ba40..2e5341e 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -106,6 +106,7 @@ int test_strategie(){ } } + int test_homologation(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; printf("Homologation\n");