Code de demonstration de la lecture des codeurs

CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.13)
 
 include(pico_sdk_import.cmake)
 
-project(PAMI_Cours C CXX ASM)
+project(PAMI_Cours_Codeurs C CXX ASM)
 set(CMAKE_C_STNDARD 11)
 set(CMAKE_CXX_STANDARD 17)
 
@@ -10,27 +10,33 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR})
 
 pico_sdk_init()
 
-add_executable(PAMI_Cours
+add_executable(PAMI_Cours_Codeurs
   main.c
+  Moteurs.c
+  QEI.c
   Teleplot.c
+  Temps.c
 )
 
-target_include_directories(PAMI_Cours PRIVATE ${CMAKE_CURRENT_LIST_DIR})
+pico_generate_pio_header(PAMI_Cours_Codeurs ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
 
-target_link_libraries(PAMI_Cours
-  hardware_i2c 
-  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 1)
-pico_enable_stdio_uart(PAMI_Cours 1)
+pico_enable_stdio_usb(PAMI_Cours_Codeurs 1)
+pico_enable_stdio_uart(PAMI_Cours_Codeurs 1)
 
-pico_add_extra_outputs(PAMI_Cours)
+pico_add_extra_outputs(PAMI_Cours_Codeurs)
 
 add_custom_target(Flash
-    DEPENDS PAMI_Cours
-    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours.uf2
+    DEPENDS PAMI_Cours_Codeurs
+    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_Codeurs.uf2
 )

Moteurs.c

@@ -0,0 +1,91 @@
+#include "hardware/pwm.h"
+#include "Moteurs.h"
+
+#define MOTEUR_A_SENS1 7
+#define MOTEUR_A_SENS2 13
+#define MOTEUR_A_VITESSE 27 //5B
+#define MOTEUR_B_SENS1 10
+#define MOTEUR_B_SENS2 5
+#define MOTEUR_B_VITESSE 9 //4B
+
+uint slice_moteur_A, slice_moteur_B, slice_moteur_C;
+
+/// @brief Initialisation les entrées / sorties requises pour les moteurs
+void Moteur_init(){
+    gpio_init(MOTEUR_A_SENS1);
+	gpio_init(MOTEUR_A_SENS2);
+	gpio_init(MOTEUR_B_SENS1);
+	gpio_init(MOTEUR_B_SENS2);
+	gpio_set_dir(MOTEUR_A_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR_A_SENS2, GPIO_OUT);
+	gpio_set_dir(MOTEUR_B_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR_B_SENS2, GPIO_OUT);
+	gpio_put(MOTEUR_A_SENS1, 0);
+	gpio_put(MOTEUR_A_SENS2, 0);
+	gpio_put(MOTEUR_B_SENS1, 0);
+	gpio_put(MOTEUR_B_SENS2, 0);
+
+	gpio_set_function(MOTEUR_A_VITESSE, GPIO_FUNC_PWM);
+	gpio_set_function(MOTEUR_B_VITESSE, GPIO_FUNC_PWM);
+	pwm_set_wrap(5, (uint16_t)MOTEUR_COMMANDE_MAX);
+	pwm_set_wrap(4, (uint16_t)MOTEUR_COMMANDE_MAX);
+	pwm_set_chan_level(5, PWM_CHAN_B, 0);
+	pwm_set_chan_level(4, PWM_CHAN_B, 0);
+	pwm_set_enabled(5, true);
+	pwm_set_enabled(4, true);
+
+    Moteur_set_commande(MOTEUR_A, 0);
+    Moteur_set_commande(MOTEUR_B, 0);
+}
+
+
+/// @brief Configure le PWM et la broche de sens en fonction de la vitesse et du moteur
+/// @param moteur : Moteur (voir enum t_moteur)
+/// @param vitesse : Vitesse entre -32767 et 32767
+void Moteur_set_commande(enum t_moteur moteur, int32_t vitesse ){
+    uint16_t u_vitesse;
+
+    // Le PWM accepte 16 bits de résolution, on se remet sur 16 bits (et non sur 32 bits)
+    if (vitesse < -MOTEUR_COMMANDE_MAX){
+        vitesse = -MOTEUR_COMMANDE_MAX;
+    }else if(vitesse > MOTEUR_COMMANDE_MAX){
+        vitesse = MOTEUR_COMMANDE_MAX;
+    }
+    // On veut une vitesse > 0 pour la consigne PWM
+    if(vitesse > 0){
+        u_vitesse = vitesse;
+    }else{
+        u_vitesse = -vitesse;
+    }
+
+    switch(moteur){
+        case MOTEUR_A:
+            pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse);
+            if(vitesse > 0){
+                gpio_put(MOTEUR_A_SENS1, 0);
+                gpio_put(MOTEUR_A_SENS2, 1);
+            }else{
+                gpio_put(MOTEUR_A_SENS1, 1);
+                gpio_put(MOTEUR_A_SENS2, 0);
+            }
+            break;
+
+        case MOTEUR_B:
+            pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
+            if(vitesse < 0){
+                gpio_put(MOTEUR_B_SENS1, 0);
+	            gpio_put(MOTEUR_B_SENS2, 1);
+            }else{
+                gpio_put(MOTEUR_B_SENS1, 1);
+	            gpio_put(MOTEUR_B_SENS2, 0);
+            }
+            break;
+
+    }
+
+}
+
+void Moteur_stop(void){
+    Moteur_set_commande(MOTEUR_A, 0);
+    Moteur_set_commande(MOTEUR_B, 0);
+}

Moteurs.h

@@ -0,0 +1,16 @@
+#include "pico/stdlib.h"
+
+#ifndef MOTEURS_H
+#define MOTEURS_H
+enum t_moteur {
+    MOTEUR_A=0,
+    MOTEUR_B=1
+};
+#endif
+
+#define MOTEUR_COMMANDE_MAX 25000
+
+
+void Moteur_init(void);
+void Moteur_set_commande(enum t_moteur moteur, int32_t vitesse );
+void Moteur_stop(void);

QEI.c

@@ -0,0 +1,86 @@
+#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 
+#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;
+}

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);

