diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index 6bb44a8..d2f14d5 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -92,7 +92,7 @@ int index_Maitre = 0; bool Mvt_tolerance_OK =false; bool Balises_OK = 0; int tolerance_position =100; -float tolerance_orientation =0.05; // 3° +float tolerance_orientation =0.03; // 2° char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"}; @@ -361,7 +361,9 @@ void gestion_match(){ enum etat_action_t Strategie(void){ static enum { STRAT_RECULE_BANDEROLE, // Deplacement relatif - STRAT_ALLER_GRADINS_1, // Déplacement absolu + STRAT_ALLER_GRADINS_1_A, // Déplacement absolu + STRAT_ALLER_GRADINS_1_B, // Déplacement relatif + STRAT_ALLER_GRADINS_1_C, // Déplacement relatif STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu STRAT_ALLER_BACKSTAGE // Déplacement relatif @@ -378,18 +380,45 @@ enum etat_action_t Strategie(void){ rotation_rad = 0; etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 0); if(etat_action == ACTION_TERMINEE){ - etat_strategie = STRAT_ALLER_GRADINS_1; + etat_strategie = STRAT_ALLER_GRADINS_1_A; } break; - case STRAT_ALLER_GRADINS_1: + case STRAT_ALLER_GRADINS_1_A: etat_action = deplacement_absolu(800, 800, -M_PI/2., 0); + if(etat_action == ACTION_TERMINEE){ + etat_strategie = STRAT_ALLER_GRADINS_1_B; + } + break; + + case STRAT_ALLER_GRADINS_1_B: + etat_action = deplacement_relatif(300, 0, 0, 0); + if(etat_action == ACTION_TERMINEE){ + etat_strategie = STRAT_ALLER_GRADINS_1_C; + } + break; + + case STRAT_ALLER_GRADINS_1_C: + etat_action = deplacement_relatif(-250, 0, 0, 0); + if(etat_action == ACTION_TERMINEE){ + etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE; + } + break; + + case STRAT_ALLER_PREPA_BACKSTAGE: + etat_action = deplacement_absolu(550, 1150, M_PI/2., 0); + if(etat_action == ACTION_TERMINEE){ + etat_strategie = STRAT_ALLER_BACKSTAGE; + } + break; + + case STRAT_ALLER_BACKSTAGE: + etat_action = deplacement_relatif(500, 0, 0, 0); if(etat_action == ACTION_TERMINEE){ etat_strategie = STRAT_RECULE_BANDEROLE; return ACTION_TERMINEE; } - - + break; } return ACTION_EN_COURS; @@ -438,14 +467,15 @@ void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_ori } } -enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int consigne_orientation_rad, int evitement){ +enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, float consigne_orientation_rad, int evitement){ static enum{ DA_INIT, DA_COMPARE_POSITIONS, DA_MVT_EN_COURS, DA_ATTENTE, } etat_deplacement = DA_INIT; - static int mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad; + static int mem_consigne_x_mm, mem_consigne_y_mm; + static float mem_consigne_orientation_rad; static struct chassis_emission_t chassis_emission; struct triangulation_reception_t triangulation_reception; enum etat_action_t etat_deplacement_relatif;