Keuronde/2026-Propulsion
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

PAMI 2 & 3 sans évitement

Browse Source
This commit is contained in:
Samuel 2025-05-29 15:32:21 +02:00
parent e342cad0d9
commit a0626c5287
6 changed files with 55 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Asser_Moteurs.c
View File

@ -60,6 +60,11 @@ uint32_t AsserMoteur_RobotImmobile(int step_ms){
return 0; 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 /// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
/// @param step_ms /// @param step_ms
void AsserMoteur_Gestion(int step_ms){ void AsserMoteur_Gestion(int step_ms){

3
Asser_Moteurs.h
View File

@ -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_getConsigne_mm_s(enum t_moteur moteur);
float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms); float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);
void AsserMoteur_Gestion(int step_ms); void AsserMoteur_Gestion(int step_ms);
void AsserMoteur_Init(int); void AsserMoteur_Init(int);
void AsserMoteurs_stop(void);

4
Servomoteur.c
View File

@ -24,9 +24,9 @@ void Servomoteur_Init(void){
slice_num = pwm_gpio_to_slice_num(SERVO1); slice_num = pwm_gpio_to_slice_num(SERVO1);
pwm_init(slice_num, &pwm_servo, true); 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){ void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){

39
Strategie.c
View File

@ -1,4 +1,5 @@
#include "Strategie.h" #include "Strategie.h"
#include "Asser_Position.h"
#include "Geometrie.h" #include "Geometrie.h"
#include "Servomoteur.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{ static enum{
SSS_INIT, SSS_INIT,
SSS_AVANCE, SSS_AVANCE,
SSS_DANCE SSS_FIN
} etat_sss = SSS_INIT; } etat_sss = SSS_INIT;
static struct trajectoire_t trajectoire_composee; static struct trajectoire_t trajectoire_composee;
static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3; 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: case SSS_AVANCE:
Trajet_config(TRAJECT_CONFIG_RAPIDE); Trajet_config(TRAJECT_CONFIG_RAPIDE);
if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = SSS_DANCE; etat_sss = SSS_FIN;
} }
break; break;
case SSS_DANCE: case SSS_FIN:
Asser_Position_maintien();
return ACTION_TERMINEE; return ACTION_TERMINEE;
} }
return ACTION_EN_COURS; 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){ switch(etat_sss){
case SSS_INIT: case SSS_INIT:
Localisation_set(45, 1895, 0); if(couleur == COULEUR_JAUNE){
Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0); Localisation_set(45, 1780, 0);
Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2); Trajectoire_bezier(&trajectoire1, 45, 1780, 480, 1780, 710, 1342, 960, 1410, 0, 0);
Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 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_init(&trajectoire_composee);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1); Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2); Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
etat_sss = SSS_AVANCE; etat_sss = SSS_AVANCE;
break; break;
@ -103,20 +109,19 @@ enum etat_action_t Strategie_groupie_2(uint32_t step_ms, enum couleur_t couleur)
switch(etat_sss){ switch(etat_sss){
case SSS_INIT: case SSS_INIT:
Localisation_set(45, 1895, 0); if(couleur == COULEUR_JAUNE){
Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0); Localisation_set(45, 1600, 0);
Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2); Trajectoire_bezier(&trajectoire1, 45, 1600, 326, 1600, 935, 980, 1500, 1380, 0, 0);
Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0); }else{
Trajectoire_composee_init(&trajectoire_composee); Localisation_set(3000 - 45, 1600, M_PI);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1); Trajectoire_bezier(&trajectoire1, 3000 - 45, 1600, 3000 - 326, 1600, 3000 - 935, 980, 3000 - 1500, 1380, 0, 0);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2); }
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
etat_sss = SSS_AVANCE; etat_sss = SSS_AVANCE;
break; break;
case SSS_AVANCE: case SSS_AVANCE:
Trajet_config(TRAJECT_CONFIG_RAPIDE); 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; etat_sss = SSS_DANCE;
} }
break; break;

1
Strategie.h
View File

@ -1,3 +1,4 @@
#include "Asser_Moteurs.h"
#include "pico/stdlib.h" #include "pico/stdlib.h"
#include "Localisation.h" #include "Localisation.h"
#include "Temps.h" #include "Temps.h"

35
main.c
View File

@ -41,7 +41,7 @@ uint get_identifiant(void);
int get_tirette(void); int get_tirette(void);
int get_couleur(void); int get_couleur(void);
void gestion_PAMI(uint32_t); void gestion_PAMI(uint32_t step_ms, int * asser_pos);
void gestion_VL53L8CX(void); void gestion_VL53L8CX(void);
const uint32_t step_ms=1; const uint32_t step_ms=1;
@ -72,6 +72,7 @@ void main(void)
uint32_t temps_ms = Temps_get_temps_ms(); uint32_t temps_ms = Temps_get_temps_ms();
uint32_t temps_depart_ms; uint32_t temps_depart_ms;
int asser_pos=1;
struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0}; struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0};
float vitesse_mm_s=100; float vitesse_mm_s=100;
@ -105,15 +106,17 @@ void main(void)
QEI_update(); QEI_update();
Localisation_gestion(); Localisation_gestion();
gestion_PAMI(step_ms); gestion_PAMI(step_ms, &asser_pos);
AsserMoteur_Gestion(step_ms); 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{ static enum{
PAMI_ATTENTE_TIRETTE, PAMI_ATTENTE_TIRETTE,
PAMI_ATTENTE_TEMPO, PAMI_ATTENTE_TEMPO,
@ -122,6 +125,7 @@ void gestion_PAMI(uint32_t step_ms){
PAMI_DANCE, PAMI_DANCE,
}etat_PAMI; }etat_PAMI;
static uint32_t temps_tirette, temps_mouvement; static uint32_t temps_tirette, temps_mouvement;
enum etat_action_t etat_action;
switch (etat_PAMI) switch (etat_PAMI)
{ {
@ -150,24 +154,31 @@ void gestion_PAMI(uint32_t step_ms){
break; break;
case PAMI_TRAJECTOIRE: case PAMI_TRAJECTOIRE:
if(Temps_get_temps_ms() - temps_tirette > 15000){ if(Temps_get_temps_ms() - temps_mouvement > 15000){
etat_PAMI = PAMI_DANCE; etat_PAMI = PAMI_DANCE;
} }
switch (get_identifiant()) switch (get_identifiant())
{ {
case 0: Strategie_super_star(step_ms, get_couleur()); break; case 0: etat_action = Strategie_super_star(step_ms, get_couleur()); break;
case 1: Strategie_groupie_1(step_ms, get_couleur()); break; case 1: etat_action = Strategie_groupie_1(step_ms, get_couleur()); break;
case 2: Strategie_groupie_2(step_ms, get_couleur()); break; case 2: etat_action = Strategie_groupie_2(step_ms, get_couleur()); break;
case 3: Strategie_groupie_3(step_ms, get_couleur()); break; case 3: etat_action = Strategie_groupie_3(step_ms, get_couleur()); break;
default: break; default: etat_action = ACTION_TERMINEE; break;
}
if(etat_action == ACTION_TERMINEE){
Moteur_Stop();
*asser_pos = 0;
} }
break; break;
case PAMI_DANCE: case PAMI_DANCE:
Moteur_Stop(); Moteur_Stop();
while(1){ if(Temps_get_temps_ms() - temps_tirette > 15000){
PAMI_dance(get_identifiant()); while(1){
PAMI_dance(get_identifiant());
}
} }
break;
default: default:
break; break;