Created a quadrature encoder reader using the Pico's PIO

drivers/CMakeLists.txt

@@ -29,3 +29,4 @@ add_subdirectory(hub75)
 add_subdirectory(uc8151)
 add_subdirectory(pwm)
 add_subdirectory(servo)
+add_subdirectory(encoder-pio)

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)

drivers/encoder-pio/capture.cpp

@@ -0,0 +1,70 @@
+#include <math.h>
+#include <cfloat>
+#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;
+  }
+  ////////////////////////////////////////////////////////////////////////////////////////////////////
+  ////////////////////////////////////////////////////////////////////////////////////////////////////
+}

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;
+  };
+
+}

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)

drivers/encoder-pio/encoder.cpp

@@ -0,0 +1,333 @@
+#include <math.h>
+#include <climits>
+#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;
+      }
+    }
+  }
+  ////////////////////////////////////////////////////////////////////////////////////////////////////
+  ////////////////////////////////////////////////////////////////////////////////////////////////////
+}

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

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