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

Correction du parcours de trajectoire, condition de fin et condition d'aservissement sur une consigne proche

Browse Source
This commit is contained in:
Samuel 2026-05-04 20:50:13 +02:00
parent ac1df80dc1
commit d30dac3b62
9 changed files with 154 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Asser_Position.c
View File

@ -47,9 +47,9 @@ void Asser_Position(struct position_t position_consigne){
avance_mm_s = delta_avance_mm * GAIN_P_POSITION; avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION; rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
if(delta_avance_mm < 10){ /*if(delta_avance_mm < 10){
rotation_radian_s=delta_avance_mm/10 * rotation_radian_s; rotation_radian_s=delta_avance_mm/10 * rotation_radian_s;
} }*/
// Commande en vitesse // Commande en vitesse
commande_vitesse(avance_mm_s, rotation_radian_s); commande_vitesse(avance_mm_s, rotation_radian_s);

20
CMakeLists.txt
View File

@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.13)
include(pico_sdk_import.cmake) include(pico_sdk_import.cmake)
project(Mon_Projet C CXX ASM) project(PAMI2026a C CXX ASM)
set(CMAKE_C_STNDARD 11) set(CMAKE_C_STNDARD 11)
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)
@ -10,7 +10,7 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR})
pico_sdk_init() pico_sdk_init()
add_executable(Mon_Projet add_executable(PAMI2026a
Asser_Position.c Asser_Position.c
Asser_Moteurs.c Asser_Moteurs.c
Commande_vitesse.c Commande_vitesse.c
@ -39,11 +39,11 @@ add_executable(Mon_Projet
Platform/platform.c Platform/platform.c
) )
pico_generate_pio_header(Mon_Projet ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) pico_generate_pio_header(PAMI2026a ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
target_include_directories(Mon_Projet PRIVATE VL53L8CX_ULD_API/inc/) target_include_directories(PAMI2026a PRIVATE VL53L8CX_ULD_API/inc/)
target_link_libraries(Mon_Projet target_link_libraries(PAMI2026a
hardware_adc hardware_adc
hardware_i2c hardware_i2c
hardware_pwm hardware_pwm
@ -52,12 +52,12 @@ target_link_libraries(Mon_Projet
pico_multicore pico_multicore
) )
pico_enable_stdio_usb(Mon_Projet 1) pico_enable_stdio_usb(PAMI2026a 1)
pico_enable_stdio_uart(Mon_Projet 1) pico_enable_stdio_uart(PAMI2026a 1)
pico_add_extra_outputs(Mon_Projet) pico_add_extra_outputs(PAMI2026a)
add_custom_target(Flash add_custom_target(Flash
DEPENDS Mon_Projet DEPENDS PAMI2026a
COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/Mon_Projet.uf2 COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI2026a.uf2
) )

2
QEI.c
View File

@ -51,7 +51,7 @@ void QEI_init(int identifiant){
QEI_est_init=true; QEI_est_init=true;
} }
impulsion_par_mm = IMPULSION_PAR_MM_50_1; impulsion_par_mm = IMPULSION_PAR_MM_50_1;
if(identifiant == 0 || identifiant >= 4){ if(identifiant >= 4){
impulsion_par_mm = IMPULSION_PAR_MM_30_1; impulsion_par_mm = IMPULSION_PAR_MM_30_1;
} }

214
Strategie.c
View File

@ -5,172 +5,124 @@
enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur){ enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t couleur){
static enum{ static enum{
SSS_INIT, NINJA_INIT,
SSS_AVANCE, NINJA_GRENIER1,
SSS_FIN NINJA_GRENIER2A,
} etat_sss = SSS_INIT; NINJA_GRENIER2B,
NINJA_GRENIER2C,
NINJA_FIN
} etat_sss = NINJA_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;
switch(etat_sss){ switch(etat_sss){
case SSS_INIT: case NINJA_INIT:
if(couleur == COULEUR_JAUNE){ if(couleur == COULEUR_JAUNE){
Localisation_set(45, 1895, 0); Localisation_set(645, 1870, 0);
Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0); Trajectoire_bezier(&trajectoire1,
Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2); 645, 1870,
Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0); 800, 1870,
890, 1730,
990, 1730, 0., 0.);
Trajectoire_droite(&trajectoire2,
990, 1730,
1150, 1730, 0, 0);
}else{ }else{
Localisation_set(3000 - 45, 1895, M_PI); Localisation_set(3000 - 45, 1870, M_PI);
Trajectoire_droite(&trajectoire1, 3000-45, 1895, 3000- 1135, 1895, M_PI, M_PI); Trajectoire_bezier(&trajectoire1,
Trajectoire_circulaire(&trajectoire2, 3000-1135, 1645, M_PI/2, M_PI, 250, M_PI, -M_PI/2); 3000 - 45, 1870, 3000 - 800, 1870,
Trajectoire_droite(&trajectoire3, 3000-1385, 1645, 3000-1385, 1580, 0, 0); 3000 - 890, 1730, 3000 - 990, 1730, 0., 0.);
Trajectoire_droite(&trajectoire2, 3000 - 990, 1730, 3000 - 1150, 1730, 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 = NINJA_GRENIER1;
etat_sss = SSS_AVANCE;
break; break;
case SSS_AVANCE: case NINJA_GRENIER1:
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_FIN; etat_sss = NINJA_GRENIER2A;
if(couleur == COULEUR_JAUNE){
Trajectoire_bezier(&trajectoire1,
1150, 1730,
1030, 1730,
980, 1850,
980, 1970, 0., 0.);
}
} }
break; break;
case SSS_FIN: case NINJA_GRENIER2A:
Asser_Position_maintien(); if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2B;
if(couleur == COULEUR_JAUNE){
Trajectoire_bezier(&trajectoire1,
980, 1970,
980, 1850,
1160, 1925,
1420, 1925, 0., 0.);
}
}
break;
case NINJA_GRENIER2B:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2C;
if(couleur == COULEUR_JAUNE){
Trajet_config(TRAJECT_CONFIG_STD);
Trajectoire_bezier(&trajectoire1,
1420, 1925,
1377, 1925,
1354, 1945,
1354, 1980, 0., 0.);
}
}
break;
case NINJA_GRENIER2C:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_FIN;
}
break;
case NINJA_FIN:
//Asser_Position_maintien();
return ACTION_TERMINEE; return ACTION_TERMINEE;
} }
return ACTION_EN_COURS; return ACTION_EN_COURS;
} }
enum etat_action_t Strategie_test(uint32_t step_ms){
static enum{
TEST_INIT,
TEST_EN_COURS,
TEST_TERMINE
} etat_test = TEST_INIT;
switch(etat_test){
enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur){ case TEST_INIT:
static enum{ if(Strategie_recale(0, step_ms) == ACTION_TERMINEE){
SSS_INIT, etat_test = TEST_TERMINE;
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, 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);
etat_sss = SSS_AVANCE;
break;
case SSS_AVANCE:
Trajet_config(TRAJECT_CONFIG_RAPIDE);
if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_PAUSE_DEVANT_OBSTACLE) == ACTION_TERMINEE){
etat_sss = SSS_DANCE;
} }
break; break;
case SSS_DANCE: case TEST_TERMINE:
return ACTION_TERMINEE; return ACTION_TERMINEE;
} }
return ACTION_EN_COURS;
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:
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(trajectoire1, step_ms, EVITEMENT_PAUSE_DEVANT_OBSTACLE) == 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){ void PAMI_dance(int identifiant){
switch (identifiant) switch (identifiant)
{ {

10
Strategie.h
View File

@ -13,6 +13,8 @@
#define TIRETTE 14 #define TIRETTE 14
//#define CORR_ANGLE_DEPART_DEGREE (-1.145) //#define CORR_ANGLE_DEPART_DEGREE (-1.145)
#define CORR_ANGLE_DEPART_DEGREE (0) #define CORR_ANGLE_DEPART_DEGREE (0)
#define RECALE_SENS_AVANT 1
#define RECALE_SENS_ARRIERE 0
enum etat_action_t{ enum etat_action_t{
ACTION_EN_COURS, ACTION_EN_COURS,
@ -46,13 +48,13 @@ struct objectif_t{
ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible; ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible;
}; };
enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t); enum etat_action_t Strategie_Ninja(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_test(uint32_t step_ms);
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); void PAMI_dance(int);
enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement); enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
enum etat_action_t Strategie_recale(int sens_avant, uint32_t step_ms);
// STRATEGIE_H // STRATEGIE_H
#endif #endif

35
Strategie_deplacement.c
View File

@ -71,3 +71,38 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
return etat_action; return etat_action;
} }
enum etat_action_t Strategie_recale(int sens_avant, uint32_t step_ms){
const float vitesse_recale = 200;
static int timer_recale_ms;
static enum {
RECALE_INIT,
RECALE_AVANCE,
} etat_recale=RECALE_INIT;
switch(etat_recale){
case RECALE_INIT:
if(sens_avant){
AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_recale);
AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_recale);
}else{
AsserMoteur_setConsigne_mm_s(MOTEUR_A, -vitesse_recale);
AsserMoteur_setConsigne_mm_s(MOTEUR_B, -vitesse_recale);
}
timer_recale_ms = 2000;
etat_recale=RECALE_AVANCE;
break;
case RECALE_AVANCE:
timer_recale_ms -= step_ms;
if(timer_recale_ms <= 0){
AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0);
etat_recale=RECALE_INIT;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}

