Keuronde/Art_deplacer_robot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Code de demonstration de la lecture des codeurs

Browse Source
This commit is contained in:
Samuel 2025-01-15 11:06:48 +01:00
parent 86d8399486
commit 4bebc91c81
7 changed files with 308 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
CMakeLists.txt
View File

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

1
Moteurs.h
View File

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

86
QEI.c Normal file
View File

@ -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;
}

16
QEI.h Normal file
View File

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

6
Readme.md
View File

@ -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.

23
main.c
View File

@ -5,6 +5,7 @@
*/
#include "pico/stdlib.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Teleplot.h"
#include "Temps.h"
#include <stdio.h>
@ -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();
}
}
}

165
quadrature_encoder.pio Normal file
View File

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