add a quadrature encoder pio example (#126)

Co-authored-by: Paulo Marques <pm@quant-insight.com>

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)

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)

pio/quadrature_encoder/quadrature_encoder.c

@@ -0,0 +1,59 @@
+#include <stdio.h>
+#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;
+}
+

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