From 4bebc91c81833a7b73387b939a8afe2f4ce17d20 Mon Sep 17 00:00:00 2001 From: Samuel Date: Wed, 15 Jan 2025 11:06:48 +0100 Subject: [PATCH] Code de demonstration de la lecture des codeurs --- CMakeLists.txt | 25 ++++--- Moteurs.h | 1 + QEI.c | 86 +++++++++++++++++++++ QEI.h | 16 ++++ Readme.md | 6 +- main.c | 23 ++++++ quadrature_encoder.pio | 165 +++++++++++++++++++++++++++++++++++++++++ 7 files changed, 308 insertions(+), 14 deletions(-) create mode 100644 QEI.c create mode 100644 QEI.h create mode 100644 quadrature_encoder.pio diff --git a/CMakeLists.txt b/CMakeLists.txt index 8aceef8..4dda132 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.13) include(pico_sdk_import.cmake) -project(PAMI_Cours_Moteurs C CXX ASM) +project(PAMI_Cours_Codeurs C CXX ASM) set(CMAKE_C_STNDARD 11) set(CMAKE_CXX_STANDARD 17) @@ -10,30 +10,33 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR}) pico_sdk_init() -add_executable(PAMI_Cours_Moteurs +add_executable(PAMI_Cours_Codeurs main.c - Moteurs.c + QEI.c Teleplot.c Temps.c ) -target_include_directories(PAMI_Cours_Moteurs PRIVATE ${CMAKE_CURRENT_LIST_DIR}) +pico_generate_pio_header(PAMI_Cours_Codeurs ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) -target_link_libraries(PAMI_Cours_Moteurs - hardware_uart +target_include_directories(PAMI_Cours_Codeurs PRIVATE ${CMAKE_CURRENT_LIST_DIR}) + +target_link_libraries(PAMI_Cours_Codeurs + hardware_uart + hardware_pio hardware_pwm pico_stdlib pico_multicore pico_cyw43_arch_lwip_poll ) -pico_enable_stdio_usb(PAMI_Cours_Moteurs 1) -pico_enable_stdio_uart(PAMI_Cours_Moteurs 1) +pico_enable_stdio_usb(PAMI_Cours_Codeurs 1) +pico_enable_stdio_uart(PAMI_Cours_Codeurs 1) -pico_add_extra_outputs(PAMI_Cours_Moteurs) +pico_add_extra_outputs(PAMI_Cours_Codeurs) add_custom_target(Flash - DEPENDS PAMI_Cours_Moteurs - COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_Moteurs.uf2 + DEPENDS PAMI_Cours_Codeurs + COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_Codeurs.uf2 ) diff --git a/Moteurs.h b/Moteurs.h index d34ef7a..1e42206 100644 --- a/Moteurs.h +++ b/Moteurs.h @@ -10,6 +10,7 @@ enum t_moteur { #define MOTEUR_COMMANDE_MAX 25000 + void Moteur_init(void); void Moteur_set_commande(enum t_moteur moteur, int32_t vitesse ); void Moteur_stop(void); diff --git a/QEI.c b/QEI.c new file mode 100644 index 0000000..8e13bc0 --- /dev/null +++ b/QEI.c @@ -0,0 +1,86 @@ +#include +#include "pico/stdlib.h" +#include "hardware/pio.h" +#include "hardware/timer.h" +#include "QEI.h" +#include "quadrature_encoder.pio.h" + + +// C'est ici que se fait la conversion en mm +#define IMPULSION_PAR_MM (1.f) + +float impulsion_par_mm; + + +struct QEI_t QEI_A, QEI_B; + +bool QEI_est_init = false; + +PIO pio_QEI = pio0; + + +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); + + QEI_A.value=0; + QEI_B.value=0; + QEI_est_init=true; + } + impulsion_par_mm = IMPULSION_PAR_MM; +} + +/// @brief Lit les modules QEI et stock l'écart en cette lecture et la lecture précédente. +void QEI_update(void){ + + int old_value; + + old_value = QEI_A.value; + QEI_A.value = quadrature_encoder_get_count(pio_QEI, 0); + QEI_A.delta = QEI_A.value - old_value; + + old_value = QEI_B.value; + QEI_B.value = quadrature_encoder_get_count(pio_QEI, 1); + QEI_B.delta = QEI_B.value - old_value; + +} + +/// @brief Renvoi le nombre d'impulsion du module QEI depuis la lecture précédente +/// Les signe sont inversés (sauf A) car le reducteur inverse le sens de rotation. +/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électornique +/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME) +/// @return Nombre d'impulsion calculé lors du dernier appel de la function QEI_Update() +int QEI_get(enum QEI_name_t qei){ + switch (qei) + { + case QEI_A_NAME: + return QEI_A.delta; + break; + + case QEI_B_NAME: + return -QEI_B.delta; + break; + + default: + break; + } +} + +/// @brief Renvoi la distance parcourue en mm depuis la lecture précédente +/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME) +/// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update() +float QEI_get_mm(enum QEI_name_t qei){ + return ((float) QEI_get(qei)) / impulsion_par_mm; +} \ No newline at end of file diff --git a/QEI.h b/QEI.h new file mode 100644 index 0000000..3e40105 --- /dev/null +++ b/QEI.h @@ -0,0 +1,16 @@ +struct QEI_t{ + int value; + int delta; +}; + +enum QEI_name_t{ + QEI_A_NAME=0, + QEI_B_NAME=1, +}; + +extern struct QEI_t QEI_A, QEI_B, QEI_C; + +void QEI_update(void); +void QEI_init(); +int QEI_get(enum QEI_name_t qei); +float QEI_get_mm(enum QEI_name_t qei); \ No newline at end of file diff --git a/Readme.md b/Readme.md index c66331e..15b17aa 100644 --- a/Readme.md +++ b/Readme.md @@ -1,10 +1,10 @@ -De l’art de déplacer un robot avec classe - Commande des moteurs +De l’art de déplacer un robot avec classe - Lecture des codeurs ================================================================ Le but est de vous proposer une série d’articles vous présentant les arcanes du déplacement d’un robot, en fournissant à chaque étape une idée de démonstration et les principales pistes de débogage. Les articles sont disponibles ici : [Wiki Eurobot](https://www.eurobot.org/wiki/fr/informatics/de_l_art_de_deplacer_son_robot_avec_classe) -Commande des moteurs +Lecture des codeurs ------------------- -Ajout des fichiers pour gérer les moteurs. Le code principal fait évoluer la vitesse des moteurs progressivement en fonction du temps. +Ajout des fichiers pour gérer les codeurs. Le code principal fait évoluer la vitesse des moteurs progressivement en fonction du temps et envoi la valeurs des codeurs. diff --git a/main.c b/main.c index 0e520f0..22abfb3 100644 --- a/main.c +++ b/main.c @@ -5,6 +5,7 @@ */ #include "pico/stdlib.h" #include "Moteurs.h" +#include "QEI.h" #include "Teleplot.h" #include "Temps.h" #include @@ -14,6 +15,7 @@ void setup(void); void loop(void); int valeur = 1; +uint32_t m_temps_ms; void main(void) { @@ -28,10 +30,31 @@ void setup(void){ Teleplot_init(); Temps_init(); Moteur_init(); + QEI_init(); } void loop(void){ + static float cA_distance = 0, cB_distance = 0; float t = (float) Temps_get_temps_ms() / 1000. ; Moteur_set_commande(MOTEUR_A, sin(t * 3.14) * MOTEUR_COMMANDE_MAX) ; Moteur_set_commande(MOTEUR_B, sin(t * 3.14) * MOTEUR_COMMANDE_MAX) ; + if(m_temps_ms != Temps_get_temps_ms()){ + m_temps_ms = Temps_get_temps_ms(); + + if(m_temps_ms % 1 == 0){ // Fréquence d'actualisation des codeurs (1 ms) + QEI_update(); + cA_distance += QEI_get_mm(QEI_A_NAME); + cB_distance += QEI_get_mm(QEI_B_NAME); + } + + if(m_temps_ms % 20 == 0){ // Fréquence d'affichage (20 ms) + Teleplot_fige_temps(); + Teleplot_add_variable_float_2decimal("codeur_A_vitesse", QEI_get_mm(QEI_A_NAME)); + Teleplot_add_variable_float_2decimal("codeur_B_vitesse", QEI_get_mm(QEI_B_NAME)); + Teleplot_add_variable_float_2decimal("codeur_A_distance", cA_distance); + Teleplot_add_variable_float_2decimal("codeur_B_distance", cA_distance); + Teleplot_envoie_tampon(); + } + } + } \ No newline at end of file diff --git a/quadrature_encoder.pio b/quadrature_encoder.pio new file mode 100644 index 0000000..d245d4b --- /dev/null +++ b/quadrature_encoder.pio @@ -0,0 +1,165 @@ +; +; Copyright (c) 2021 pmarques-dev @ github +; +; SPDX-License-Identifier: BSD-3-Clause +; + +.program quadrature_encoder + +; this code must be loaded into address 0, but at 29 instructions, it probably +; wouldn't be able to share space with other programs anyway +.origin 0 + + +; the code works by running a loop that continuously shifts the 2 phase pins into +; ISR and looks at the lower 4 bits to do a computed jump to an instruction that +; does the proper "do nothing" | "increment" | "decrement" action for that pin +; state change (or no change) + +; ISR holds the last state of the 2 pins during most of the code. The Y register +; keeps the current encoder count and is incremented / decremented according to +; the steps sampled + +; writing any non zero value to the TX FIFO makes the state machine push the +; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case +; sampling loop takes 14 cycles, so this program is able to read step rates up +; to sysclk / 14 (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec) + + +; 00 state + JMP update ; read 00 + JMP decrement ; read 01 + JMP increment ; read 10 + JMP update ; read 11 + +; 01 state + JMP increment ; read 00 + JMP update ; read 01 + JMP update ; read 10 + JMP decrement ; read 11 + +; 10 state + JMP decrement ; read 00 + JMP update ; read 01 + JMP update ; read 10 + JMP increment ; read 11 + +; to reduce code size, the last 2 states are implemented in place and become the +; target for the other jumps + +; 11 state + JMP update ; read 00 + JMP increment ; read 01 +decrement: + ; note: the target of this instruction must be the next address, so that + ; the effect of the instruction does not depend on the value of Y. The + ; same is true for the "JMP X--" below. Basically "JMP Y--, " + ; is just a pure "decrement Y" instruction, with no other side effects + JMP Y--, update ; read 10 + + ; this is where the main loop starts +.wrap_target +update: + ; we start by checking the TX FIFO to see if the main code is asking for + ; the current count after the PULL noblock, OSR will have either 0 if + ; there was nothing or the value that was there + SET X, 0 + PULL noblock + + ; since there are not many free registers, and PULL is done into OSR, we + ; have to do some juggling to avoid losing the state information and + ; still place the values where we need them + MOV X, OSR + MOV OSR, ISR + + ; the main code did not ask for the count, so just go to "sample_pins" + JMP !X, sample_pins + + ; if it did ask for the count, then we push it + MOV ISR, Y ; we trash ISR, but we already have a copy in OSR + PUSH + +sample_pins: + ; we shift into ISR the last state of the 2 input pins (now in OSR) and + ; the new state of the 2 pins, thus producing the 4 bit target for the + ; computed jump into the correct action for this state + MOV ISR, NULL + IN OSR, 2 + IN PINS, 2 + MOV PC, ISR + + ; the PIO does not have a increment instruction, so to do that we do a + ; negate, decrement, negate sequence +increment: + MOV X, !Y + JMP X--, increment_cont +increment_cont: + MOV Y, !X +.wrap ; the .wrap here avoids one jump instruction and saves a cycle too + + + +% c-sdk { + +#include "hardware/clocks.h" +#include "hardware/gpio.h" + +// max_step_rate is used to lower the clock of the state machine to save power +// if the application doesn't require a very high sampling rate. Passing zero +// will set the clock to the maximum, which gives a max step rate of around +// 8.9 Msteps/sec at 125MHz + +static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate) +{ + pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); + gpio_pull_up(pin); + gpio_pull_up(pin + 1); + + pio_sm_config c = quadrature_encoder_program_get_default_config(offset); + sm_config_set_in_pins(&c, pin); // for WAIT, IN + sm_config_set_jmp_pin(&c, pin); // for JMP + // shift to left, autopull disabled + sm_config_set_in_shift(&c, false, false, 32); + // don't join FIFO's + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); + + // passing "0" as the sample frequency, + if (max_step_rate == 0) { + sm_config_set_clkdiv(&c, 1.0); + } else { + // one state machine loop takes at most 14 cycles + float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate); + sm_config_set_clkdiv(&c, div); + } + + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + + +// When requesting the current count we may have to wait a few cycles (average +// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple +// encoders, we may request them all in one go and then fetch them all, thus +// avoiding doing the wait multiple times. If we are reading just one encoder, +// we can use the "get_count" function to request and wait + +static inline void quadrature_encoder_request_count(PIO pio, uint sm) +{ + pio->txf[sm] = 1; +} + +static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm) +{ + while (pio_sm_is_rx_fifo_empty(pio, sm)) + tight_loop_contents(); + return pio->rxf[sm]; +} + +static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm) +{ + quadrature_encoder_request_count(pio, sm); + return quadrature_encoder_fetch_count(pio, sm); +} + +%} +