Simple Quadrature Reader for Raspberry Pi Pico

As featured in the XtendedGreg YouTube Live Stream: https://youtube.com/live/jklc2Aq9-1E

Introduction

This Gist has the code that we used to get the direction and speed of the quadrature output of a motor encoder. Quadrature is a simple two sensor mechanism that relies upon the order of offset to make that determination. This is a simplified example that counts the number of rotations and increments the counter or decrements the counter depending upon which direction it is rotating. Every second, it will print the number of cycles since the last time it was read to the console.

The Code

The majority of this code was taken from Peter Hinch's Micropython Samples: https://github.com/peterhinch/micropython-samples/blob/master/encoders/encoder_rp2.py

# encoder_rp2.py Uses the PIO for rapid response on RP2 chips (Pico)

# Copyright (c) 2022 Peter Hinch
# Released under the MIT License (MIT) - see LICENSE file
# Modifications added by XtendedGreg

# PIO and SM code written by Sandor Attila Gerendi (@sanyi)
# https://github.com/micropython/micropython/pull/6894

from machine import Pin
from array import array
import rp2
import time

# Closure enables Viper to retain state. Currently (V1.17) nonlocal doesn't
# work: https://github.com/micropython/micropython/issues/8086
# so using arrays.
def make_isr(pos):
    old_x = array('i', (0,))
    @micropython.viper
    def isr(sm):
        i = ptr32(pos)
        p = ptr32(old_x)
        while sm.rx_fifo():
            v : int = int(sm.get()) & 3
            x : int = v & 1
            y : int = v >> 1
            s : int = 1 if (x ^ y) else -1
            i[0] = i[0] + (s if (x ^ p[0]) else (0 - s))
            p[0] = x
    return isr

# Args:
# StateMachine no. (0-7): each instance must have a different sm_no.
# An initialised input Pin: this and the next pin are the encoder interface.
class Encoder:
    def __init__(self, sm_no, base_pin, scale=1):
        self.scale = scale
        self._pos = array("i", (0,))  # [pos]
        self.sm = rp2.StateMachine(sm_no, self.pio_quadrature, in_base=base_pin)
        self.sm.irq(make_isr(self._pos))  # Instantiate the closure
        self.sm.exec("set(y, 99)")  # Initialise y: guarantee different to the input
        self.sm.active(1)

    @rp2.asm_pio()
    def pio_quadrature(in_init=rp2.PIO.IN_LOW):
        wrap_target()
        label("again")
        in_(pins, 2)
        mov(x, isr)
        jmp(x_not_y, "push_data")
        mov(isr, null)
        jmp("again")
        label("push_data")
        push()
        irq(block, rel(0))
        mov(y, x)
        wrap()

    def position(self, value=None):
        if value is not None:
            self._pos[0] = round(value / self.scale)
        return self._pos[0] * self.scale

    def value(self, value=None):
        if value is not None:
            self._pos[0] = value
        return self._pos[0]
    
# Test with encoder on pins 2 and 3:
e = Encoder(0, Pin(3))
lastE = e.value()
while True:
    delta = e.value() - lastE
    if delta > 0:
        direction = "+"
    elif delta < 0:
        direction = "-"
    else:
        direction = "stopped"
    lastE = e.value()
    time.sleep(1)
    print("Speed: ", abs(delta), " - Direction: ", direction, "(",e.value(), ")")