diff --git a/Strategie.c b/Strategie.c index 55d9f24..f00f0a2 100644 --- a/Strategie.c +++ b/Strategie.c @@ -131,6 +131,108 @@ enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t couleur){ } +enum etat_action_t Strategie_Ninja_2(uint32_t step_ms, enum couleur_t couleur){ + static enum{ + NINJA_INIT, + NINJA_GRENIER1A, + NINJA_GRENIER1B, + NINJA_GRENIER1B_RECALE, + NINJA_GRENIER1C, + NINJA_GRENIER1D, + NINJA_GRENIER2, + NINJA_FIN + } etat_sss = NINJA_INIT; + static struct trajectoire_t trajectoire_composee; + static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3; + + switch(etat_sss){ + case NINJA_INIT: + if(couleur == COULEUR_JAUNE){ + Localisation_set(645, 1870, 0); + Trajectoire_bezier(&trajectoire1, + 645, 1870, + 935, 1870, + 1025, 1920, + 1315-50, 1920, 0., 0.); + + } + etat_sss = NINJA_GRENIER1A; + break; + + case NINJA_GRENIER1A: + Trajet_config(TRAJECT_CONFIG_STD); + if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ + etat_sss = NINJA_GRENIER1B; + if(couleur == COULEUR_JAUNE){ + Trajet_config(100,100); + Trajectoire_bezier(&trajectoire1, + Localisation_get().x_mm, Localisation_get().y_mm, + 1270-50, 1920, + 1230-50, 1900, + 1230-50, 1970, 0., 0.); + } + + } + break; + + case NINJA_GRENIER1B: + if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ + etat_sss = NINJA_GRENIER1B_RECALE; + } + break; + + case NINJA_GRENIER1B_RECALE: + if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){ + etat_sss = NINJA_GRENIER1C; + Trajet_config(100,100); + Localisation_set(1230-50, 1975, -M_PI/2); + if(couleur == COULEUR_JAUNE){ + Trajectoire_bezier(&trajectoire1, + 1230-50, 1970, + 1230, 1900, + 1255, 1725, + 1195, 1725, 0., 0.); + } + } + break; + + case NINJA_GRENIER1C: + if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ + etat_sss = NINJA_GRENIER1D; + if(couleur == COULEUR_JAUNE){ + Trajet_config(TRAJECT_CONFIG_STD); + Trajectoire_droite(&trajectoire1, + 1195, 1725, + 900, 1725, 0., 0.); + } + } + break; + + case NINJA_GRENIER1D: + if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ + etat_sss = NINJA_GRENIER2; + Trajectoire_droite(&trajectoire1, + 900, 1725, + 1650, 1775, 0., 0.); + } + break; + + case NINJA_GRENIER2: + if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ + etat_sss = NINJA_FIN; + } + break; + + + + case NINJA_FIN: + //Asser_Position_maintien(); + return ACTION_TERMINEE; + } + return ACTION_EN_COURS; + +} + enum etat_action_t Strategie_test(uint32_t step_ms){ static enum{ TEST_INIT, diff --git a/Strategie.h b/Strategie.h index e998d1e..02f622a 100644 --- a/Strategie.h +++ b/Strategie.h @@ -49,6 +49,7 @@ struct objectif_t{ }; enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t); +enum etat_action_t Strategie_Ninja_2(uint32_t step_ms, enum couleur_t couleur); enum etat_action_t Strategie_test(uint32_t step_ms); enum etat_action_t Strategie_1_metre(uint32_t step_ms); diff --git a/Strategie_deplacement.c b/Strategie_deplacement.c index 215375b..ff3e61c 100644 --- a/Strategie_deplacement.c +++ b/Strategie_deplacement.c @@ -89,7 +89,7 @@ enum etat_action_t Strategie_recale(int sens_avant, uint32_t step_ms){ AsserMoteur_setConsigne_mm_s(MOTEUR_A, -vitesse_recale); AsserMoteur_setConsigne_mm_s(MOTEUR_B, -vitesse_recale); } - timer_recale_ms = 2000; + timer_recale_ms = 800; etat_recale=RECALE_AVANCE; break; diff --git a/main.c b/main.c index 6f80919..3059ff1 100644 --- a/main.c +++ b/main.c @@ -160,7 +160,7 @@ void gestion_PAMI(uint32_t step_ms, int * asser_pos){ if(etat_action == ACTION_EN_COURS){ switch (get_identifiant()) { - case 0: etat_action = Strategie_Ninja(step_ms, get_couleur()); break; + case 0: etat_action = Strategie_Ninja_2(step_ms, get_couleur()); break; case 1: etat_action = Strategie_1_metre(step_ms); break; case 7: etat_action = Strategie_test(step_ms); break; default: etat_action = ACTION_TERMINEE; break; @@ -202,7 +202,7 @@ void gestion_VL53L8CX(void){ if(isReady){ VL53L8_lecture( &Dev, &Results); VL53L8_min_distance(Results, &distance_obstacle); - Trajet_set_obstacle_mm(distance_obstacle); + //Trajet_set_obstacle_mm(distance_obstacle); } affichage(); } @@ -226,7 +226,7 @@ void affichage(void){ printf(">pos_angle:%.1f\n", Localisation_get().angle_radian); printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm()); - printf(">abscisse:%f\n",abscisse); + printf(">abscisse:%lu:%f\n",temps_ms, abscisse); struct position_t position_actuelle; position_actuelle = Localisation_get(); @@ -234,7 +234,7 @@ void affichage(void){ printf(">pos_x:%lu:%.2f\n>pos_y:%lu:%.2f\n", temps_ms, position_actuelle.x_mm, temps_ms, position_actuelle.y_mm); printf(">con_x:%lu:%.2f\n>con_y:%lu:%.2f\n", temps_ms, point.point_xy.x, temps_ms, point.point_xy.y); - printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), get_identifiant(), get_tirette()); + printf(">couleur:%lu:%d\n>id:%lu:%d\n>Tirette:%lu:%d\n", temps_ms, get_couleur(),temps_ms, get_identifiant(),temps_ms, get_tirette()); } void tension_batterie_init(void){