Déplacement des graphs, compilation avec les modules QEI, mais non testé

CMakeLists.txt

@@ -4,6 +4,9 @@ project(test_project C CXX ASM)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_CXX_STANDARD 17)
 pico_sdk_init()
+
+
+
 add_executable(test
 test.c
 spi_nb.c
@@ -12,13 +15,16 @@ Temps.c
 Servomoteur.c
 gyro_L3GD20H.c
 gyro_ADXRS453.c
+QEI.c
 )
 
+pico_generate_pio_header(test ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
+
 add_definitions(-DGYRO_ADXRS453)
 pico_enable_stdio_usb(test 1)
 pico_enable_stdio_uart(test 1)
 pico_add_extra_outputs(test)
-target_link_libraries(test pico_stdlib hardware_spi hardware_pwm hardware_structs)
+target_link_libraries(test pico_stdlib hardware_spi hardware_pwm hardware_structs hardware_pio)
 
 add_custom_target(Flash
     DEPENDS test

QEI.c

@@ -0,0 +1,55 @@
+#include <stdio.h>
+#include "pico/stdlib.h"
+#include "hardware/pio.h"
+#include "hardware/timer.h"
+#include "QEI.h"
+#include "quadrature_encoder.pio.h"
+
+
+struct QEI_t QEI[3];
+
+PIO pio_QEI = pio0;
+
+void QEI_init(){
+    // Initialisation des 3 modules QEI
+    // Chaque module QEI sera dans une machine à état du PIO 0
+    
+
+    // 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 26 et 27 - pio : pio0, sm : 0, Offset : 0, broches 26 et 27, clock div : 0 pour commencer
+    // QEI1 : !!! Attention, il faudra modifier la carte élec !!!
+    quadrature_encoder_program_init(pio_QEI, 0, offset, 26, 0);
+    // QEI2: broche 26 et 27 - pio : pio0, sm : 1, Offset : 0, broches 26 et 27, clock div : 0 pour commencer
+    quadrature_encoder_program_init(pio_QEI, 1, offset, 20, 0);
+    // QEI3: broche 24 et 25 - pio : pio0, sm : 1, Offset : 0, broches 26 et 27, clock div : 0 pour commencer
+    quadrature_encoder_program_init(pio_QEI, 2, offset, 24, 0);
+
+    QEI[0].value=0;
+    QEI[1].value=0;
+    QEI[2].value=0;
+
+}
+
+void QEI_update(){
+    int old_value;
+
+    old_value = QEI[0].value;
+    QEI[0].value = quadrature_encoder_get_count(pio_QEI, 0);
+    QEI[0].delta = QEI[0].value - old_value;
+
+    old_value = QEI[1].value;
+    QEI[1].value = quadrature_encoder_get_count(pio_QEI, 1);
+    QEI[1].delta = QEI[1].value - old_value;
+
+    old_value = QEI[2].value;
+    QEI[2].value = quadrature_encoder_get_count(pio_QEI, 2);
+    QEI[2].delta = QEI[2].value - old_value;
+
+}

QEI.h

@@ -0,0 +1,7 @@
+struct QEI_t{
+    int value;
+    int delta;
+};
+
+void QEI_update(void);
+void QEI_init(void);

data/multi_calb.csv

@@ -0,0 +1,87 @@
+fin calibration, -362, 1693, 3815
+.fin calibration, -342, 1682, 3710
+.fin calibration, -351, 1695, 3677
+.fin calibration, -345, 1696, 3660
+.fin calibration, -334, 1686, 3656
+.fin calibration, -343, 1688, 3639
+.fin calibration, -331, 1702, 3626
+.fin calibration, -335, 1686, 3634
+.fin calibration, -321, 1678, 3666
+.fin calibration, -323, 1692, 3616
+.fin calibration, -330, 1692, 3617
+.fin calibration, -319, 1693, 3623
+.fin calibration, -320, 1690, 3619
+.fin calibration, -320, 1700, 3626
+.fin calibration, -320, 1694, 3639
+.fin calibration, -317, 1693, 3602
+.fin calibration, -319, 1698, 3607
+.fin calibration, -316, 1694, 3598
+.fin calibration, -313, 1687, 3632
+.fin calibration, -310, 1685, 3608
+.fin calibration, -313, 1700, 3595
+.fin calibration, -331, 1704, 3576
+.fin calibration, -330, 1702, 3584
+.fin calibration, -324, 1699, 3594
+.fin calibration, -323, 1699, 3585
+.fin calibration, -306, 1682, 3616
+.fin calibration, -297, 1676, 3631
+.fin calibration, -312, 1701, 3589
+.fin calibration, -308, 1687, 3595
+.fin calibration, -315, 1695, 3574
+.fin calibration, -314, 1695, 3567
+.fin calibration, -307, 1687, 3593
+.fin calibration, -313, 1694, 3566
+.fin calibration, -307, 1696, 3582
+.fin calibration, -308, 1705, 3569
+.fin calibration, -306, 1697, 3591
+.fin calibration, -295, 1686, 3575
+.fin calibration, -315, 1706, 3550
+.fin calibration, -308, 1697, 3567
+.fin calibration, -305, 1706, 3560
+.fin calibration, -296, 1691, 3598
+.fin calibration, -308, 1708, 3558
+.fin calibration, -310, 1709, 3547
+.fin calibration, -306, 1711, 3546
+.fin calibration, -305, 1708, 3550
+.fin calibration, -304, 1711, 3554
+.fin calibration, -301, 1704, 3565
+.fin calibration, -310, 1712, 3555
+.fin calibration, -300, 1702, 3563
+.fin calibration, -301, 1709, 3534
+.fin calibration, -293, 1703, 3541
+.fin calibration, -277, 1685, 3595
+.fin calibration, -307, 1715, 3527
+.fin calibration, -298, 1703, 3526
+.fin calibration, -305, 1716, 3519
+.fin calibration, -292, 1710, 3554
+.fin calibration, -300, 1720, 3511
+.fin calibration, -298, 1717, 3513
+.fin calibration, -302, 1717, 3508
+.fin calibration, -308, 1728, 3491
+.fin calibration, -324, 1745, 3475
+.fin calibration, -312, 1726, 3498
+.fin calibration, -297, 1728, 3499
+.fin calibration, -301, 1719, 3496
+.fin calibration, -305, 1722, 3492
+.fin calibration, -300, 1717, 3503
+.fin calibration, -291, 1714, 3530
+.fin calibration, -292, 1719, 3499
+.fin calibration, -293, 1720, 3511
+.fin calibration, -288, 1726, 3511
+.fin calibration, -305, 1729, 3490
+.fin calibration, -310, 1732, 3466
+.fin calibration, -311, 1738, 3462
+.fin calibration, -295, 1738, 3468
+.fin calibration, -302, 1734, 3478
+.fin calibration, -291, 1725, 3511
+.fin calibration, -302, 1738, 3464
+.fin calibration, -305, 1735, 3479
+.fin calibration, -299, 1732, 3468
+.fin calibration, -303, 1743, 3478
+.fin calibration, -305, 1737, 3466
+.fin calibration, -307, 1750, 3452
+.fin calibration, -303, 1747, 3461
+.fin calibration, -307, 1753, 3461
+.fin calibration, -316, 1752, 3459
+.fin calibration, -302, 1739, 3460
+

data/multi_calb_1.csv

@@ -0,0 +1,71 @@
+vitesse SPI : 1953125
+
+Gyroscope trouve
+
+Nb lu: 2
+
+gyro_config ok !
+fin calibration, -386, 1661, 3479
+fin calibration, -379, 1673, 3402
+fin calibration, -386, 1671, 3398
+fin calibration, -382, 1671, 3406
+fin calibration, -392, 1670, 3408
+fin calibration, -380, 1671, 3417
+fin calibration, -384, 1680, 3429
+fin calibration, -379, 1671, 3448
+fin calibration, -387, 1678, 3428
+fin calibration, -391, 1674, 3436
+fin calibration, -382, 1684, 3430
+fin calibration, -392, 1676, 3427
+fin calibration, -391, 1686, 3442
+fin calibration, -390, 1681, 3438
+fin calibration, -398, 1676, 3449
+fin calibration, -401, 1681, 3447
+fin calibration, -391, 1667, 3464
+fin calibration, -395, 1673, 3471
+fin calibration, -396, 1675, 3469
+fin calibration, -389, 1679, 3466
+fin calibration, -383, 1678, 3458
+fin calibration, -382, 1670, 3463
+fin calibration, -388, 1676, 3471
+fin calibration, -399, 1691, 3436
+fin calibration, -401, 1690, 3452
+fin calibration, -406, 1709, 3412
+fin calibration, -408, 1707, 3417
+fin calibration, -410, 1698, 3425
+fin calibration, -418, 1697, 3428
+fin calibration, -418, 1705, 3420
+fin calibration, -408, 1708, 3414
+fin calibration, -417, 1719, 3404
+fin calibration, -405, 1695, 3449
+fin calibration, -390, 1686, 3487
+fin calibration, -386, 1681, 3500
+fin calibration, -398, 1684, 3502
+fin calibration, -383, 1676, 3500
+fin calibration, -392, 1686, 3473
+fin calibration, -386, 1681, 3506
+fin calibration, -386, 1678, 3517
+fin calibration, -393, 1679, 3490
+fin calibration, -382, 1680, 3486
+fin calibration, -393, 1686, 3492
+fin calibration, -387, 1692, 3480
+fin calibration, -388, 1691, 3485
+fin calibration, -390, 1678, 3500
+fin calibration, -385, 1686, 3485
+fin calibration, -397, 1692, 3483
+fin calibration, -393, 1692, 3497
+fin calibration, -394, 1682, 3489
+fin calibration, -393, 1682, 3508
+fin calibration, -393, 1692, 3513
+fin calibration, -382, 1673, 3560
+fin calibration, -397, 1695, 3495
+fin calibration, -387, 1688, 3510
+fin calibration, -377, 1677, 3520
+fin calibration, -381, 1680, 3533
+fin calibration, -388, 1675, 3531
+fin calibration, -392, 1680, 3533
+fin calibration, -380, 1685, 3537
+fin calibration, -384, 1694, 3527
+
+Calibration...
+

gyro.c

@@ -45,9 +45,9 @@ struct t_angle_gyro_double gyro_get_vitesse(void){
 
 void Gyro_Init(void){
     // 
-    gpio_set_function(16, GPIO_FUNC_SPI); // SDI
-    gpio_set_function(18, GPIO_FUNC_SPI); // SCK
-    gpio_set_function(19, GPIO_FUNC_SPI); // SDO
+    gpio_set_function(0, GPIO_FUNC_SPI); // SDI (ancien : 16)
+    gpio_set_function(2, GPIO_FUNC_SPI); // SCK (ancien : 18)
+    gpio_set_function(3, GPIO_FUNC_SPI); // SDO (ancien : 19)
     gpio_set_function(PIN_CS, GPIO_OUT); // CSn
 
     gpio_init(PIN_CS);

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

test.c

@@ -46,11 +46,7 @@ int main() {
         
         // Tous les pas de step_ms
         if(!(temps_ms % step_ms)){
-            uint64_t _time;
-            uint32_t time_after_read, time_after_filter, time_after_printf;
-            _time = time_us_64();
             Gyro_Read(step_ms);
-            time_after_read = time_us_64() - _time;
             
             //gyro_affiche(gyro_get_vitesse(), "Angle :");
             // Filtre 
@@ -64,12 +60,10 @@ int main() {
                 vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre;
                 vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre;
             }
-            time_after_filter = time_us_64() - _time;
 
             //printf("%#x, %#x\n", (double)temps_ms_old / 1000,  vitesse_filtre_z);
 
             //printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000));
-            time_after_printf = time_us_64() - _time;
             //gyro_affiche(angle_gyro, "Vitesse (°/s),");
             
         }