Created
August 3, 2023 20:25
-
-
Save wizhippo/98fefa63fb099b34cb6e24ea224dfe5d to your computer and use it in GitHub Desktop.
pi pico quad with set counter
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
; | |
; Copyright (c) 2023 Raspberry Pi (Trading) Ltd. | |
; | |
; SPDX-License-Identifier: BSD-3-Clause | |
; | |
.program quadrature_encoder | |
; the code must be loaded at address 0, because it uses computed jumps | |
.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 | |
; the program keeps trying to write the current count to the RX FIFO without | |
; blocking. To read the current count, the user code must drain the FIFO first | |
; and wait for a fresh sample (takes ~4 SM cycles on average). The worst case | |
; sampling loop takes 10 cycles, so this program is able to read step rates up | |
; to sysclk / 10 (e.g., sysclk 125MHz, max step rate = 12.5 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: | |
MOV ISR, Y ; read 11 | |
PUSH noblock | |
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. Both the PUSH | |
; above and the OUT below zero out the other bits in ISR | |
OUT ISR, 2 | |
IN PINS, 2 | |
; save the state in the OSR, so that we can use ISR for other purposes | |
MOV OSR, ISR | |
; jump to the correct state machine action | |
MOV PC, ISR | |
; the PIO does not have a increment instruction, so to do that we do a | |
; negate, decrement, negate sequence | |
increment: | |
MOV Y, ~Y | |
JMP Y--, increment_cont | |
increment_cont: | |
MOV Y, ~Y | |
.wrap ; the .wrap here avoids one jump instruction and saves a cycle too | |
; set counter | |
PUBLIC set_count: | |
MOV X, OSR | |
PULL | |
OUT Y, 32 | |
MOV OSR, X | |
JMP update | |
% 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 | |
static inline void quadrature_encoder_program_init(PIO pio, uint sm, 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(0); | |
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 10 cycles | |
float div = (float)clock_get_hz(clk_sys) / (10 * max_step_rate); | |
sm_config_set_clkdiv(&c, div); | |
} | |
pio_sm_init(pio, sm, 0, &c); | |
pio_sm_set_enabled(pio, sm, true); | |
} | |
static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm) | |
{ | |
uint ret; | |
int n; | |
// if the FIFO has N entries, we fetch them to drain the FIFO, | |
// plus one entry which will be guaranteed to not be stale | |
n = pio_sm_get_rx_fifo_level(pio, sm) + 1; | |
while (n > 0) { | |
ret = pio_sm_get_blocking(pio, sm); | |
n--; | |
} | |
return ret; | |
} | |
static inline void quadrature_encoder_set_count(PIO pio, uint sm, uint32_t count) { | |
pio_sm_exec(pio, sm, pio_encode_jmp(quadrature_encoder_offset_set_count)); | |
pio_sm_put_blocking(pio, sm, count); | |
} | |
%} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment