diff --git a/pio/CMakeLists.txt b/pio/CMakeLists.txt index 8855161..8e289bf 100644 --- a/pio/CMakeLists.txt +++ b/pio/CMakeLists.txt @@ -10,6 +10,7 @@ if (NOT PICO_NO_HARDWARE) add_subdirectory(manchester_encoding) add_subdirectory(pio_blink) add_subdirectory(pwm) + add_subdirectory(quadrature_encoder) add_subdirectory(spi) add_subdirectory(squarewave) add_subdirectory(st7789_lcd) diff --git a/pio/quadrature_encoder/CMakeLists.txt b/pio/quadrature_encoder/CMakeLists.txt new file mode 100644 index 0000000..b118f3c --- /dev/null +++ b/pio/quadrature_encoder/CMakeLists.txt @@ -0,0 +1,18 @@ +add_executable(pio_quadrature_encoder) + +pico_generate_pio_header(pio_quadrature_encoder ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) + +target_sources(pio_quadrature_encoder PRIVATE quadrature_encoder.c) + +target_link_libraries(pio_quadrature_encoder PRIVATE + pico_stdlib + pico_multicore + hardware_pio + ) + +pico_enable_stdio_usb(pio_quadrature_encoder 1) + +pico_add_extra_outputs(pio_quadrature_encoder) + +# add url via pico_set_program_url +example_auto_set_url(pio_quadrature_encoder) diff --git a/pio/quadrature_encoder/quadrature_encoder.c b/pio/quadrature_encoder/quadrature_encoder.c new file mode 100644 index 0000000..de119d9 --- /dev/null +++ b/pio/quadrature_encoder/quadrature_encoder.c @@ -0,0 +1,59 @@ +#include +#include "pico/stdlib.h" +#include "hardware/pio.h" +#include "hardware/timer.h" + +#include "quadrature_encoder.pio.h" + +// +// ---- quadrature encoder interface example +// +// the PIO program reads phase A/B of a quadrature encoder and increments or +// decrements an internal counter to keep the current absolute step count +// updated. At any point, the main code can query the current count by using +// the quadrature_encoder_*_count functions. The counter is kept in a full +// 32 bit register that just wraps around. Two's complement arithmetic means +// that it can be interpreted as a 32 bit signed or unsigned value and it will +// work anyway. +// +// As an example, a two wheel robot being controlled at 100Hz, can use two +// state machines to read the two encoders and in the main control loop it can +// simply ask for the current encoder counts to get the absolute step count. It +// can also subtract the values from the last sample to check how many steps +// each wheel as done since the last sample period. +// +// One advantage of this approach is that it requires zero CPU time to keep the +// encoder count updated and because of that it supports very high step rates. +// + + +int main() +{ + int new_value, delta, old_value = 0; + + // Base pin to connect the A phase of the encoder. + // The B phase must be connected to the next pin + const uint PIN_AB = 10; + + stdio_init_all(); + + PIO pio = pio0; + const uint sm = 0; + + uint offset = pio_add_program(pio, &quadrature_encoder_program); + quadrature_encoder_program_init(pio, sm, offset, PIN_AB, 0); + + while (1) { + // note: thanks to two's complement arithmetic delta will always + // be correct even when new_value wraps around MAXINT / MININT + new_value = quadrature_encoder_get_count(pio, sm); + delta = new_value - old_value; + old_value = new_value; + + printf("position %8d, delta %6d\n", new_value, delta); + sleep_ms(100); + } + + return 0; +} + diff --git a/pio/quadrature_encoder/quadrature_encoder.pio b/pio/quadrature_encoder/quadrature_encoder.pio new file mode 100644 index 0000000..34ff9e6 --- /dev/null +++ b/pio/quadrature_encoder/quadrature_encoder.pio @@ -0,0 +1,160 @@ + +.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 contiously 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--, " + ; 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); + pio_gpio_init(pio, pin); + gpio_pull_up(pin); + + 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); +} + +%} +