diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index 32c6169c..52d60ebe 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -29,3 +29,4 @@ add_subdirectory(hub75) add_subdirectory(uc8151) add_subdirectory(pwm) add_subdirectory(servo) +add_subdirectory(encoder-pio) diff --git a/drivers/encoder-pio/CMakeLists.txt b/drivers/encoder-pio/CMakeLists.txt new file mode 100644 index 00000000..3acea382 --- /dev/null +++ b/drivers/encoder-pio/CMakeLists.txt @@ -0,0 +1,13 @@ +add_library(encoder-pio INTERFACE) + +target_sources(encoder-pio INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/encoder.cpp + ${CMAKE_CURRENT_LIST_DIR}/capture.cpp +) + +pico_generate_pio_header(encoder-pio ${CMAKE_CURRENT_LIST_DIR}/encoder.pio) + +target_include_directories(encoder-pio INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(encoder-pio INTERFACE pico_stdlib hardware_pio) \ No newline at end of file diff --git a/drivers/encoder-pio/capture.cpp b/drivers/encoder-pio/capture.cpp new file mode 100644 index 00000000..e0208f08 --- /dev/null +++ b/drivers/encoder-pio/capture.cpp @@ -0,0 +1,70 @@ +#include +#include +#include "capture.hpp" + +namespace pimoroni { + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // CONSTRUCTORS + //////////////////////////////////////////////////////////////////////////////////////////////////// + Capture::Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution) : + captured_count(captured_count), count_change(count_change), average_frequency(average_frequency), + counts_per_revolution(std::max(counts_per_revolution, FLT_MIN)) { //Clamp counts_per_rev to avoid potential NaN + } + + + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // METHODS + //////////////////////////////////////////////////////////////////////////////////////////////////// + int32_t Capture::get_count() const { + return captured_count; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_revolutions() const { + return (float)get_count() / counts_per_revolution; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_angle_degrees() const { + return get_revolutions() * 360.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_angle_radians() const { + return get_revolutions() * M_TWOPI; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + int32_t Capture::get_count_change() const { + return count_change; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_frequency() const { + return average_frequency; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_revolutions_per_second() const { + return get_frequency() / counts_per_revolution; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_revolutions_per_minute() const { + return get_revolutions_per_second() * 60.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_degrees_per_second() const { + return get_revolutions_per_second() * 360.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Capture::get_radians_per_second() const { + return get_revolutions_per_second() * M_TWOPI; + } + //////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////// +} \ No newline at end of file diff --git a/drivers/encoder-pio/capture.hpp b/drivers/encoder-pio/capture.hpp new file mode 100644 index 00000000..2aa6edaf --- /dev/null +++ b/drivers/encoder-pio/capture.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include "pico/stdlib.h" + +namespace pimoroni { + + class Capture { + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + const int32_t captured_count = 0; + const int32_t count_change = 0; + const float average_frequency = 0.0f; + const float counts_per_revolution = 1; + + + //-------------------------------------------------- + // Constructors + //-------------------------------------------------- + public: + Capture() {} + Capture(int32_t captured_count, int32_t count_change, float average_frequency, float counts_per_revolution); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + int32_t get_count() const; + float get_revolutions() const; + float get_angle_degrees() const; + float get_angle_radians() const; + + int32_t get_count_change() const; + + float get_frequency() const; + float get_revolutions_per_second() const; + float get_revolutions_per_minute() const; + float get_degrees_per_second() const; + float get_radians_per_second() const; + }; + +} \ No newline at end of file diff --git a/drivers/encoder-pio/encoder-pio.cmake b/drivers/encoder-pio/encoder-pio.cmake new file mode 100644 index 00000000..37b3959a --- /dev/null +++ b/drivers/encoder-pio/encoder-pio.cmake @@ -0,0 +1,12 @@ +add_library(encoder-pio INTERFACE) + +target_sources(encoder-pio INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/msa301.cpp +) + +pico_generate_pio_header(encoder-pio ${CMAKE_CURRENT_LIST_DIR}/encoder.pio) + +target_include_directories(encoder-pio INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(encoder-pio INTERFACE pico_stdlib hardware_i2c) \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.cpp b/drivers/encoder-pio/encoder.cpp new file mode 100644 index 00000000..730b74df --- /dev/null +++ b/drivers/encoder-pio/encoder.cpp @@ -0,0 +1,333 @@ +#include +#include +#include "hardware/irq.h" +#include "encoder.hpp" +#include "encoder.pio.h" + +#define LAST_STATE(state) ((state) & 0b0011) +#define CURR_STATE(state) (((state) & 0b1100) >> 2) + +namespace pimoroni { + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // STATICS + //////////////////////////////////////////////////////////////////////////////////////////////////// + Encoder* Encoder::pio_encoders[][NUM_PIO_STATE_MACHINES] = { { nullptr, nullptr, nullptr, nullptr }, { nullptr, nullptr, nullptr, nullptr } }; + uint8_t Encoder::pio_claimed_sms[] = { 0x0, 0x0 }; + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::pio0_interrupt_callback() { + //Go through each of encoders on this PIO to see which triggered this interrupt + for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { + if(pio_encoders[0][sm] != nullptr) { + pio_encoders[0][sm]->check_for_transition(); + } + } + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::pio1_interrupt_callback() { + //Go through each of encoders on this PIO to see which triggered this interrupt + for(uint8_t sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) { + if(pio_encoders[1][sm] != nullptr) { + pio_encoders[1][sm]->check_for_transition(); + } + } + } + + + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // CONSTRUCTORS / DESTRUCTOR + //////////////////////////////////////////////////////////////////////////////////////////////////// + Encoder::Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC, + float counts_per_revolution, bool count_microsteps, + uint16_t freq_divider) : + enc_pio(pio), pinA(pinA), pinB(pinB), pinC(pinC), + counts_per_revolution(counts_per_revolution), count_microsteps(count_microsteps), + freq_divider(freq_divider), clocks_per_time((float)(clock_get_hz(clk_sys) / (ENC_LOOP_CYCLES * freq_divider))) { + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + Encoder::~Encoder() { + //Clean up our use of the SM associated with this encoder + encoder_program_release(enc_pio, enc_sm); + uint index = pio_get_index(enc_pio); + pio_encoders[index][enc_sm] = nullptr; + pio_claimed_sms[index] &= ~(1u << enc_sm); + + //If there are no more SMs using the encoder program, then we can remove it from the PIO + if(pio_claimed_sms[index] == 0) { + pio_remove_program(enc_pio, &encoder_program, enc_offset); + } + } + + + + //////////////////////////////////////////////////////////////////////////////////////////////////// + // METHODS + //////////////////////////////////////////////////////////////////////////////////////////////////// + bool Encoder::init() { + bool initialised = false; + + //Are the pins we want to use actually valid? + if((pinA < NUM_BANK0_GPIOS) && (pinB < NUM_BANK0_GPIOS)) { + + //If a Pin C was defined, and valid, set it as a GND to pull the other two pins down + if((pinC != PIN_UNUSED) && (pinC < NUM_BANK0_GPIOS)) { + gpio_init(pinC); + gpio_set_dir(pinC, GPIO_OUT); + gpio_put(pinC, false); + } + + enc_sm = pio_claim_unused_sm(enc_pio, true); + uint pio_idx = pio_get_index(enc_pio); + + //Is this the first time using an encoder on this PIO? + if(pio_claimed_sms[pio_idx] == 0) { + //Add the program to the PIO memory and enable the appropriate interrupt + enc_offset = pio_add_program(enc_pio, &encoder_program); + encoder_program_init(enc_pio, enc_sm, enc_offset, pinA, pinB, freq_divider); + hw_set_bits(&enc_pio->inte0, PIO_IRQ0_INTE_SM0_RXNEMPTY_BITS << enc_sm); + if(pio_idx == 0) { + irq_set_exclusive_handler(PIO0_IRQ_0, pio0_interrupt_callback); + irq_set_enabled(PIO0_IRQ_0, true); + } + else { + irq_set_exclusive_handler(PIO1_IRQ_0, pio1_interrupt_callback); + irq_set_enabled(PIO1_IRQ_0, true); + } + } + + //Keep a record of this encoder for the interrupt callback + pio_encoders[pio_idx][enc_sm] = this; + pio_claimed_sms[pio_idx] |= 1u << enc_sm; + + //Read the current state of the encoder pins and start the PIO program on the SM + stateA = gpio_get(pinA); + stateB = gpio_get(pinB); + encoder_program_start(enc_pio, enc_sm, stateA, stateB); + + initialised = true; + } + return initialised; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + bool Encoder::get_state_a() const { + return stateA; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + bool Encoder::get_state_b() const { + return stateB; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + int32_t Encoder::get_count() const { + return count - count_offset; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_revolutions() const { + return (float)get_count() / counts_per_revolution; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_angle_degrees() const { + return get_revolutions() * 360.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_angle_radians() const { + return get_revolutions() * M_TWOPI; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_frequency() const { + return clocks_per_time / (float)time_since; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_revolutions_per_second() const { + return get_frequency() / counts_per_revolution; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_revolutions_per_minute() const { + return get_revolutions_per_second() * 60.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_degrees_per_second() const { + return get_revolutions_per_second() * 360.0f; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + float Encoder::get_radians_per_second() const { + return get_revolutions_per_second() * M_TWOPI; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::zero_count() { + count_offset = count; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + Capture Encoder::perform_capture() { + //Capture the current values + int32_t captured_count = count; + int32_t captured_cumulative_time = cumulative_time; + cumulative_time = 0; + + //Determine the change in counts since the last capture was performed + int32_t count_change = captured_count - last_captured_count; + last_captured_count = captured_count; + + //Calculate the average frequency of state transitions + float average_frequency = 0.0f; + if(count_change != 0 && captured_cumulative_time != INT_MAX) { + average_frequency = (clocks_per_time * (float)count_change) / (float)captured_cumulative_time; + } + + return Capture(captured_count, count_change, average_frequency, counts_per_revolution); + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::microstep_up(int32_t time) { + count++; + time_since = time; + microstep_time = 0; + + if(time + cumulative_time < time) //Check to avoid integer overflow + cumulative_time = INT_MAX; + else + cumulative_time += time; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::microstep_down(int32_t time) { + count--; + time_since = 0 - time; + microstep_time = 0; + + if(time + cumulative_time < time) //Check to avoid integer overflow + cumulative_time = INT_MAX; + else + cumulative_time += time; + } + + //////////////////////////////////////////////////////////////////////////////////////////////////// + void Encoder::check_for_transition() { + while(enc_pio->ints0 & (PIO_IRQ0_INTS_SM0_RXNEMPTY_BITS << enc_sm)) { + uint32_t received = pio_sm_get(enc_pio, enc_sm); + + // Extract the current and last encoder states from the received value + stateA = (bool)(received & STATE_A_MASK); + stateB = (bool)(received & STATE_B_MASK); + uint8_t states = (received & STATES_MASK) >> 28; + + // Extract the time (in cycles) it has been since the last received + int32_t time_received = (received & TIME_MASK) + ENC_DEBOUNCE_TIME; + + // For rotary encoders, only every fourth transition is cared about, causing an inaccurate time value + // To address this we accumulate the times received and zero it when a transition is counted + if(!count_microsteps) { + if(time_received + microstep_time < time_received) //Check to avoid integer overflow + time_received = INT32_MAX; + else + time_received += microstep_time; + microstep_time = time_received; + } + + // Determine what transition occurred + switch(LAST_STATE(states)) { + //-------------------------------------------------- + case MICROSTEP_0: + switch(CURR_STATE(states)) { + // A ____|‾‾‾‾ + // B _________ + case MICROSTEP_1: + if(count_microsteps) + microstep_up(time_received); + break; + + // A _________ + // B ____|‾‾‾‾ + case MICROSTEP_3: + if(count_microsteps) + microstep_down(time_received); + break; + } + break; + + //-------------------------------------------------- + case MICROSTEP_1: + switch(CURR_STATE(states)) { + // A ‾‾‾‾‾‾‾‾‾ + // B ____|‾‾‾‾ + case MICROSTEP_2: + if(count_microsteps || last_travel_dir == CLOCKWISE) + microstep_up(time_received); + + last_travel_dir = NO_DIR; //Finished turning clockwise + break; + + // A ‾‾‾‾|____ + // B _________ + case MICROSTEP_0: + if(count_microsteps) + microstep_down(time_received); + break; + } + break; + + //-------------------------------------------------- + case MICROSTEP_2: + switch(CURR_STATE(states)) { + // A ‾‾‾‾|____ + // B ‾‾‾‾‾‾‾‾‾ + case MICROSTEP_3: + if(count_microsteps) + microstep_up(time_received); + + last_travel_dir = CLOCKWISE; //Started turning clockwise + break; + + // A ‾‾‾‾‾‾‾‾‾ + // B ‾‾‾‾|____ + case MICROSTEP_1: + if(count_microsteps) + microstep_down(time_received); + + last_travel_dir = COUNTERCLOCK; //Started turning counter-clockwise + break; + } + break; + + //-------------------------------------------------- + case MICROSTEP_3: + switch(CURR_STATE(states)) { + // A _________ + // B ‾‾‾‾|____ + case MICROSTEP_0: + if(count_microsteps) + microstep_up(time_received); + break; + + // A ____|‾‾‾‾ + // B ‾‾‾‾‾‾‾‾‾ + case MICROSTEP_2: + if(count_microsteps || last_travel_dir == COUNTERCLOCK) + microstep_down(time_received); + + last_travel_dir = NO_DIR; //Finished turning counter-clockwise + break; + } + break; + } + } + } + //////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////// +} \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.hpp b/drivers/encoder-pio/encoder.hpp new file mode 100644 index 00000000..a3f27df5 --- /dev/null +++ b/drivers/encoder-pio/encoder.hpp @@ -0,0 +1,126 @@ +#pragma once + +#include "hardware/pio.h" +#include "capture.hpp" + +namespace pimoroni { + + class Encoder { + //-------------------------------------------------- + // Constants + //-------------------------------------------------- + public: + static constexpr float DEFAULT_COUNTS_PER_REV = 24; + static const uint16_t DEFAULT_COUNT_MICROSTEPS = false; + static const uint16_t DEFAULT_FREQ_DIVIDER = 1; + static const uint8_t PIN_UNUSED = UINT8_MAX; + + private: + static const uint32_t STATE_A_MASK = 0x80000000; + static const uint32_t STATE_B_MASK = 0x40000000; + static const uint32_t STATE_A_LAST_MASK = 0x20000000; + static const uint32_t STATE_B_LAST_MASK = 0x10000000; + + static const uint32_t STATES_MASK = STATE_A_MASK | STATE_B_MASK | + STATE_A_LAST_MASK | STATE_B_LAST_MASK; + + static const uint32_t TIME_MASK = 0x0fffffff; + + static const uint8_t MICROSTEP_0 = 0b00; + static const uint8_t MICROSTEP_1 = 0b10; + static const uint8_t MICROSTEP_2 = 0b11; + static const uint8_t MICROSTEP_3 = 0b01; + + + //-------------------------------------------------- + // Enums + //-------------------------------------------------- + private: + enum Direction { + NO_DIR = 0, + CLOCKWISE = 1, + COUNTERCLOCK = -1, + }; + + + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + private: + const PIO enc_pio = pio0; + const uint8_t pinA = PIN_UNUSED; + const uint8_t pinB = PIN_UNUSED; + const uint8_t pinC = PIN_UNUSED; + + const float counts_per_revolution = DEFAULT_COUNTS_PER_REV; + const bool count_microsteps = DEFAULT_COUNT_MICROSTEPS; + const uint16_t freq_divider = DEFAULT_FREQ_DIVIDER; + const float clocks_per_time = 0; + + //-------------------------------------------------- + + uint enc_sm = 0; + uint enc_offset = 0; + + volatile bool stateA = false; + volatile bool stateB = false; + volatile int32_t count = 0; + volatile int32_t time_since = 0; + volatile Direction last_travel_dir = NO_DIR; + volatile int32_t microstep_time = 0; + volatile int32_t cumulative_time = 0; + + int32_t count_offset = 0; + int32_t last_captured_count = 0; + + + //-------------------------------------------------- + // Statics + //-------------------------------------------------- + public: + static Encoder* pio_encoders[NUM_PIOS][NUM_PIO_STATE_MACHINES]; + static uint8_t pio_claimed_sms[NUM_PIOS]; + static void pio0_interrupt_callback(); + static void pio1_interrupt_callback(); + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + Encoder() {} + Encoder(PIO pio, uint8_t pinA, uint8_t pinB, uint8_t pinC = PIN_UNUSED, + float counts_per_revolution = DEFAULT_COUNTS_PER_REV, bool count_microsteps = DEFAULT_COUNT_MICROSTEPS, + uint16_t freq_divider = DEFAULT_FREQ_DIVIDER); + ~Encoder(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + bool init(); + + bool get_state_a() const; + bool get_state_b() const; + int32_t get_count() const; + float get_revolutions() const; + float get_angle_degrees() const; + float get_angle_radians() const; + + float get_frequency() const; + float get_revolutions_per_second() const; + float get_revolutions_per_minute() const; + float get_degrees_per_second() const; + float get_radians_per_second() const; + + void zero_count(); + Capture perform_capture(); + + private: + void microstep_up(int32_t time_since); + void microstep_down(int32_t time_since); + void check_for_transition(); + }; + +} \ No newline at end of file diff --git a/drivers/encoder-pio/encoder.pio b/drivers/encoder-pio/encoder.pio new file mode 100644 index 00000000..26138986 --- /dev/null +++ b/drivers/encoder-pio/encoder.pio @@ -0,0 +1,119 @@ +; -------------------------------------------------- +; Quadrature Encoder reader using PIO +; by Christopher (@ZodiusInfuser) Parrott +; -------------------------------------------------- +; +; Watches any two pins (i.e. do not need to be consecutive) for +; when their state changes, and pushes that new state along with +; the old state, and time since the last change. +; +; - X is used for storing the last state +; - Y is used as a general scratch register and for storing the current state +; - OSR is used for storing the state-change timer +; +; After data is pushed into the system, a long delay takes place +; as a form of switch debounce to deal with rotary encoder dials. +; This is currently set to 500 cycles, but can be changed using the +; debounce constants below, as well as adjusting the frequency the PIO +; state machine runs at. E.g. a freq_divider of 250 gives a 1ms debounce. + + +; Debounce Constants +; -------------------------------------------------- +.define SET_CYCLES 10 +.define ITERATIONS 30 +.define JMP_CYCLES 16 +.define public ENC_DEBOUNCE_CYCLES (SET_CYCLES + (JMP_CYCLES * ITERATIONS)) + +; Ensure that ENC_DEBOUNCE_CYCLES is a multiple of the number of cycles the +; wrap takes, which is currently 10 cycles, otherwise timing may be inaccurate + + +; Encoder Program +; -------------------------------------------------- +.program encoder + +.wrap_target +loop: + ; Copy the state-change timer from OSR, decrement it, and save it back + mov y, osr + jmp y-- osr_dec +osr_dec: + mov osr, y + ; takes 3 cycles + + ; Read the state of both encoder pins and check if they are different from the last state + jmp pin encA_was_high + mov isr, null + jmp read_encB +encA_was_high: + set y, 1 + mov isr, y +read_encB: + in pins, 1 + mov y, isr + jmp x!=y state_changed [1] + ; takes 7 cycles on both paths +.wrap + +state_changed: + ; Put the last state and the timer value into ISR alongside the current state, + ; and push that state to the system. Then override the last state with the current state + in x, 2 + mov x, ~osr ; invert the timer value to give a sensible value to the system + in x, 28 + push noblock ; this also clears isr + mov x, y + + ; Perform a delay to debounce switch inputs + set y, (ITERATIONS - 1) [SET_CYCLES - 1] +debounce_loop: + jmp y-- debounce_loop [JMP_CYCLES - 1] + + ; Initialise the timer, as an inverse, and decrement it to account for the time this setup takes + mov y, ~null + jmp y-- y_dec +y_dec: + mov osr, y + jmp loop [1] + ;takes 10 cycles, not counting whatever the debounce adds + + +; Initialisation Code +; -------------------------------------------------- +% c-sdk { +#include "hardware/clocks.h" + +static const uint8_t ENC_LOOP_CYCLES = encoder_wrap - encoder_wrap_target; + +//The time that the debounce takes, as the number of wrap loops that the debounce is equivalent to +static const uint8_t ENC_DEBOUNCE_TIME = ENC_DEBOUNCE_CYCLES / ENC_LOOP_CYCLES; + + +static inline void encoder_program_init(PIO pio, uint sm, uint offset, uint pinA, uint pinB, uint16_t divider) { + pio_sm_config c = encoder_program_get_default_config(offset); + + sm_config_set_jmp_pin(&c, pinA); + sm_config_set_in_pins(&c, pinB); + sm_config_set_in_shift(&c, false, false, 1); + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX); + pio_gpio_init(pio, pinA); + pio_gpio_init(pio, pinB); + gpio_pull_up(pinA); + gpio_pull_up(pinB); + pio_sm_set_consecutive_pindirs(pio, sm, pinA, 1, 0); + pio_sm_set_consecutive_pindirs(pio, sm, pinB, 1, 0); + sm_config_set_clkdiv_int_frac(&c, divider, 0); + pio_sm_init(pio, sm, offset, &c); +} + +static inline void encoder_program_start(PIO pio, uint sm, bool stateA, bool stateB) { + pio_sm_exec(pio, sm, pio_encode_set(pio_x, (uint)stateA << 1 | (uint)stateB)); + pio_sm_set_enabled(pio, sm, true); +} + +static inline void encoder_program_release(PIO pio, uint sm) { + pio_sm_set_enabled(pio, sm, false); + pio_sm_unclaim(pio, sm); +} +%} \ No newline at end of file