From 2e38c678fdfdaa2feaa6ae1ec84f4e0da4fae632 Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 27 Mar 2023 20:38:37 +0200 Subject: [PATCH] Homologation : Approche panier + correction bug "longe cerise" --- Strategie.c | 24 ++++++++++++++++- Strategie_prise_cerises.c | 57 +++++++++++++++++---------------------- 2 files changed, 48 insertions(+), 33 deletions(-) diff --git a/Strategie.c b/Strategie.c index d0b1468..81ba9f9 100644 --- a/Strategie.c +++ b/Strategie.c @@ -13,6 +13,9 @@ void Homologation(uint32_t step_ms){ APPROCHE_CERISE_1_A, APPROCHE_CERISE_1_B, ATTRAPE_CERISE_1, + APPROCHE_PANIER_1_A, + APPROCHE_PANIER_1_B, + CALAGE_PANIER_1, STRATEGIE_FIN }etat_strategie=STRATEGIE_INIT; @@ -44,10 +47,29 @@ void Homologation(uint32_t step_ms){ case ATTRAPE_CERISE_1: etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms); if(etat_action == ACTION_TERMINEE){ - etat_strategie = STRATEGIE_FIN; + etat_strategie = APPROCHE_PANIER_1_A; } break; + case APPROCHE_PANIER_1_A: + Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, + 485, Localisation_get().y_mm, + 465, 857, + 465,2830, + +30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN); + Trajet_debut_trajectoire(trajectoire); + etat_strategie = APPROCHE_PANIER_1_B; + break; + + case APPROCHE_PANIER_1_B: + etat_trajet = Trajet_avance(step_ms/1000.); + if(etat_trajet == TRAJET_TERMINE){ + etat_strategie = ATTRAPE_CERISE_1; + } + break; + + + case STRATEGIE_FIN: break; diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 5355086..b9d2d71 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -46,7 +46,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct switch(etat_attrape){ case ATTRAPE_INIT: etat_attrape=ATTRAPE_VERS_BORDURE; - printf("ATTRAPE_VERS_BORDURE\n"); break; case ATTRAPE_VERS_BORDURE: @@ -54,7 +53,6 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) || (longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){ etat_attrape = TURBINE_DEMARRAGE; - printf("TURBINE_DEMARRAGE\n"); } break; @@ -64,27 +62,28 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct commande_vitesse_stop(); tempo_ms = 2000; etat_attrape = TURBINE_DEMARRAGE_TEMPO; - printf("TURBINE_DEMARRAGE_TEMPO\n"); + break; case TURBINE_DEMARRAGE_TEMPO: if(tempo_ms < step_ms){ etat_attrape = ASPIRE_LONGE; - printf("ASPIRE_LONGE\n"); + printf(">e:ASPIRE_LONGE|t\n"); + }else{ + tempo_ms -= step_ms; } - tempo_ms -= step_ms; break; case ASPIRE_LONGE: longer_direction_aspire = inverser_longe_direction(longer_direction); - cerise_longer_bord(longer_direction_aspire); + cerise_longer_bord(LONGER_VERS_A); // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Z pour respecter cette condition 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("ASPIRE_LIBRE\n"); + printf(">e:ASPIRE_LIBRE|t\n"); } break; @@ -104,43 +103,37 @@ enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){ static enum { LONGE_INIT, - LONGE_VERS_A, - LONGE_VERS_C, + LONGE_NORMAL, LONGE_COLLE } etat_longer_bord=LONGE_INIT; + + switch (etat_longer_bord){ case LONGE_INIT: - if(longer_direction==LONGER_VERS_A){ - etat_longer_bord=LONGE_VERS_A; - }else{ - // longer_direction==LONGER_VERS_C - etat_longer_bord=LONGE_VERS_C; - } + etat_longer_bord=LONGE_NORMAL; break; - case LONGE_VERS_A: - commande_translation_longer_vers_A(); - if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || - (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){ - etat_longer_bord = LONGE_COLLE; - printf("Longer colle\n"); - } - break; - - case LONGE_VERS_C: - commande_translation_longer_vers_C(); - if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || - (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){ - etat_longer_bord = LONGE_COLLE; - printf("Longer colle\n"); + case LONGE_NORMAL: + if(longer_direction==LONGER_VERS_A){ + commande_translation_longer_vers_A(); + if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || + (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){ + etat_longer_bord = LONGE_COLLE; + } + }else{ + commande_translation_longer_vers_C(); + if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) || + (i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){ + etat_longer_bord = LONGE_COLLE; + } + } break; case LONGE_COLLE: if(cerise_accostage() == ACTION_TERMINEE){ - etat_longer_bord = LONGE_INIT; - printf("Longer INIT\n"); + etat_longer_bord = LONGE_NORMAL; } } return ACTION_EN_COURS;