From f1ac15a3276bffeb08eaa893ea85dba7583bda2b Mon Sep 17 00:00:00 2001
From: Peter Hinch <peter@hinch.me.uk>
Date: Sat, 1 Jan 2022 15:43:20 +0000
Subject: [PATCH] Add RP2 fast encoder.

---
 encoders/ENCODERS.md    |  4 ++-
 encoders/encoder_rp2.py | 73 +++++++++++++++++++++++++++++++++++++++++
 2 files changed, 76 insertions(+), 1 deletion(-)
 create mode 100644 encoders/encoder_rp2.py

diff --git a/encoders/ENCODERS.md b/encoders/ENCODERS.md
index 9c92606..85410a3 100644
--- a/encoders/ENCODERS.md
+++ b/encoders/ENCODERS.md
@@ -129,7 +129,9 @@ the rate at which callbacks occur.
  practice this is likely to need filtering to reduce jitter caused by
  imperfections in the encoder geometry. With a mechanical knob turned by an
  anthropoid ape it's debatable whether it produces anything useful :)
- 3. `encoder.py` An old Pyboard-specific version.
+ 3. `encoder_rp2.py` Version specific to Raspberry Pico RP2 chip. This uses the
+ PIO and Viper code to achieve fast response - upto ~10K transitions/s.
+ 4. `encoder.py` An old Pyboard-specific version.
 
 These were written for encoders producing logic outputs. For switches, adapt
 the pull definition to provide a pull up or pull down as required, or provide
diff --git a/encoders/encoder_rp2.py b/encoders/encoder_rp2.py
new file mode 100644
index 0000000..255a077
--- /dev/null
+++ b/encoders/encoder_rp2.py
@@ -0,0 +1,73 @@
+# 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
+
+# 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
+
+# Test with encoder on pins 2 and 3:
+#e = Encoder(0, Pin(2))
+
+#while True:    
+    #time.sleep(1)
+    #print(e.value())
+
+# 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]