diff --git a/Strategie.c b/Strategie.c index 6c777d6..3fee903 100644 --- a/Strategie.c +++ b/Strategie.c @@ -3,7 +3,55 @@ -enum etat_action_t Strategie_super_star(uint32_t step_ms){ +enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur){ + static enum{ + SSS_INIT, + SSS_AVANCE, + SSS_DANCE + } etat_sss = SSS_INIT; + static struct trajectoire_t trajectoire_composee; + static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3; + + + + switch(etat_sss){ + case SSS_INIT: + if(couleur == COULEUR_JAUNE){ + Localisation_set(45, 1895, 0); + Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0); + Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2); + Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0); + }else{ + Localisation_set(3000 - 45, 1895, M_PI); + Trajectoire_droite(&trajectoire1, 3000-45, 1895, 3000- 1135, 1895, M_PI, M_PI); + Trajectoire_circulaire(&trajectoire2, 3000-1135, 1645, M_PI/2, M_PI, 250, M_PI, -M_PI/2); + Trajectoire_droite(&trajectoire3, 3000-1385, 1645, 3000-1385, 1580, 0, 0); + } + + Trajectoire_composee_init(&trajectoire_composee); + Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1); + Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2); + Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3); + etat_sss = SSS_AVANCE; + break; + + case SSS_AVANCE: + Trajet_config(TRAJECT_CONFIG_RAPIDE); + if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ + etat_sss = SSS_DANCE; + } + break; + + case SSS_DANCE: + return ACTION_TERMINEE; + } + return ACTION_EN_COURS; + +} + + + +enum etat_action_t Strategie_groupie_1(uint32_t step_ms){ static enum{ SSS_INIT, SSS_AVANCE, diff --git a/Strategie.h b/Strategie.h index b7e6512..4d4c83f 100644 --- a/Strategie.h +++ b/Strategie.h @@ -45,7 +45,7 @@ struct objectif_t{ ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible; }; -enum etat_action_t Strategie_super_star(uint32_t step_ms); +enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t); enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement); diff --git a/main.c b/main.c index 1f1404f..387072a 100644 --- a/main.c +++ b/main.c @@ -27,8 +27,8 @@ #define TIRETTE_PIN 6 #define COULEUR_PIN 4 -#define COULEUR_BLEU 1 -#define COULEUR_JAUNE 0 +#define COULEUR_BLEU 0 +#define COULEUR_JAUNE 1 void affichage(void); void gestion_affichage(void); @@ -83,7 +83,7 @@ void main(void) multicore_launch_core1(gestion_affichage); // TODO: A remettre - quand on aura récupéré un capteur //multicore_launch_core1(gestion_VL53L8CX); - sleep_ms(5000); + sleep_ms(1500); printf("Demarrage...\n"); @@ -115,7 +115,7 @@ void main(void) QEI_update(); Localisation_gestion(); - if(Strategie_super_star(step_ms) == ACTION_TERMINEE){ + if(Strategie_super_star(step_ms, COULEUR_BLEU) == ACTION_TERMINEE){ Asser_Position_maintien(); if(Asser_Position_panic_angle()){ fin_match=1;