diff --git a/Servomoteur.c b/Servomoteur.c new file mode 100644 index 0000000..269a471 --- /dev/null +++ b/Servomoteur.c @@ -0,0 +1,74 @@ +#include "pico/stdlib.h" +#include "hardware/pwm.h" +#include "Servomoteur.h" + + + +void Servomoteur_Init(void){ + uint slice_num; + gpio_set_function(SERVO5, GPIO_FUNC_PWM); + gpio_set_function(SERVO6, GPIO_FUNC_PWM); + gpio_set_function(SERVO7, GPIO_FUNC_PWM); + gpio_set_function(SERVO_TURBINE, GPIO_FUNC_PWM); + + pwm_config pwm_servo = pwm_get_default_config(); + + // On veut un rebouclage au bout de 20 ms (50 Hz). + // Division de l'horloge système (133 MHz) par 254 : 523 kHz + pwm_config_set_clkdiv_int(&pwm_servo, 254); + // Valeur max du PXM pour avoir 50 Hz : 523 kHz / 50 Hz : 10460 + pwm_config_set_wrap(&pwm_servo, 10460); + // À la valeur finale, rebouclage à 0 (et non un compte à rebours) + pwm_config_set_phase_correct(&pwm_servo, false); + + slice_num = pwm_gpio_to_slice_num(SERVO5); + pwm_init(slice_num, &pwm_servo, true); + + slice_num = pwm_gpio_to_slice_num(SERVO_TURBINE); + pwm_init(slice_num, &pwm_servo, true); + + slice_num = pwm_gpio_to_slice_num(SERVO6); + pwm_init(slice_num, &pwm_servo, true); + + slice_num = pwm_gpio_to_slice_num(SERVO7); + pwm_init(slice_num, &pwm_servo, true); + + Servomoteur_set(SERVO5, SERVO_VALEUR_1_5MS); + Servomoteur_set(SERVO6, SERVO_VALEUR_1_5MS); + Servomoteur_set(SERVO7, SERVO_VALEUR_1_5MS); + Servomoteur_set(SERVO_TURBINE, SERVO_VALEUR_1MS); +} + +void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){ + uint slice_num = pwm_gpio_to_slice_num(servomoteur); + // Chan : A ou 0 si la GPIO est paire + // B ou 1 si la GPIO est impaire + pwm_set_chan_level(slice_num, servomoteur & 0x01, valeur); +} + +void Moteur_Init(){ + uint slice_num; + gpio_set_function(SERVO8_MOT_PROPULSEUR, GPIO_FUNC_PWM); + + pwm_config pwm_servo = pwm_get_default_config(); + + // On veut un rebouclage au bout de 20 ms (50 Hz). + // Division de l'horloge système (133 MHz) par 25 : 5230 kHz + pwm_config_set_clkdiv_int(&pwm_servo, 25); + // Valeur max du PXM pour avoir 5 kHz : 5230 kHz / 5 kHz : 1064 + pwm_config_set_wrap(&pwm_servo, 1064); + // À la valeur finale, rebouclage à 0 (et non un compte à rebours) + pwm_config_set_phase_correct(&pwm_servo, false); + + slice_num = pwm_gpio_to_slice_num(SERVO8_MOT_PROPULSEUR); + pwm_init(slice_num, &pwm_servo, true); + + Moteur_Set(0); + +} + +/// @brief Envoie la consigne PWM au moteur. +/// @param consigne : consigne entre 0 et 1. +void Moteur_Set(double consigne){ + Servomoteur_set(SERVO8_MOT_PROPULSEUR, (uint16_t) (1064 * consigne)); +} \ No newline at end of file diff --git a/Servomoteur.h b/Servomoteur.h new file mode 100644 index 0000000..27d9419 --- /dev/null +++ b/Servomoteur.h @@ -0,0 +1,25 @@ +#include "pico/stdlib.h" + +// Valeur des servomoteurs +#define SERVO_VALEUR_MAX 10459 +#define SERVO_VALEUR_2MS 1056 +#define SERVO_VALEUR_1_5MS 784 +#define SERVO_VALEUR_1MS 523 +#define SERVO_VALEUR_0_5MS 261 + + +// Servomoteurs +#define SERVO5 4 +#define SERVO6 5 +#define SERVO7 6 +#define SERVO8_MOT_PROPULSEUR 15 +#define SERVO_TURBINE 7 + +#define DOIGT_AVANCE 1, SERVO_VALEUR_1_5MS +#define DOIGT_RETRAIT 1, SERVO_VALEUR_1_5MS + + +void Servomoteur_Init(void); +void Servomoteur_set(uint8_t servomoteur, uint16_t valeur); +void Moteur_Init(void); +void Moteur_Set(double consigne); diff --git a/Strategie.c b/Strategie.c index 3fee903..1abe906 100644 --- a/Strategie.c +++ b/Strategie.c @@ -1,5 +1,6 @@ #include "Strategie.h" #include "Geometrie.h" +#include "Servomoteur.h" @@ -51,7 +52,7 @@ enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur -enum etat_action_t Strategie_groupie_1(uint32_t step_ms){ +enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur){ static enum{ SSS_INIT, SSS_AVANCE, @@ -88,3 +89,131 @@ enum etat_action_t Strategie_groupie_1(uint32_t step_ms){ return ACTION_EN_COURS; } + +enum etat_action_t Strategie_groupie_2(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: + 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); + 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_3(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: + 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); + 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; + +} + +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; + } + +} \ No newline at end of file diff --git a/Strategie.h b/Strategie.h index 4d4c83f..30aaaea 100644 --- a/Strategie.h +++ b/Strategie.h @@ -46,6 +46,10 @@ struct objectif_t{ }; enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t); +enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur); +enum etat_action_t Strategie_groupie_2(uint32_t step_ms, enum couleur_t couleur); +enum etat_action_t Strategie_groupie_3(uint32_t step_ms, enum couleur_t couleur); +void PAMI_dance(int); 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 387072a..fc1a1ea 100644 --- a/main.c +++ b/main.c @@ -36,7 +36,7 @@ void tension_batterie_init(void); uint16_t tension_batterie_lire(void); void identifiant_init(void); -uint identifiant_lire(void); +uint get_identifiant(void); int get_tirette(void); int get_couleur(void); @@ -63,8 +63,8 @@ void main(void) Temps_init(); identifiant_init(); - Localisation_init(identifiant_lire()); - Trajet_init(identifiant_lire()); + Localisation_init(get_identifiant()); + Trajet_init(get_identifiant()); i2c_maitre_init(); @@ -79,11 +79,16 @@ void main(void) gpio_set_dir(LED1PIN, GPIO_OUT ); gpio_put(LED1PIN, 1); + gpio_init(TIRETTE_PIN); + gpio_set_dir(LED1PIN, GPIO_IN); + + multicore_launch_core1(gestion_affichage); // TODO: A remettre - quand on aura récupéré un capteur //multicore_launch_core1(gestion_VL53L8CX); - sleep_ms(1500); + + sleep_ms(85000); printf("Demarrage...\n"); @@ -96,7 +101,7 @@ void main(void) gpio_put(LED1PIN, 0); // Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part - if(identifiant_lire() == 3){ + if(get_identifiant() == 3){ sleep_ms(90000); } @@ -107,7 +112,9 @@ void main(void) // Fin du match if((Temps_get_temps_ms() -temps_depart_ms) >15000 || (fin_match == 1)){ Moteur_Stop(); - while(1); + while(1){ + + } } if(temps_ms != Temps_get_temps_ms()){ temps_ms = Temps_get_temps_ms(); @@ -115,7 +122,7 @@ void main(void) QEI_update(); Localisation_gestion(); - if(Strategie_super_star(step_ms, COULEUR_BLEU) == ACTION_TERMINEE){ + if(Strategie_super_star(step_ms, COULEUR_JAUNE) == ACTION_TERMINEE){ Asser_Position_maintien(); if(Asser_Position_panic_angle()){ fin_match=1; @@ -130,6 +137,65 @@ void main(void) } } +void gestion_PAMI(uint32_t step_ms){ + static enum{ + PAMI_ATTENTE_TIRETTE, + PAMI_ATTENTE_TEMPO, + PAMI_FIN_TEMPO_MANUELLE, + PAMI_TRAJECTOIRE, + PAMI_DANCE, + }etat_PAMI; + static uint32_t temps_tirette, temps_mouvement; + + switch (etat_PAMI) + { + case PAMI_ATTENTE_TIRETTE: + if(get_tirette() == 0){ + etat_PAMI = PAMI_ATTENTE_TEMPO; + temps_tirette = Temps_get_temps_ms(); + } + break; + + case PAMI_ATTENTE_TEMPO: + if(get_tirette() == 1 && (Temps_get_temps_ms() - temps_tirette > 1000)){ + etat_PAMI = PAMI_FIN_TEMPO_MANUELLE; + } + if (Temps_get_temps_ms() - temps_tirette > 85000){ + etat_PAMI = PAMI_TRAJECTOIRE; + temps_mouvement = Temps_get_temps_ms(); + } + break; + + case PAMI_FIN_TEMPO_MANUELLE: + if(get_tirette() == 0){ + etat_PAMI = PAMI_TRAJECTOIRE; + temps_mouvement = Temps_get_temps_ms(); + } + break; + + case PAMI_TRAJECTOIRE: + switch (get_identifiant()) + { + case 0: Strategie_super_star(step_ms, get_couleur()); break; + case 1: Strategie_groupie_1(step_ms, get_couleur()); break; + case 2: Strategie_groupie_2(step_ms, get_couleur()); break; + case 3: Strategie_groupie_3(step_ms, get_couleur()); break; + default: break; + } + break; + + case PAMI_DANCE: + Moteur_Stop(); + while(1){ + PAMI_dance(get_identifiant()); + } + + default: + break; + } + +} + /// @brief Obtient la distance de l'obstacle le plus proche. /// @param void gestion_VL53L8CX(void){ @@ -176,7 +242,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(), identifiant_lire(), get_tirette()); + printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), get_identifiant(), get_tirette()); } void tension_batterie_init(void){ @@ -216,6 +282,8 @@ void identifiant_init(){ gpio_set_dir(COULEUR_PIN, GPIO_IN); } +/// @brief Lit la tirette +/// @return 1 si la tirette est branchée, 0 sinon int get_tirette(){ return !gpio_get(TIRETTE_PIN); @@ -227,6 +295,6 @@ int get_couleur(void){ /// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse /// @return identifiant du robot (PDI switch) -uint identifiant_lire(){ +uint get_identifiant(){ return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26); }