2
Trajet.c
View File

@ -103,7 +103,7 @@ int Trajet_terminee(float abscisse){
return 1; return 1;
}*/ }*/
if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){ if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER && trajet_trajectoire.type != TRAJECTOIRE_COMPOSEE){
if(abscisse >= 1 || distance_fin_trajectoire_mm < 0.1){ if(abscisse >= 1 || distance_fin_trajectoire_mm < 0.1){
return 1; return 1;
} }

2
VL53L8_2024.c
View File

@ -154,7 +154,7 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
int min_distance = 2000; int min_distance = 2000;
int col, row; int col, row;
for(col=0; col<8; col++){ for(col=0; col<8; col++){
for(row=0; row<=3; row++){ for(row=5; row<8; row++){
if(min_distance > Results.distance_mm[col + 8*row]){ if(min_distance > Results.distance_mm[col + 8*row]){
min_distance = Results.distance_mm[col + 8*row]; min_distance = Results.distance_mm[col + 8*row];
} }

33
main.c
View File

@ -89,12 +89,8 @@ void main(void)
// TODO: A remettre - quand on aura récupéré un capteur // TODO: A remettre - quand on aura récupéré un capteur
if(get_identifiant() != 0){ multicore_launch_core1(gestion_VL53L8CX);
multicore_launch_core1(gestion_VL53L8CX);
}else{
multicore_launch_core1(gestion_affichage);
}
printf("Demarrage...\n"); printf("Demarrage...\n");
@ -129,7 +125,7 @@ void gestion_PAMI(uint32_t step_ms, int * asser_pos){
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; enum etat_action_t etat_action=ACTION_EN_COURS;
switch (etat_PAMI) switch (etat_PAMI)
{ {
@ -161,22 +157,23 @@ void gestion_PAMI(uint32_t step_ms, int * asser_pos){
if(Temps_get_temps_ms() - temps_mouvement > 15000){ if(Temps_get_temps_ms() - temps_mouvement > 15000){
etat_PAMI = PAMI_DANCE; etat_PAMI = PAMI_DANCE;
} }
switch (get_identifiant()) if(etat_action == ACTION_EN_COURS){
{ switch (get_identifiant())
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 0: etat_action = Strategie_Ninja(step_ms, get_couleur()); break;
case 2: etat_action = Strategie_groupie_2(step_ms, get_couleur()); break; case 7: etat_action = Strategie_test(step_ms); break;
case 3: etat_action = Strategie_groupie_3(step_ms, get_couleur()); break; default: etat_action = ACTION_TERMINEE; break;
default: etat_action = ACTION_TERMINEE; break; }
} if(etat_action == ACTION_TERMINEE){
if(etat_action == ACTION_TERMINEE){ Moteur_Stop();
Moteur_Stop(); *asser_pos = 0;
*asser_pos = 0; }
} }
break; break;
case PAMI_DANCE: case PAMI_DANCE:
Moteur_Stop(); Moteur_Stop();
*asser_pos = 0;
if(Temps_get_temps_ms() - temps_tirette > 15000){ if(Temps_get_temps_ms() - temps_tirette > 15000){
while(1){ while(1){
PAMI_dance(get_identifiant()); PAMI_dance(get_identifiant());