Readme.md

@@ -1,13 +1,10 @@
-De l’art de déplacer un robot avec classe - Teleplot
-====================================================
+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)
 
-Teleplot
---------
+Lecture des codeurs
+-------------------
 
-Cette première étape consiste à transmettre des données depuis le robot vers un ordinateur, au format [Teleplot](https://teleplot.fr/) afin d'avoir de joli graphique. Pour profiter des graphiques, il faut avoir Teleplot d'installé sur l'ordinateur, le plus simple étant d'utiliser le plugin pour VS Code.
+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.
 
-Le code fourni est un peu compliqué pour ce qu'il fait car il gère à la fois le cas du transfert par la liaison série et le transfert par la liaison UDP/Wifi. Dans le cas d'un transfert par UDP, il est très avantageux de mettre en place un système de tampon pour grouper les envois.
-
-Pour que le code fonctionne, vous devez copier le fichier *wifi_settings.h.default* en *wifi_settings.h* puis l'éditer, même si vous n'utilisez pas le WIFI.

Teleplot.c

@@ -14,6 +14,10 @@ ip_addr_t addr;
 long teleplote_temps_ms;
 bool teleplot_temps_fige;
 
+
+/// @brief Initialise le module Teleplot, 
+/// @param  
+/// @return /
 int Teleplot_init(void){
 #ifdef WIFI_ENABLE
     if (cyw43_arch_init()) {

Temps.c

@@ -0,0 +1,28 @@
+#include <stdio.h>
+#include "pico/stdlib.h"
+#include "Temps.h"
+
+uint32_t temps_ms=0;
+bool temps_est_init=false;
+struct repeating_timer timer;
+
+/// @brief Fonction qui incrément notre temps_ms de 1, appelé par interruption toutes les 1 ms.
+/// @param t : necessaire uniquement à cause du SDK du RPi Pico.
+bool Temps_increment(struct repeating_timer *t){
+    temps_ms++;
+    return true;
+}
+
+/// @brief Crée l'interruption qui appellera la fonction Temps_increment toutes les 1 ms.
+void Temps_init(void){
+    if(!temps_est_init){
+        temps_ms=0;
+        add_repeating_timer_ms(-1, Temps_increment, NULL, &timer);
+        temps_est_init = true;
+    }
+}
+
+/// @brief Renvoie le temps actuel, depuis l'appel de Temps_init, en millisecondes
+uint32_t Temps_get_temps_ms(void){
+    return temps_ms;
+}

Temps.h

@@ -0,0 +1,5 @@
+#include "pico/stdlib.h"
+
+bool Temps_increment(struct repeating_timer *t);
+void Temps_init(void);
+uint32_t Temps_get_temps_ms(void);

main.c

@@ -4,12 +4,19 @@
  * SPDX-License-Identifier: BSD-3-Clause
 */
 #include "pico/stdlib.h"
+#include "Moteurs.h"
+#include "QEI.h"
 #include "Teleplot.h"
+#include "Temps.h"
 #include <stdio.h>
+#include <math.h>
 
 void setup(void);
 void loop(void);
 
+int valeur = 1;
+uint32_t m_temps_ms;
+
 void main(void)
 {
     setup();
@@ -21,11 +28,33 @@ void main(void)
 void setup(void){
 	stdio_init_all();
 	Teleplot_init();
+	Temps_init();
+	Moteur_init();
+	QEI_init();
 }
 
 void loop(void){
-	printf("Exemple\n");
-	Teleplot_add_variable_int("t", 2);
-	Teleplot_envoie_tampon();
-	sleep_ms(1000);
+	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();
+		}
+	}
+
 }

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);
+}
+
+%}
+