Keuronde/PAMI_2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Debut de la dance des PAMI + optimisation tirette/attente

Browse Source
This commit is contained in:
Samuel 2025-05-28 09:30:24 +02:00
parent 243e8c1e36
commit 2d84c5748a
5 changed files with 310 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
Servomoteur.c Normal file
View File

@ -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));
}

25
Servomoteur.h Normal file
View File

@ -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);

131
Strategie.c
View File

@ -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;
}
}

4
Strategie.h
View File

@ -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);

86
main.c
View File

@ -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);
}