#include "Strategie.h" #include "Asser_Position.h" #include "Geometrie.h" #include "Servomoteur.h" enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t couleur){ static enum{ NINJA_INIT, NINJA_GRENIER1, NINJA_GRENIER2A, NINJA_GRENIER2A_CALAGE, NINJA_GRENIER2B, NINJA_GRENIER2C, NINJA_GRENIER2C_CALAGE, NINJA_GRENIER2D, 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, 800, 1870, 890, 1730, 990, 1730, 0., 0.); Trajectoire_droite(&trajectoire2, 990, 1730, 1150, 1730, 0, 0); }else{ Localisation_set(3000 - 45, 1870, M_PI); Trajectoire_bezier(&trajectoire1, 3000 - 45, 1870, 3000 - 800, 1870, 3000 - 890, 1730, 3000 - 990, 1730, 0., 0.); Trajectoire_droite(&trajectoire2, 3000 - 990, 1730, 3000 - 1150, 1730, 0, 0); } Trajectoire_composee_init(&trajectoire_composee); Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1); Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2); etat_sss = NINJA_GRENIER1; break; case NINJA_GRENIER1: Trajet_config(TRAJECT_CONFIG_STD); if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2A; if(couleur == COULEUR_JAUNE){ Trajectoire_bezier(&trajectoire1, 1150, 1730, 1030, 1730, 980, 1850, 980, 1970, 0., 0.); } } break; case NINJA_GRENIER2A: if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2A_CALAGE; } break; case NINJA_GRENIER2A_CALAGE: if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2B; Localisation_set_y(1975); Localisation_set_x(980); Localisation_set_angle(-M_PI/2); if(couleur == COULEUR_JAUNE){ Trajectoire_bezier(&trajectoire1, 980, 1975, 980, 1850, 1160, 1925, 1420, 1925, 0., 0.); } } break; case NINJA_GRENIER2B: if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2C; if(couleur == COULEUR_JAUNE){ Trajectoire_bezier(&trajectoire1, 1420, 1925, 1377, 1925, 1354, 1945, 1354, 1980, 0., 0.); } } break; case NINJA_GRENIER2C: if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2C_CALAGE; } break; case NINJA_GRENIER2C_CALAGE: if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2D; Localisation_set_y(1975); Localisation_set_x(1354); Localisation_set_angle(-M_PI/2); Trajectoire_droite(&trajectoire1, 1354, 1975, 1354, 1630., 0., 0.); } break; case NINJA_GRENIER2D: if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = ACTION_TERMINEE; } break; case NINJA_FIN: //Asser_Position_maintien(); return ACTION_TERMINEE; } return ACTION_EN_COURS; } 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_GRENIER2A, NINJA_GRENIER2A_RECALE, NINJA_GRENIER2B, NINJA_GRENIER2C, 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); Trajet_config(TRAJECT_CONFIG_STD); Trajectoire_bezier(&trajectoire1, 645, 1870, 935, 1870, 1025, 1920, 1315-50, 1920, 0., 0.); } etat_sss = NINJA_GRENIER1A; break; case NINJA_GRENIER1A: 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, 1315-50, 1920, 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-50, 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, 1050, 1725, 0., 0.); } } break; case NINJA_GRENIER1D: if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2A; Trajectoire_bezier(&trajectoire1, 1050, 1725, 1170, 1725, 1200, 1820, 1200, 1950, 0., 0.); } break; case NINJA_GRENIER2A: if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2A_RECALE; } break; case NINJA_GRENIER2A_RECALE: if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2B; Trajet_config(100, 100); Localisation_set(1200, 1975, -M_PI/2); if(couleur == COULEUR_JAUNE){ Trajectoire_bezier(&trajectoire1, 1200, 1975, 1200, 1890, 1135, 1775, 1260, 1775, 0., 0.); } } break; case NINJA_GRENIER2B: if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = NINJA_GRENIER2C; Trajet_config(TRAJECT_CONFIG_STD); if(couleur == COULEUR_JAUNE){ Trajectoire_droite(&trajectoire1, 1260, 1775, 1400, 1775, 0., 0.); } } break; case NINJA_GRENIER2C: 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, TEST_EN_COURS, TEST_TERMINE } etat_test = TEST_INIT; switch(etat_test){ case TEST_INIT: if(Strategie_recale(0, step_ms) == ACTION_TERMINEE){ etat_test = TEST_TERMINE; } break; case TEST_TERMINE: return ACTION_TERMINEE; } return ACTION_EN_COURS; } enum etat_action_t Strategie_1_metre(uint32_t step_ms){ static enum{ TEST_INIT, TEST_EN_COURS, TEST_TERMINE } etat_test = TEST_INIT; static struct trajectoire_t trajectoire1; Trajet_config(TRAJECT_CONFIG_STD); switch(etat_test){ case TEST_INIT: Localisation_set(0, 0, 0); Trajectoire_droite(&trajectoire1, 0, 0, 1000, 0, 0., 0.); etat_test = TEST_EN_COURS; break; case TEST_EN_COURS: if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_test = TEST_TERMINE; } break; case TEST_TERMINE: return ACTION_TERMINEE; } return ACTION_EN_COURS; } void PAMI_dance(int identifiant){ switch (identifiant) { case 0: sleep_ms(500); Servomoteur_set(DOIGT_AVANCE); sleep_ms(250); Servomoteur_set(DOIGT_RETRAIT); sleep_ms(250); // t = 1,0 s sleep_ms(750); // t = 1,75 s Servomoteur_set(DOIGT_AVANCE); sleep_ms(250); Servomoteur_set(DOIGT_RETRAIT); sleep_ms(250); // t = 2,25 s sleep_ms(950); // t = 3,2 s for(int i =0; i<3;i++){ Servomoteur_set(DOIGT_AVANCE); sleep_ms(200); Servomoteur_set(DOIGT_RETRAIT); sleep_ms(200); }// t = 4,4 s sleep_ms(1400); // t = 5,8 break; case 1: case 2: case 3: sleep_ms(1100); // t = 1,1 s Servomoteur_set(DOIGT_AVANCE); sleep_ms(250); Servomoteur_set(DOIGT_RETRAIT); sleep_ms(250); // t = 1,6 s sleep_ms(800); // t = 2,4 s Servomoteur_set(DOIGT_AVANCE); sleep_ms(250); Servomoteur_set(DOIGT_RETRAIT); sleep_ms(250); // t = 2,9 s sleep_ms(1600); // t = 4,5 s for(int i =0; i<3;i++){ Servomoteur_set(DOIGT_AVANCE); sleep_ms(150); Servomoteur_set(DOIGT_RETRAIT); sleep_ms(150); }// t = 5,4 s sleep_ms(400); // t = 5,8 s break; default: break; } }