diff --git a/Asser_Position.c b/Asser_Position.c index 7aa2c5b..0f9b7a9 100644 --- a/Asser_Position.c +++ b/Asser_Position.c @@ -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); diff --git a/CMakeLists.txt b/CMakeLists.txt index ea24613..244775a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.13) include(pico_sdk_import.cmake) -project(Mon_Projet C CXX ASM) +project(PAMI2026a 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(Mon_Projet +add_executable(PAMI2026a Asser_Position.c Asser_Moteurs.c Commande_vitesse.c @@ -39,11 +39,11 @@ add_executable(Mon_Projet 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_i2c hardware_pwm @@ -52,12 +52,12 @@ target_link_libraries(Mon_Projet pico_multicore ) -pico_enable_stdio_usb(Mon_Projet 1) -pico_enable_stdio_uart(Mon_Projet 1) +pico_enable_stdio_usb(PAMI2026a 1) +pico_enable_stdio_uart(PAMI2026a 1) -pico_add_extra_outputs(Mon_Projet) +pico_add_extra_outputs(PAMI2026a) add_custom_target(Flash - DEPENDS Mon_Projet - COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/Mon_Projet.uf2 + DEPENDS PAMI2026a + COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI2026a.uf2 ) diff --git a/QEI.c b/QEI.c index c95dbe3..af7be13 100644 --- a/QEI.c +++ b/QEI.c @@ -51,7 +51,7 @@ void QEI_init(int identifiant){ QEI_est_init=true; } impulsion_par_mm = IMPULSION_PAR_MM_50_1; - if(identifiant == 0 || identifiant >= 4){ + if(identifiant >= 4){ impulsion_par_mm = IMPULSION_PAR_MM_30_1; } diff --git a/Strategie.c b/Strategie.c index 71b593f..7e07a1b 100644 --- a/Strategie.c +++ b/Strategie.c @@ -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{ - SSS_INIT, - SSS_AVANCE, - SSS_FIN - } etat_sss = SSS_INIT; + NINJA_INIT, + NINJA_GRENIER1, + NINJA_GRENIER2A, + NINJA_GRENIER2B, + NINJA_GRENIER2C, + NINJA_FIN + } etat_sss = NINJA_INIT; static struct trajectoire_t trajectoire_composee; static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3; - - switch(etat_sss){ - case SSS_INIT: + case NINJA_INIT: if(couleur == COULEUR_JAUNE){ - 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); + 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); + }else{ - 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); + 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); } 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; + etat_sss = NINJA_GRENIER1; break; - case SSS_AVANCE: + case NINJA_GRENIER1: Trajet_config(TRAJECT_CONFIG_RAPIDE); 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; - case SSS_FIN: - Asser_Position_maintien(); + case NINJA_GRENIER2A: + 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_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_1(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, 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; + switch(etat_test){ + case TEST_INIT: + if(Strategie_recale(0, step_ms) == ACTION_TERMINEE){ + etat_test = TEST_TERMINE; } break; - case SSS_DANCE: + case TEST_TERMINE: 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){ switch (identifiant) { diff --git a/Strategie.h b/Strategie.h index eb11610..51e7553 100644 --- a/Strategie.h +++ b/Strategie.h @@ -13,6 +13,8 @@ #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, @@ -46,13 +48,13 @@ struct objectif_t{ 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_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); +enum etat_action_t Strategie_Ninja(uint32_t step_ms, enum couleur_t); +enum etat_action_t Strategie_test(uint32_t step_ms); + 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 diff --git a/Strategie_deplacement.c b/Strategie_deplacement.c index f5200a5..215375b 100644 --- a/Strategie_deplacement.c +++ b/Strategie_deplacement.c @@ -71,3 +71,38 @@ 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 = 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; + +} \ No newline at end of file diff --git a/Trajet.c b/Trajet.c index 7808b21..a39e619 100644 --- a/Trajet.c +++ b/Trajet.c @@ -103,7 +103,7 @@ int Trajet_terminee(float abscisse){ 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){ return 1; } diff --git a/VL53L8_2024.c b/VL53L8_2024.c index cdde1f5..baf79c7 100644 --- a/VL53L8_2024.c +++ b/VL53L8_2024.c @@ -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=0; row<=3; row++){ + for(row=5; row<8; row++){ if(min_distance > Results.distance_mm[col + 8*row]){ min_distance = Results.distance_mm[col + 8*row]; } diff --git a/main.c b/main.c index 010b902..372a7e6 100644 --- a/main.c +++ b/main.c @@ -89,12 +89,8 @@ void main(void) // TODO: A remettre - quand on aura récupéré un capteur - if(get_identifiant() != 0){ - multicore_launch_core1(gestion_VL53L8CX); - }else{ - multicore_launch_core1(gestion_affichage); - } - + multicore_launch_core1(gestion_VL53L8CX); + printf("Demarrage...\n"); @@ -129,7 +125,7 @@ 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; + enum etat_action_t etat_action=ACTION_EN_COURS; 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){ etat_PAMI = PAMI_DANCE; } - 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 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; + if(etat_action == ACTION_EN_COURS){ + switch (get_identifiant()) + { + case 0: etat_action = Strategie_Ninja(step_ms, get_couleur()); 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; + } } break; case PAMI_DANCE: Moteur_Stop(); + *asser_pos = 0; if(Temps_get_temps_ms() - temps_tirette > 15000){ while(1){ PAMI_dance(get_identifiant());