1  import array
  2  import sys
  3  
  4  import adafruit_pioasm
  5  
  6  if sys.implementation.name == 'circuitpython':
  7      from rp2pio import StateMachine
  8      _n_read = 9
  9  else:
 10      from adafruit_rp1pio import StateMachine
 11      _n_read = 17
 12  
 13  _program = adafruit_pioasm.Program("""
 14  ;
 15  ; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
 16  ;
 17  ; SPDX-License-Identifier: BSD-3-Clause
 18  ;
 19  .pio_version 0 // only requires PIO version 0
 20  
 21  .program quadrature_encoder
 22  
 23  ; the code must be loaded at address 0, because it uses computed jumps
 24  .origin 0
 25  
 26  
 27  ; the code works by running a loop that continuously shifts the 2 phase pins into
 28  ; ISR and looks at the lower 4 bits to do a computed jump to an instruction that
 29  ; does the proper "do nothing" | "increment" | "decrement" action for that pin
 30  ; state change (or no change)
 31  
 32  ; ISR holds the last state of the 2 pins during most of the code. The Y register
 33  ; keeps the current encoder count and is incremented / decremented according to
 34  ; the steps sampled
 35  
 36  ; the program keeps trying to write the current count to the RX FIFO without
 37  ; blocking. To read the current count, the user code must drain the FIFO first
 38  ; and wait for a fresh sample (takes ~4 SM cycles on average). The worst case
 39  ; sampling loop takes 10 cycles, so this program is able to read step rates up
 40  ; to sysclk / 10  (e.g., sysclk 125MHz, max step rate = 12.5 Msteps/sec)
 41  
 42  ; 00 state
 43      jmp update    ; read 00
 44      jmp decrement ; read 01
 45      jmp increment ; read 10
 46      jmp update    ; read 11
 47  
 48  ; 01 state
 49      jmp increment ; read 00
 50      jmp update    ; read 01
 51      jmp update    ; read 10
 52      jmp decrement ; read 11
 53  
 54  ; 10 state
 55      jmp decrement ; read 00
 56      jmp update    ; read 01
 57      jmp update    ; read 10
 58      jmp increment ; read 11
 59  
 60  ; to reduce code size, the last 2 states are implemented in place and become the
 61  ; target for the other jumps
 62  
 63  ; 11 state
 64      jmp update    ; read 00
 65      jmp increment ; read 01
 66  decrement:
 67      ; note: the target of this instruction must be the next address, so that
 68      ; the effect of the instruction does not depend on the value of Y. The
 69      ; same is true for the "jmp y--" below. Basically "jmp y--, <next addr>"
 70      ; is just a pure "decrement y" instruction, with no other side effects
 71      jmp y--, update ; read 10
 72  
 73      ; this is where the main loop starts
 74  .wrap_target
 75  update:
 76      mov isr, y      ; read 11
 77      push noblock
 78  
 79  sample_pins:
 80      ; we shift into ISR the last state of the 2 input pins (now in OSR) and
 81      ; the new state of the 2 pins, thus producing the 4 bit target for the
 82      ; computed jump into the correct action for this state. Both the PUSH
 83      ; above and the OUT below zero out the other bits in ISR
 84      out isr, 2
 85      in pins, 2
 86  
 87      ; save the state in the OSR, so that we can use ISR for other purposes
 88      mov osr, isr
 89      ; jump to the correct state machine action
 90      mov pc, isr
 91  
 92      ; the PIO does not have a increment instruction, so to do that we do a
 93      ; negate, decrement, negate sequence
 94  increment:
 95      mov y, ~y
 96      jmp y--, increment_cont
 97  increment_cont:
 98      mov y, ~y
 99  .wrap    ; the .wrap here avoids one jump instruction and saves a cycle too
100  """)
101  
102  _zero_y = adafruit_pioasm.assemble("set y 0")
103  
104  class IncrementalEncoder:
105      def __init__(self, pin_a, pin_b=None, divisor=4):
106          """Create an incremental encoder on pin_a and the next higher pin
107  
108          Always operates in "x4" mode (one count per quadrature edge)
109  
110          Assumes but does not check that pin_b is one above pin_a."""
111          self._sm = StateMachine(
112              _program.assembled,
113              frequency=0,
114              init=_zero_y,
115              first_in_pin=pin_a,
116              in_pin_count=2,
117              pull_in_pin_up=0x3,
118              auto_push=True,
119              push_threshold=32,
120              in_shift_right=False,
121              **_program.pio_kwargs
122          )
123          self._buffer = array.array('i',[0] * _n_read)
124          self.divisor = divisor
125          self._position = 0
126  
127      def deinit(self):
128          self._sm.deinit()
129  
130      @property
131      def position(self):
132          self._sm.readinto(self._buffer) # read N stale values + 1 fresh value
133          raw_position = self._buffer[-1]
134          delta = int((raw_position - self._position * self.divisor) / self.divisor)
135          self._position += delta
136          return self._position
137  
138  if __name__ == '__main__':
139      import board
140      # D17/D18 on header pins 11/12
141      # GND on header pin 6/9
142      # +5V on header pins 2/4
143      q = IncrementalEncoder(board.D17)
144      old_position = q.position
145      while True:
146          position = q.position
147          if position != old_position:
148              delta = position - old_position
149              print(f"{position:8d} {delta=}")
150          old_position = position