From a0626c52876c5c726c66a26c0d166a5444dba12c Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 29 May 2025 15:32:21 +0200 Subject: [PATCH] =?UTF-8?q?PAMI=202=20&=203=20sans=20=C3=A9vitement?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Asser_Moteurs.c | 5 +++++ Asser_Moteurs.h | 3 ++- Servomoteur.c | 4 ++-- Strategie.c | 39 ++++++++++++++++++++++----------------- Strategie.h | 1 + main.c | 35 +++++++++++++++++++++++------------ 6 files changed, 55 insertions(+), 32 deletions(-) diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index 0098144..cb17b3a 100644 --- a/Asser_Moteurs.c +++ b/Asser_Moteurs.c @@ -60,6 +60,11 @@ uint32_t AsserMoteur_RobotImmobile(int step_ms){ return 0; } +void AsserMoteurs_stop(void){ + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0); + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0); +} + /// @brief Fonction d'asservissement des moteurs, à appeler périodiquement /// @param step_ms void AsserMoteur_Gestion(int step_ms){ diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h index 42adbda..dcdfe8f 100644 --- a/Asser_Moteurs.h +++ b/Asser_Moteurs.h @@ -5,4 +5,5 @@ void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s); float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur); float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms); void AsserMoteur_Gestion(int step_ms); -void AsserMoteur_Init(int); \ No newline at end of file +void AsserMoteur_Init(int); +void AsserMoteurs_stop(void); \ No newline at end of file diff --git a/Servomoteur.c b/Servomoteur.c index 242b545..faa76ad 100644 --- a/Servomoteur.c +++ b/Servomoteur.c @@ -24,9 +24,9 @@ void Servomoteur_Init(void){ slice_num = pwm_gpio_to_slice_num(SERVO1); pwm_init(slice_num, &pwm_servo, true); - Servomoteur_set(SERVO0, SERVO_VALEUR_1_5MS); + Servomoteur_set(SERVO0, SERVO_VALEUR_0_5MS); - Servomoteur_set(SERVO1, SERVO_VALEUR_1_5MS); + Servomoteur_set(SERVO1, SERVO_VALEUR_0_5MS); } void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){ diff --git a/Strategie.c b/Strategie.c index 1abe906..d42cbdf 100644 --- a/Strategie.c +++ b/Strategie.c @@ -1,4 +1,5 @@ #include "Strategie.h" +#include "Asser_Position.h" #include "Geometrie.h" #include "Servomoteur.h" @@ -8,7 +9,7 @@ enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur static enum{ SSS_INIT, SSS_AVANCE, - SSS_DANCE + SSS_FIN } etat_sss = SSS_INIT; static struct trajectoire_t trajectoire_composee; static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3; @@ -39,11 +40,12 @@ enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur 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; + etat_sss = SSS_FIN; } break; - case SSS_DANCE: + case SSS_FIN: + Asser_Position_maintien(); return ACTION_TERMINEE; } return ACTION_EN_COURS; @@ -65,14 +67,18 @@ enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur) 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); + if(couleur == COULEUR_JAUNE){ + Localisation_set(45, 1780, 0); + Trajectoire_bezier(&trajectoire1, 45, 1780, 480, 1780, 710, 1342, 960, 1410, 0, 0); + Trajectoire_bezier(&trajectoire2, 960, 1410, 1014, 1424, 1053, 1432, 1092, 1460, 0, 0); + }else{ + Localisation_set(3000 - 45, 1780, M_PI); + Trajectoire_bezier(&trajectoire1, 3000 - 45, 1780, 3000 - 480, 1780, 3000 - 710, 1342, 3000 - 960, 1410, 0, 0); + Trajectoire_bezier(&trajectoire2, 3000 - 960, 1410, 3000 - 1014, 1424, 3000 - 1053, 1432, 3000 - 1092, 1460, 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; @@ -103,20 +109,19 @@ enum etat_action_t Strategie_groupie_2(uint32_t step_ms, enum couleur_t couleur) 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); + if(couleur == COULEUR_JAUNE){ + Localisation_set(45, 1600, 0); + Trajectoire_bezier(&trajectoire1, 45, 1600, 326, 1600, 935, 980, 1500, 1380, 0, 0); + }else{ + Localisation_set(3000 - 45, 1600, M_PI); + Trajectoire_bezier(&trajectoire1, 3000 - 45, 1600, 3000 - 326, 1600, 3000 - 935, 980, 3000 - 1500, 1380, 0, 0); + } 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){ + if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = SSS_DANCE; } break; diff --git a/Strategie.h b/Strategie.h index 30aaaea..eb11610 100644 --- a/Strategie.h +++ b/Strategie.h @@ -1,3 +1,4 @@ +#include "Asser_Moteurs.h" #include "pico/stdlib.h" #include "Localisation.h" #include "Temps.h" diff --git a/main.c b/main.c index 5724f03..986c62b 100644 --- a/main.c +++ b/main.c @@ -41,7 +41,7 @@ uint get_identifiant(void); int get_tirette(void); int get_couleur(void); -void gestion_PAMI(uint32_t); +void gestion_PAMI(uint32_t step_ms, int * asser_pos); void gestion_VL53L8CX(void); const uint32_t step_ms=1; @@ -72,6 +72,7 @@ void main(void) uint32_t temps_ms = Temps_get_temps_ms(); uint32_t temps_depart_ms; + int asser_pos=1; struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0}; float vitesse_mm_s=100; @@ -105,15 +106,17 @@ void main(void) QEI_update(); Localisation_gestion(); - gestion_PAMI(step_ms); - AsserMoteur_Gestion(step_ms); + gestion_PAMI(step_ms, &asser_pos); + if(asser_pos){ + AsserMoteur_Gestion(step_ms); + } } } } } -void gestion_PAMI(uint32_t step_ms){ +void gestion_PAMI(uint32_t step_ms, int * asser_pos){ static enum{ PAMI_ATTENTE_TIRETTE, PAMI_ATTENTE_TEMPO, @@ -122,6 +125,7 @@ void gestion_PAMI(uint32_t step_ms){ PAMI_DANCE, }etat_PAMI; static uint32_t temps_tirette, temps_mouvement; + enum etat_action_t etat_action; switch (etat_PAMI) { @@ -150,24 +154,31 @@ void gestion_PAMI(uint32_t step_ms){ break; case PAMI_TRAJECTOIRE: - if(Temps_get_temps_ms() - temps_tirette > 15000){ + if(Temps_get_temps_ms() - temps_mouvement > 15000){ etat_PAMI = PAMI_DANCE; } 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; + case 0: etat_action = Strategie_super_star(step_ms, get_couleur()); break; + case 1: etat_action = Strategie_groupie_1(step_ms, get_couleur()); break; + case 2: etat_action = Strategie_groupie_2(step_ms, get_couleur()); break; + case 3: etat_action = Strategie_groupie_3(step_ms, get_couleur()); break; + default: etat_action = ACTION_TERMINEE; break; + } + if(etat_action == ACTION_TERMINEE){ + Moteur_Stop(); + *asser_pos = 0; } break; case PAMI_DANCE: Moteur_Stop(); - while(1){ - PAMI_dance(get_identifiant()); + if(Temps_get_temps_ms() - temps_tirette > 15000){ + while(1){ + PAMI_dance(get_identifiant()); + } } + break; default: break;