From bde7f64555f48ed4826decbf3b9c6a43794e7601 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 28 Apr 2024 21:03:01 +0200 Subject: [PATCH] Asservissement des moteurs fonctionnel --- Asser_Moteurs.c | 16 ++-------- CMakeLists.txt | 1 + QEI.c | 49 +++++++++++------------------- main.c | 80 ++++++++++++++++++++++++++++++++++++++----------- 4 files changed, 83 insertions(+), 63 deletions(-) diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index 300a9e8..808989d 100644 --- a/Asser_Moteurs.c +++ b/Asser_Moteurs.c @@ -2,20 +2,8 @@ #include "Moteurs.h" #include "Asser_Moteurs.h" -/*** C'est ici que se fait la conversion en mm - * ***/ - -// Roues 60 mm de diamètre, 188,5 mm de circonférence -// Réduction Moteur 30:1 -// Réduction poulie 16:12 -// Nombre d'impulsions par tour moteur : 200 -// Nombre d'impulsions par tour réducteur : 6000 -// Nombre d'impulsions par tour de roue : 8000 -// Impulsion / mm : 42,44 - -#define IMPULSION_PAR_MM (1.f) -#define ASSERMOTEUR_GAIN_P 2.f -#define ASSERMOTEUR_GAIN_I 0.f +#define ASSERMOTEUR_GAIN_P 300000.f +#define ASSERMOTEUR_GAIN_I 30000.f float consigne_mm_s[3]; // Consigne de vitesse (en mm/s) float commande_I[3]; // Terme integral diff --git a/CMakeLists.txt b/CMakeLists.txt index 4030050..7707fc3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ pico_generate_pio_header(Mon_Projet ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder target_include_directories(Mon_Projet PRIVATE Mon_Projet_ULD_API/inc/) target_link_libraries(Mon_Projet + hardware_adc hardware_i2c hardware_pwm hardware_pio diff --git a/QEI.c b/QEI.c index af9d384..149d115 100644 --- a/QEI.c +++ b/QEI.c @@ -17,9 +17,8 @@ // Nombre d'impulsions par tour de roue : 8000 // Impulsion / mm : 42,44 -#define IMPULSION_PAR_MM (1.f) -#define ASSERMOTEUR_GAIN_P 2 -#define ASSERMOTEUR_GAIN_I .0f +#define IMPULSION_PAR_MM (12.45f) + struct QEI_t QEI_A, QEI_B; @@ -27,38 +26,26 @@ bool QEI_est_init = false; PIO pio_QEI = pio0; -const uint CODEUR_1_A = 26; -const uint CODEUR_1_B = 27; void QEI_init(){ // Initialisation des 3 modules QEI // Chaque module QEI sera dans une machine à état du PIO 0 if(!QEI_est_init){ + // Offset le début du programme + // Si ce n'est pas 0, le programme ne marchera pas + uint offset = pio_add_program(pio_QEI, &quadrature_encoder_program); + if(offset != 0){ + printf("PIO init error: offset != 0"); + } + // Initialisation des "machines à états" : + // 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, 11, 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, 2, 0); - // Offset le début du programme - // Si ce n'est pas 0, le programme ne marchera pas - uint offset = pio_add_program(pio_QEI, &quadrature_encoder_program); - if(offset != 0){ - printf("PIO init error: offset != 0"); - } - // bizarrement, il faut initialiser les boches en entrée pour les GPIO 26 et 27. - // Probablement car elle sont en analogique par défaut... - /*gpio_init(CODEUR_1_A); - gpio_set_dir(CODEUR_1_A, GPIO_IN); - - gpio_init(CODEUR_1_B); - gpio_set_dir(CODEUR_1_B, GPIO_IN);*/ - - // Initialisation des "machines à états" : - // QEI1 : broche 31 et 32 - pio : pio0, sm : 0, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer - // QEI1 : !!! Attention, il faudra modifier la carte élec !!! => Fait. - quadrature_encoder_program_init(pio_QEI, 0, offset, 2, 0); - // QEI2 : broche 26 et 27 - pio : pio0, sm : 1, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer - quadrature_encoder_program_init(pio_QEI, 1, offset, 11, 0); - - QEI_A.value=0; - QEI_B.value=0; - QEI_est_init=true; + QEI_A.value=0; + QEI_B.value=0; + QEI_est_init=true; } } @@ -87,11 +74,11 @@ int QEI_get(enum QEI_name_t qei){ switch (qei) { case QEI_A_NAME: - return -QEI_A.delta; + return QEI_A.delta; break; case QEI_B_NAME: - return QEI_B.delta; + return -QEI_B.delta; break; default: diff --git a/main.c b/main.c index 653ae26..cd50c96 100644 --- a/main.c +++ b/main.c @@ -5,6 +5,7 @@ */ #include "pico/stdlib.h" #include "pico/multicore.h" +#include "hardware/adc.h" #include "hardware/pwm.h" #include "Asser_Moteurs.h" #include "Moteurs.h" @@ -13,43 +14,53 @@ #include "QEI.h" #define LED1PIN 20 -#define M1_SENS1 7 -#define M1_SENS2 13 -#define M1_VITESSE 27 //5B -#define M2_SENS1 10 -#define M2_SENS2 5 -#define M2_VITESSE 9 //4B void affichage(void); +void tension_batterie_init(void); +uint16_t tension_batterie_lire(void); + +void identifiant_init(void); +uint identifiant_lire(void); + +uint32_t step_ms=1; +float distance1_mm=0, distance2_mm=0; void main(void) { int ledpower = 500; + stdio_init_all(); AsserMoteur_Init(); Temps_init(); + tension_batterie_init(); + identifiant_init(); uint32_t temps_ms = Temps_get_temps_ms(); - uint32_t step_ms=1; + gpio_init(LED1PIN); - gpio_set_dir(LED1PIN, GPIO_OUT ); + gpio_set_dir(LED1PIN, GPIO_OUT ); gpio_put(LED1PIN, 1); - AsserMoteur_setConsigne_mm_s(MOTEUR_A, 6000); - AsserMoteur_setConsigne_mm_s(MOTEUR_B, 6000); + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100.); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100.); + multicore_launch_core1(affichage); - Moteur_SetVitesse(MOTEUR_A, 16000); - Moteur_SetVitesse(MOTEUR_B, 16000); + Moteur_SetVitesse(MOTEUR_A, 000); + Moteur_SetVitesse(MOTEUR_B, 000); + while(1){ if(temps_ms != Temps_get_temps_ms()){ temps_ms = Temps_get_temps_ms(); - QEI_update(); - AsserMoteur_Gestion(step_ms); + if(temps_ms % step_ms == 0){ + QEI_update(); + AsserMoteur_Gestion(step_ms); + } if(temps_ms % 100 == 0){ + identifiant_lire(); //printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) ); } } @@ -61,11 +72,44 @@ void main(void) void affichage(void){ while(1){ + printf(">c1_mm:%f\n>c2_mm:%f\n", QEI_get_mm(QEI_A_NAME), QEI_get_mm(QEI_B_NAME) ); - printf(">c1:%d\n>c1_mm:%f\n", QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME) ); - //printf(">c2:%d\n>c2_mm:%f\n", QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME) ); - printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1), AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1) ); - printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) ); + /*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) ); + printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ sleep_ms(100); } +} + +void tension_batterie_init(void){ + adc_init(); + adc_gpio_init(28); // Analog_2 + adc_select_input(2); + adc_run(1); // Free running mode +} + +uint16_t tension_batterie_lire(){ + uint16_t result = (uint16_t) adc_hw->result; + const float conversion_factor = 3.3f / (1 << 12); + float v_bat = result * conversion_factor * 11.; + + return result; +} + +void identifiant_init(){ + gpio_init(21); + gpio_init(22); + gpio_init(26); + gpio_pull_up(21); + gpio_pull_up(22); + gpio_pull_up(26); + gpio_set_dir(21, GPIO_IN); + gpio_set_dir(22, GPIO_IN); + gpio_set_dir(26, GPIO_IN); +} + + +/// @brief !! Arg la GPIO 26 ne répond pas ! +/// @return +uint identifiant_lire(){ + return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26); } \ No newline at end of file