Lecture des codeurs: OK
This commit is contained in:
parent
4f8ba36580
commit
5b37406e9c
@ -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
|
||||
)
|
||||
|
||||
107
QEI.c
Normal file
107
QEI.c
Normal file
@ -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;
|
||||
}
|
||||
16
QEI.h
Normal file
16
QEI.h
Normal 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(void);
|
||||
int QEI_get(enum QEI_name_t qei);
|
||||
float QEI_get_mm(enum QEI_name_t qei);
|
||||
18
Readme.md
18
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
|
||||
|
||||
7
main.c
7
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);
|
||||
}
|
||||
}
|
||||
|
||||
165
quadrature_encoder.pio
Normal file
165
quadrature_encoder.pio
Normal 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);
|
||||
}
|
||||
|
||||
%}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user