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

Compare commits

base: Keuronde:9b69210d36b4d3141cf54c5b340c1ab6b7e35f91
Branches Tags
Keuronde:master
..
compare: Keuronde:2d84c5748ad99d2f18591f75bf5354888eea2b3c
Branches Tags
Keuronde:master

No commits in common. "9b69210d36b4d3141cf54c5b340c1ab6b7e35f91" and "2d84c5748ad99d2f18591f75bf5354888eea2b3c" have entirely different histories.
9b69210d36 ... 2d84c5748a

15 changed files with 230 additions and 407 deletions
Show Stats Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@ -12,7 +12,6 @@
"asser_position.h": "c",
"stdlib.h": "c",
"strategie.h": "c",
"strategie_deplacement.h": "c",
"servomoteur.h": "c"
"strategie_deplacement.h": "c"
}
}

4
.vscode/tasks.json vendored
View File

@ -2,7 +2,7 @@
"tasks": [
{
"type": "shell",
"command": "mkdir -p build; cd build; cmake ../; make",
"command": "cd build; cmake ../; make",
"label": "CMake in build/",
"problemMatcher": [],
"group": {
@ -13,7 +13,7 @@
{
"type": "shell",
"command": "cd build; cmake ../; make Flash",
"label": "mkdir -p build; CMake & Make & Flash",
"label": "CMake & Make & Flash",
"problemMatcher": [],
"group": {
"kind": "build",

5
Asser_Moteurs.c
View File

@ -60,11 +60,6 @@ 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){

3
Asser_Moteurs.h
View File

@ -5,5 +5,4 @@ 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);
void AsserMoteurs_stop(void);
void AsserMoteur_Init(int);

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;
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;
}*/
}
// Commande en vitesse
commande_vitesse(avance_mm_s, rotation_radian_s);

21
CMakeLists.txt
View File

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

7
QEI.c
View File

@ -17,10 +17,7 @@
// Nombre d'impulsions par tour de roue : 8000
// Impulsion / mm : 42,44
// #define IMPULSION_PAR_MM_50_1 (14.15f)
// Mesuré sur 1 mètre
#define IMPULSION_PAR_MM_50_1 (14.15f)
#define IMPULSION_PAR_MM_50_1 (12.45f)
#define IMPULSION_PAR_MM_30_1 (7.47f)
float impulsion_par_mm;
@ -54,7 +51,7 @@ void QEI_init(int identifiant){
QEI_est_init=true;
}
impulsion_par_mm = IMPULSION_PAR_MM_50_1;
if(identifiant >= 4){
if(identifiant == 0 || identifiant >= 4){
impulsion_par_mm = IMPULSION_PAR_MM_30_1;
}

49
Servomoteur.c
View File

@ -6,8 +6,11 @@
void Servomoteur_Init(void){
uint slice_num;
gpio_set_function(SERVO0, GPIO_FUNC_PWM);
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).
@ -18,15 +21,22 @@ void Servomoteur_Init(void){
// À 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(SERVO0);
slice_num = pwm_gpio_to_slice_num(SERVO5);
pwm_init(slice_num, &pwm_servo, true);
slice_num = pwm_gpio_to_slice_num(SERVO1);
slice_num = pwm_gpio_to_slice_num(SERVO_TURBINE);
pwm_init(slice_num, &pwm_servo, true);
Servomoteur_set(SERVO0, SERVO_VALEUR_0_5MS);
slice_num = pwm_gpio_to_slice_num(SERVO6);
pwm_init(slice_num, &pwm_servo, true);
Servomoteur_set(SERVO1, SERVO_VALEUR_0_5MS);
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){
@ -35,3 +45,30 @@ void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){
// 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));
}

14
Servomoteur.h
View File

@ -9,11 +9,17 @@
// Servomoteurs
#define SERVO0 0
#define SERVO1 1
#define DOIGT_AVANCE 0, SERVO_VALEUR_1_5MS
#define DOIGT_RETRAIT 0, SERVO_VALEUR_0_5MS
#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);

372
Strategie.c
View File

@ -1,330 +1,170 @@
#include "Strategie.h"
#include "Asser_Position.h"
#include "Geometrie.h"
#include "Servomoteur.h"
enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t couleur){
enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur){
static enum{
NINJA_INIT,
NINJA_GRENIER1,
NINJA_GRENIER2A,
NINJA_GRENIER2A_CALAGE,
NINJA_GRENIER2B,
NINJA_GRENIER2C,
NINJA_GRENIER2C_CALAGE,
NINJA_GRENIER2D,
NINJA_FIN
} etat_sss = NINJA_INIT;
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 NINJA_INIT:
case SSS_INIT:
if(couleur == COULEUR_JAUNE){
Localisation_set(645, 1870, 0);
Trajectoire_bezier(&trajectoire1,
645, 1870,
800, 1870,
890, 1730,
990, 1730, 0., 0.);
Trajectoire_droite(&trajectoire2,
990, 1730,
1150, 1730, 0, 0);
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);
}else{
Localisation_set(3000 - 45, 1870, M_PI);
Trajectoire_bezier(&trajectoire1,
3000 - 45, 1870, 3000 - 800, 1870,
3000 - 890, 1730, 3000 - 990, 1730, 0., 0.);
Trajectoire_droite(&trajectoire2, 3000 - 990, 1730, 3000 - 1150, 1730, 0, 0);
Localisation_set(3000 - 45, 1895, M_PI);
Trajectoire_droite(&trajectoire1, 3000-45, 1895, 3000- 1135, 1895, M_PI, M_PI);
Trajectoire_circulaire(&trajectoire2, 3000-1135, 1645, M_PI/2, M_PI, 250, M_PI, -M_PI/2);
Trajectoire_droite(&trajectoire3, 3000-1385, 1645, 3000-1385, 1580, 0, 0);
}
Trajectoire_composee_init(&trajectoire_composee);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
etat_sss = NINJA_GRENIER1;
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
etat_sss = SSS_AVANCE;
break;
case NINJA_GRENIER1:
Trajet_config(TRAJECT_CONFIG_STD);
case SSS_AVANCE:
Trajet_config(TRAJECT_CONFIG_RAPIDE);
if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2A;
if(couleur == COULEUR_JAUNE){
Trajectoire_bezier(&trajectoire1,
1150, 1730,
1030, 1730,
980, 1850,
980, 1970, 0., 0.);
}
etat_sss = SSS_DANCE;
}
break;
case NINJA_GRENIER2A:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2A_CALAGE;
}
break;
case NINJA_GRENIER2A_CALAGE:
if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2B;
Localisation_set_y(1975);
Localisation_set_x(980);
Localisation_set_angle(-M_PI/2);
if(couleur == COULEUR_JAUNE){
Trajectoire_bezier(&trajectoire1,
980, 1975,
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){
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_GRENIER2C_CALAGE;
}
break;
case NINJA_GRENIER2C_CALAGE:
if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2D;
Localisation_set_y(1975);
Localisation_set_x(1354);
Localisation_set_angle(-M_PI/2);
Trajectoire_droite(&trajectoire1,
1354, 1975,
1354, 1630., 0., 0.);
}
break;
case NINJA_GRENIER2D:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = ACTION_TERMINEE;
}
break;
case NINJA_FIN:
//Asser_Position_maintien();
case SSS_DANCE:
return ACTION_TERMINEE;
}
return ACTION_EN_COURS;
}
enum etat_action_t Strategie_Ninja_2(uint32_t step_ms, enum couleur_t couleur){
enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur){
static enum{
NINJA_INIT,
NINJA_GRENIER1A,
NINJA_GRENIER1B,
NINJA_GRENIER1B_RECALE,
NINJA_GRENIER1C,
NINJA_GRENIER1D,
NINJA_GRENIER2A,
NINJA_GRENIER2A_RECALE,
NINJA_GRENIER2B,
NINJA_GRENIER2C,
NINJA_FIN
} etat_sss = NINJA_INIT;
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 NINJA_INIT:
if(couleur == COULEUR_JAUNE){
Localisation_set(645, 1870, 0);
Trajet_config(TRAJECT_CONFIG_STD);
Trajectoire_bezier(&trajectoire1,
645, 1870,
935, 1870,
1025, 1920,
1315-50, 1920, 0., 0.);
}
etat_sss = NINJA_GRENIER1A;
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 NINJA_GRENIER1A:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER1B;
if(couleur == COULEUR_JAUNE){
Trajet_config(100,100);
Trajectoire_bezier(&trajectoire1,
1315-50, 1920,
1270-50, 1920,
1230-50, 1900,
1230-50, 1970, 0., 0.);
}
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 NINJA_GRENIER1B:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER1B_RECALE;
}
break;
case NINJA_GRENIER1B_RECALE:
if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER1C;
Trajet_config(100,100);
Localisation_set(1230-50, 1975, -M_PI/2);
if(couleur == COULEUR_JAUNE){
Trajectoire_bezier(&trajectoire1,
1230-50, 1970,
1230-50, 1900,
1255, 1725,
1195, 1725, 0., 0.);
}
}
break;
case NINJA_GRENIER1C:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER1D;
if(couleur == COULEUR_JAUNE){
Trajet_config(TRAJECT_CONFIG_STD);
Trajectoire_droite(&trajectoire1,
1195, 1725,
1050, 1725, 0., 0.);
}
}
break;
case NINJA_GRENIER1D:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2A;
Trajectoire_bezier(&trajectoire1,
1050, 1725,
1170, 1725,
1200, 1820,
1200, 1950,
0., 0.);
}
break;
case NINJA_GRENIER2A:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2A_RECALE;
}
break;
case NINJA_GRENIER2A_RECALE:
if(Strategie_recale(RECALE_SENS_ARRIERE, step_ms) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2B;
Trajet_config(100, 100);
Localisation_set(1200, 1975, -M_PI/2);
if(couleur == COULEUR_JAUNE){
Trajectoire_bezier(&trajectoire1,
1200, 1975,
1200, 1890,
1135, 1775,
1260, 1775, 0., 0.);
}
}
break;
case NINJA_GRENIER2B:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_sss = NINJA_GRENIER2C;
Trajet_config(TRAJECT_CONFIG_STD);
if(couleur == COULEUR_JAUNE){
Trajectoire_droite(&trajectoire1,
1260, 1775,
1400, 1775,
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();
case SSS_DANCE:
return ACTION_TERMINEE;
}
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;
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_test){
case TEST_INIT:
if(Strategie_recale(0, step_ms) == ACTION_TERMINEE){
etat_test = TEST_TERMINE;
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 TEST_TERMINE:
case SSS_DANCE:
return ACTION_TERMINEE;
}
}
return ACTION_EN_COURS;
}
enum etat_action_t Strategie_1_metre(uint32_t step_ms){
static enum{
TEST_INIT,
TEST_EN_COURS,
TEST_TERMINE
} etat_test = TEST_INIT;
static struct trajectoire_t trajectoire1;
Trajet_config(TRAJECT_CONFIG_STD);
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_test){
case TEST_INIT:
Localisation_set(0, 0, 0);
Trajectoire_droite(&trajectoire1,
0, 0,
1000, 0, 0., 0.);
etat_test = TEST_EN_COURS;
break;
case TEST_EN_COURS:
if(Strategie_parcourir_trajet(trajectoire1, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
etat_test = TEST_TERMINE;
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 TEST_TERMINE:
case SSS_DANCE:
return ACTION_TERMINEE;
}
}
return ACTION_EN_COURS;
}
}
void PAMI_dance(int identifiant){
switch (identifiant)

13
Strategie.h
View File

@ -1,4 +1,3 @@
#include "Asser_Moteurs.h"
#include "pico/stdlib.h"
#include "Localisation.h"
#include "Temps.h"
@ -13,8 +12,6 @@
#define TIRETTE 14
//#define CORR_ANGLE_DEPART_DEGREE (-1.145)
#define CORR_ANGLE_DEPART_DEGREE (0)
#define RECALE_SENS_AVANT 1
#define RECALE_SENS_ARRIERE 0
enum etat_action_t{
ACTION_EN_COURS,
@ -48,15 +45,13 @@ struct objectif_t{
ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_5} cible;
};
enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t);
enum etat_action_t Strategie_Ninja_2(uint32_t step_ms, enum couleur_t couleur);
enum etat_action_t Strategie_test(uint32_t step_ms);
enum etat_action_t Strategie_1_metre(uint32_t step_ms);
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);
enum etat_action_t Strategie_recale(int sens_avant, uint32_t step_ms);
// STRATEGIE_H
#endif

