From 5b37406e9ce88cdb9bda66795864aad064e62476 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Fri, 15 Mar 2024 23:45:05 +0100
Subject: [PATCH] Lecture des codeurs: OK
---
CMakeLists.txt | 4 +
QEI.c | 107 ++++++++++++++++++++++++++
QEI.h | 16 ++++
Readme.md | 18 ++++-
main.c | 7 +-
quadrature_encoder.pio | 165 +++++++++++++++++++++++++++++++++++++++++
6 files changed, 313 insertions(+), 4 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 7171dd2..232101a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,13 +12,17 @@ pico_sdk_init()
add_executable(Mon_Projet
main.c
+ QEI.c
)
+pico_generate_pio_header(Mon_Projet ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
+
target_include_directories(Mon_Projet PRIVATE Mon_Projet_ULD_API/inc/)
target_link_libraries(Mon_Projet
hardware_i2c
hardware_uart
+ hardware_pio
pico_stdlib
pico_multicore
)
diff --git a/QEI.c b/QEI.c
new file mode 100644
index 0000000..4a5e431
--- /dev/null
+++ b/QEI.c
@@ -0,0 +1,107 @@
+#include <stdio.h>
+#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
+ * ***/
+
+// 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 (95.4929658551372f)
+#define ASSERMOTEUR_GAIN_P 160
+#define ASSERMOTEUR_GAIN_I .80f
+
+struct QEI_t QEI_A, QEI_B;
+
+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");
+ }
+ // 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;
+ }
+
+}
+
+/// @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) / (float)IMPULSION_PAR_MM;
+}
\ No newline at end of file
diff --git a/QEI.h b/QEI.h
new file mode 100644
index 0000000..b2b132c
--- /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(void);
+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 cbe3ef5..1f05600 100644
--- a/Readme.md
+++ b/Readme.md
@@ -1,4 +1,18 @@
-Projet modèle pour le Rpi Pico (RP2040)
+PAMI 2024 - Poivron Robotique
=======================================
-Ce projet est un example pour le RPI Pico, tentant d'être le plus prêt à l'emploi possible.
+Code du PAMI 2024 de l'équipe Poivron Robotique.
+
+La cart e contien les éléments suivants :
+
+* Microcontrôleur Raspberry Pi Pico
+* Connecteur pour l’arrêt d’urgence
+* 2 prises moteurs (pilotés par un L293D)
+* 2 prises codeurs
+* 1 prise Gyroscope (L3GD20H)
+* 1 prise I2C pour du TOF
+* 1 prise "choix couleur"
+* 1 prise tirette
+* Surveillance tension batterie
+* 1 LED
+* 3 Dip Switch
diff --git a/main.c b/main.c
index f79df64..aae8c2a 100644
--- a/main.c
+++ b/main.c
@@ -5,12 +5,15 @@
*/
#include "pico/stdlib.h"
#include <stdio.h>
+#include "QEI.h"
void main(void)
{
stdio_init_all();
+ QEI_init();
while(1){
- printf("Exemple\n");
- sleep_ms(1000);
+ QEI_update();
+ printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );
+ sleep_ms(10);
}
}
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--, <next addr>"
+ ; 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);
+}
+
+%}
+