diff --git a/.vscode/settings.json b/.vscode/settings.json index 241c813..be8f621 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -15,6 +15,7 @@ "strategie_deplacement.h": "c", "servomoteur.h": "c", "moteurs.h": "c", - "messagerie_applicative.h": "c" + "messagerie_applicative.h": "c", + "config_robot.h": "c" } } \ No newline at end of file diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index 59263a5..749f44a 100644 --- a/Asser_Moteurs.c +++ b/Asser_Moteurs.c @@ -4,12 +4,16 @@ #include "Asser_Moteurs.h" // Paramètres pour PAMI -//#define ASSERMOTEUR_GAIN_P 30000.f -//#define ASSERMOTEUR_GAIN_I 3000.f +#ifdef ROBOT_TYPE_PAMI +#define ASSERMOTEUR_GAIN_P 30000.f +#define ASSERMOTEUR_GAIN_I 3000.f +#endif // Paramètre Robot 2026 +#ifdef ROBOT_PROPULSION_2026 #define ASSERMOTEUR_GAIN_P 150.f #define ASSERMOTEUR_GAIN_I 1.f +#endif float consigne_mm_s[3]; // Consigne de vitesse (en mm/s) diff --git a/Moteurs.c b/Moteurs.c index f6a3df2..0ed1141 100644 --- a/Moteurs.c +++ b/Moteurs.c @@ -1,3 +1,4 @@ +#include "config_robot.h" #include "hardware/pwm.h" #include "Moteurs.h" @@ -90,7 +91,11 @@ void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){ switch(moteur){ case MOTEUR_A: pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse); +#ifdef ROBOT_PROPULSION_2026 if(vitesse > 0){ +#else + if(vitesse < 0){ +#endif gpio_put(M1_SENS1, 1); gpio_put(M1_SENS2, 0); }else{ @@ -101,7 +106,11 @@ void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){ case MOTEUR_B: pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse); +#ifdef ROBOT_PROPULSION_2026 if(vitesse < 0){ +#else + if(vitesse > 0){ +#endif gpio_put(M2_SENS1, 1); gpio_put(M2_SENS2, 0); }else{ diff --git a/QEI.c b/QEI.c index b167691..eda8562 100644 --- a/QEI.c +++ b/QEI.c @@ -1,3 +1,4 @@ +#include "config_robot.h" #include #include "pico/stdlib.h" #include "hardware/pio.h" @@ -19,6 +20,7 @@ #define IMPULSION_PAR_MM_50_1 (12.45f) #define IMPULSION_PAR_MM_30_1 (7.47f) +#define IMPULSION_PAR_MM_robot_2026 (7.72f) float impulsion_par_mm; @@ -41,19 +43,30 @@ void QEI_init(int identifiant){ printf("PIO init error: offset != 0"); } // Initialisation des "machines à états" : +#ifdef ROBOT_PROPULSION_2026 + // QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 14 et 15, clock div : 0 pour commencer + quadrature_encoder_program_init(pio_QEI, 0, offset, 14, 0); + // QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 16 et 17, clock div : 0 pour commencer + quadrature_encoder_program_init(pio_QEI, 1, offset, 16, 0); +#else // QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer quadrature_encoder_program_init(pio_QEI, 0, offset, 14, 0); // QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer quadrature_encoder_program_init(pio_QEI, 1, offset, 16, 0); +#endif QEI_A.value=0; QEI_B.value=0; QEI_est_init=true; } +#ifdef ROBOT_PROPULSION_2026 + impulsion_par_mm = IMPULSION_PAR_MM_robot_2026; +#else impulsion_par_mm = IMPULSION_PAR_MM_50_1; if(identifiant == 0 || identifiant >= 4){ impulsion_par_mm = IMPULSION_PAR_MM_30_1; } +#endif } @@ -85,7 +98,11 @@ int QEI_get(enum QEI_name_t qei){ break; case QEI_B_NAME: +#ifdef ROBOT_PROPULSION_2026 return QEI_B.delta; +#else + return -QEI_B.delta; +#endif break; default: diff --git a/main.c b/main.c index 236167d..36075ab 100644 --- a/main.c +++ b/main.c @@ -48,7 +48,11 @@ int get_couleur(void); void gestion_PAMI(uint32_t step_ms, int * asser_pos); void gestion_VL53L8CX(void); +#ifdef ROBOT_PROPULSION_2026 const uint32_t step_ms=10; +#else +const uint32_t step_ms=1; +#endif float distance1_mm=0, distance2_mm=0; // DEBUG