Skip to content

Commit 09e34e7

Browse files
pmarques-devPaulo Marques
pmarques-dev
and
Paulo Marques
authored
add a quadrature encoder pio example (#126)
Co-authored-by: Paulo Marques <[email protected]>
1 parent 6e647c6 commit 09e34e7

File tree

4 files changed

+238
-0
lines changed

4 files changed

+238
-0
lines changed

‎pio/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ if (NOT PICO_NO_HARDWARE)
1010
add_subdirectory(manchester_encoding)
1111
add_subdirectory(pio_blink)
1212
add_subdirectory(pwm)
13+
add_subdirectory(quadrature_encoder)
1314
add_subdirectory(spi)
1415
add_subdirectory(squarewave)
1516
add_subdirectory(st7789_lcd)

‎pio/quadrature_encoder/CMakeLists.txt

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
add_executable(pio_quadrature_encoder)
2+
3+
pico_generate_pio_header(pio_quadrature_encoder ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
4+
5+
target_sources(pio_quadrature_encoder PRIVATE quadrature_encoder.c)
6+
7+
target_link_libraries(pio_quadrature_encoder PRIVATE
8+
pico_stdlib
9+
pico_multicore
10+
hardware_pio
11+
)
12+
13+
pico_enable_stdio_usb(pio_quadrature_encoder 1)
14+
15+
pico_add_extra_outputs(pio_quadrature_encoder)
16+
17+
# add url via pico_set_program_url
18+
example_auto_set_url(pio_quadrature_encoder)

‎pio/quadrature_encoder/quadrature_encoder.c

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
#include <stdio.h>
2+
#include "pico/stdlib.h"
3+
#include "hardware/pio.h"
4+
#include "hardware/timer.h"
5+
6+
#include "quadrature_encoder.pio.h"
7+
8+
//
9+
// ---- quadrature encoder interface example
10+
//
11+
// the PIO program reads phase A/B of a quadrature encoder and increments or
12+
// decrements an internal counter to keep the current absolute step count
13+
// updated. At any point, the main code can query the current count by using
14+
// the quadrature_encoder_*_count functions. The counter is kept in a full
15+
// 32 bit register that just wraps around. Two's complement arithmetic means
16+
// that it can be interpreted as a 32 bit signed or unsigned value and it will
17+
// work anyway.
18+
//
19+
// As an example, a two wheel robot being controlled at 100Hz, can use two
20+
// state machines to read the two encoders and in the main control loop it can
21+
// simply ask for the current encoder counts to get the absolute step count. It
22+
// can also subtract the values from the last sample to check how many steps
23+
// each wheel as done since the last sample period.
24+
//
25+
// One advantage of this approach is that it requires zero CPU time to keep the
26+
// encoder count updated and because of that it supports very high step rates.
27+
//
28+
29+
30+
int main()
31+
{
32+
int new_value, delta, old_value = 0;
33+
34+
// Base pin to connect the A phase of the encoder.
35+
// The B phase must be connected to the next pin
36+
const uint PIN_AB = 10;
37+
38+
stdio_init_all();
39+
40+
PIO pio = pio0;
41+
const uint sm = 0;
42+
43+
uint offset = pio_add_program(pio, &quadrature_encoder_program);
44+
quadrature_encoder_program_init(pio, sm, offset, PIN_AB, 0);
45+
46+
while (1) {
47+
// note: thanks to two's complement arithmetic delta will always
48+
// be correct even when new_value wraps around MAXINT / MININT
49+
new_value = quadrature_encoder_get_count(pio, sm);
50+
delta = new_value - old_value;
51+
old_value = new_value;
52+
53+
printf("position %8d, delta %6d\n", new_value, delta);
54+
sleep_ms(100);
55+
}
56+
57+
return 0;
58+
}
59+

‎pio/quadrature_encoder/quadrature_encoder.pio

Lines changed: 160 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
2+
.program quadrature_encoder
3+
4+
; this code must be loaded into address 0, but at 29 instructions, it probably
5+
; wouldn't be able to share space with other programs anyway
6+
.origin 0
7+
8+
9+
; the code works by running a loop that contiously shifts the 2 phase pins into
10+
; ISR and looks at the lower 4 bits to do a computed jump to an instruction that
11+
; does the proper "do nothing" | "increment" | "decrement" action for that pin
12+
; state change (or no change)
13+
14+
; ISR holds the last state of the 2 pins during most of the code. The Y register
15+
; keeps the current encoder count and is incremented / decremented according to
16+
; the steps sampled
17+
18+
; writing any non zero value to the TX FIFO makes the state machine push the
19+
; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case
20+
; sampling loop takes 14 cycles, so this program is able to read step rates up
21+
; to sysclk / 14 (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec)
22+
23+
24+
; 00 state
25+
JMP update ; read 00
26+
JMP decrement ; read 01
27+
JMP increment ; read 10
28+
JMP update ; read 11
29+
30+
; 01 state
31+
JMP increment ; read 00
32+
JMP update ; read 01
33+
JMP update ; read 10
34+
JMP decrement ; read 11
35+
36+
; 10 state
37+
JMP decrement ; read 00
38+
JMP update ; read 01
39+
JMP update ; read 10
40+
JMP increment ; read 11
41+
42+
; to reduce code size, the last 2 states are implemented in place and become the
43+
; target for the other jumps
44+
45+
; 11 state
46+
JMP update ; read 00
47+
JMP increment ; read 01
48+
decrement:
49+
; note: the target of this instruction must be the next address, so that
50+
; the effect of the instruction does not depend on the value of Y. The
51+
; same is true for the "JMP X--" below. Basically "JMP Y--, <next addr>"
52+
; is just a pure "decrement Y" instruction, with no other side effects
53+
JMP Y--, update ; read 10
54+
55+
; this is where the main loop starts
56+
.wrap_target
57+
update:
58+
; we start by checking the TX FIFO to see if the main code is asking for
59+
; the current count after the PULL noblock, OSR will have either 0 if
60+
; there was nothing or the value that was there
61+
SET X, 0
62+
PULL noblock
63+
64+
; since there are not many free registers, and PULL is done into OSR, we
65+
; have to do some juggling to avoid losing the state information and
66+
; still place the values where we need them
67+
MOV X, OSR
68+
MOV OSR, ISR
69+
70+
; the main code did not ask for the count, so just go to "sample_pins"
71+
JMP !X, sample_pins
72+
73+
; if it did ask for the count, then we push it
74+
MOV ISR, Y ; we trash ISR, but we already have a copy in OSR
75+
PUSH
76+
77+
sample_pins:
78+
; we shift into ISR the last state of the 2 input pins (now in OSR) and
79+
; the new state of the 2 pins, thus producing the 4 bit target for the
80+
; computed jump into the correct action for this state
81+
MOV ISR, NULL
82+
IN OSR, 2
83+
IN PINS, 2
84+
MOV PC, ISR
85+
86+
; the PIO does not have a increment instruction, so to do that we do a
87+
; negate, decrement, negate sequence
88+
increment:
89+
MOV X, !Y
90+
JMP X--, increment_cont
91+
increment_cont:
92+
MOV Y, !X
93+
.wrap ; the .wrap here avoids one jump instruction and saves a cycle too
94+
95+
96+
97+
% c-sdk {
98+
99+
#include "hardware/clocks.h"
100+
#include "hardware/gpio.h"
101+
102+
// max_step_rate is used to lower the clock of the state machine to save power
103+
// if the application doesn't require a very high sampling rate. Passing zero
104+
// will set the clock to the maximum, which gives a max step rate of around
105+
// 8.9 Msteps/sec at 125MHz
106+
107+
static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate)
108+
{
109+
pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false);
110+
pio_gpio_init(pio, pin);
111+
gpio_pull_up(pin);
112+
113+
pio_sm_config c = quadrature_encoder_program_get_default_config(offset);
114+
sm_config_set_in_pins(&c, pin); // for WAIT, IN
115+
sm_config_set_jmp_pin(&c, pin); // for JMP
116+
// shift to left, autopull disabled
117+
sm_config_set_in_shift(&c, false, false, 32);
118+
// don't join FIFO's
119+
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
120+
121+
// passing "0" as the sample frequency,
122+
if (max_step_rate == 0) {
123+
sm_config_set_clkdiv(&c, 1.0);
124+
} else {
125+
// one state machine loop takes at most 14 cycles
126+
float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate);
127+
sm_config_set_clkdiv(&c, div);
128+
}
129+
130+
pio_sm_init(pio, sm, offset, &c);
131+
pio_sm_set_enabled(pio, sm, true);
132+
}
133+
134+
135+
// When requesting the current count we may have to wait a few cycles (average
136+
// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple
137+
// encoders, we may request them all in one go and then fetch them all, thus
138+
// avoiding doing the wait multiple times. If we are reading just one encoder,
139+
// we can use the "get_count" function to request and wait
140+
141+
static inline void quadrature_encoder_request_count(PIO pio, uint sm)
142+
{
143+
pio->txf[sm] = 1;
144+
}
145+
146+
static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm)
147+
{
148+
while (pio_sm_is_rx_fifo_empty(pio, sm))
149+
tight_loop_contents();
150+
return pio->rxf[sm];
151+
}
152+
153+
static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm)
154+
{
155+
quadrature_encoder_request_count(pio, sm);
156+
return quadrature_encoder_fetch_count(pio, sm);
157+
}
158+
159+
%}
160+

0 commit comments

Comments
 (0)