35
Strategie_deplacement.c
View File

@ -71,38 +71,3 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
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 = 800;
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;
}*/
if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER && trajet_trajectoire.type != TRAJECTOIRE_COMPOSEE){
if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){
if(abscisse >= 1 || distance_fin_trajectoire_mm < 0.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 col, row;
for(col=0; col<8; col++){
for(row=5; row<8; row++){
for(row=0; row<=3; row++){
if(min_distance > Results.distance_mm[col + 8*row]){
min_distance = Results.distance_mm[col + 8*row];
}

103
main.c
View File

@ -14,7 +14,6 @@
#include "Localisation.h"
#include "Moteurs.h"
#include "Strategie.h"
#include "Servomoteur.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
@ -38,10 +37,10 @@ uint16_t tension_batterie_lire(void);
void identifiant_init(void);
uint get_identifiant(void);
int get_tirette(void);
int get_couleur(void);
void gestion_PAMI(uint32_t step_ms, int * asser_pos);
void gestion_VL53L8CX(void);
const uint32_t step_ms=1;
@ -50,6 +49,7 @@ float distance1_mm=0, distance2_mm=0;
// DEBUG
extern float abscisse;
extern struct point_xyo_t point;
float vitesse;
VL53L8CX_Configuration Dev;
void main(void)
@ -58,6 +58,7 @@ void main(void)
bool fin_match = false;
stdio_init_all();
Temps_init();
@ -66,13 +67,10 @@ void main(void)
Trajet_init(get_identifiant());
i2c_maitre_init();
Servomoteur_Init();
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;
@ -83,40 +81,63 @@ void main(void)
gpio_init(TIRETTE_PIN);
gpio_set_dir(LED1PIN, GPIO_IN);
gpio_pull_up(TIRETTE_PIN);
multicore_launch_core1(gestion_affichage);
// TODO: A remettre - quand on aura récupéré un capteur
multicore_launch_core1(gestion_VL53L8CX);
//multicore_launch_core1(gestion_VL53L8CX);
sleep_ms(85000);
printf("Demarrage...\n");
float vitesse_init =300;
vitesse = vitesse_init;
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
while(get_tirette());
gpio_put(LED1PIN, 0);
// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
if(get_identifiant() == 3){
sleep_ms(90000);
}
temps_depart_ms = Temps_get_temps_ms();
while(1){
// Fin du match
if((Temps_get_temps_ms() -temps_depart_ms) >15000 || (fin_match == 1)){
Moteur_Stop();
while(1){
}
}
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
if(temps_ms % step_ms == 0){
QEI_update();
Localisation_gestion();
gestion_PAMI(step_ms, &asser_pos);
if(asser_pos){
AsserMoteur_Gestion(step_ms);
if(Strategie_super_star(step_ms, COULEUR_JAUNE) == ACTION_TERMINEE){
Asser_Position_maintien();
if(Asser_Position_panic_angle()){
fin_match=1;
}
}
AsserMoteur_Gestion(step_ms);
}
}
}
}
void gestion_PAMI(uint32_t step_ms, int * asser_pos){
void gestion_PAMI(uint32_t step_ms){
static enum{
PAMI_ATTENTE_TIRETTE,
PAMI_ATTENTE_TEMPO,
@ -125,24 +146,6 @@ void gestion_PAMI(uint32_t step_ms, int * asser_pos){
PAMI_DANCE,
}etat_PAMI;
static uint32_t temps_tirette, temps_mouvement;
enum etat_action_t etat_action=ACTION_EN_COURS;
// Identifiant 0 : PAMI Ninja
// Tempo Demarrage : 0 s
// Tempo action : 100 s
uint32_t tempo_demarrage_ms, tempo_action_ms;
if(get_identifiant() == 0){
tempo_demarrage_ms = 2000;
tempo_action_ms = 980000;
}else{
// Autres
// Tempo Demarrage : 85 s
// Tempo action : 15 s
tempo_demarrage_ms = 85000;
tempo_action_ms = 15000;
}
switch (etat_PAMI)
{
@ -157,7 +160,7 @@ void gestion_PAMI(uint32_t step_ms, int * asser_pos){
if(get_tirette() == 1 && (Temps_get_temps_ms() - temps_tirette > 1000)){
etat_PAMI = PAMI_FIN_TEMPO_MANUELLE;
}
if (Temps_get_temps_ms() - temps_tirette > tempo_demarrage_ms){
if (Temps_get_temps_ms() - temps_tirette > 85000){
etat_PAMI = PAMI_TRAJECTOIRE;
temps_mouvement = Temps_get_temps_ms();
}
@ -171,33 +174,21 @@ void gestion_PAMI(uint32_t step_ms, int * asser_pos){
break;
case PAMI_TRAJECTOIRE:
if(Temps_get_temps_ms() - temps_mouvement > tempo_action_ms){
etat_PAMI = PAMI_DANCE;
}
if(etat_action == ACTION_EN_COURS){
switch (get_identifiant())
{
case 0: etat_action = Strategie_Ninja_2(step_ms, get_couleur()); break;
case 1: etat_action = Strategie_1_metre(step_ms); break;
case 7: etat_action = Strategie_test(step_ms); break;
default: etat_action = ACTION_TERMINEE; break;
}
if(etat_action == ACTION_TERMINEE){
Moteur_Stop();
*asser_pos = 0;
}
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();
*asser_pos = 0;
if(Temps_get_temps_ms() - temps_tirette > 15000){
while(1){
PAMI_dance(get_identifiant());
}
while(1){
PAMI_dance(get_identifiant());
}
break;
default:
break;
@ -219,7 +210,7 @@ void gestion_VL53L8CX(void){
if(isReady){
VL53L8_lecture( &Dev, &Results);
VL53L8_min_distance(Results, &distance_obstacle);
//Trajet_set_obstacle_mm(distance_obstacle);
Trajet_set_obstacle_mm(distance_obstacle);
}
affichage();
}
@ -243,7 +234,7 @@ void affichage(void){
printf(">pos_angle:%.1f\n", Localisation_get().angle_radian);
printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
printf(">abscisse:%lu:%f\n",temps_ms, abscisse);
printf(">abscisse:%f\n",abscisse);
struct position_t position_actuelle;
position_actuelle = Localisation_get();
@ -251,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:%lu:%d\n>id:%lu:%d\n>Tirette:%lu:%d\n", temps_ms, get_couleur(),temps_ms, get_identifiant(),temps_ms, get_tirette());
printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), get_identifiant(), get_tirette());
}
void tension_batterie_init(void){