1- from machine import Pin
2- from micropython import const
1+ # Modified from https://forum.micropython.org/viewtopic.php?t=12277&p=66659
32
4- class Encoder :
5- def __init__ (self , aPin :int , bPin :int , ticks_per_revolution :int = 146.25 ):
6- # TODO: Look into PIO implementation as to not take CPU time
7- self .currentPosition = 0
8- self .ticks_per_rev = ticks_per_revolution
9- # Set pins as inputs
10- self ._aPin = Pin (aPin , Pin .IN )
11- self ._bPin = Pin (bPin , Pin .IN )
12- # Set up both pins with an interrupt to update the encoder count on any change
13- self ._aPin .irq (trigger = Pin .IRQ_RISING , handler = lambda pin :self ._isr ())
14- #self._bPin.irq(trigger=Pin.IRQ_RISING | Pin.IRQ_FALLING, handler=lambda pin:self.isr("b"))
15-
16- #self.prevAState = 0
17- #self.prevBState = 0
18- # Use a lookup table based on the pin transitions to figure out if we are running forwards or backwards
19- #self._ENCODER_LOOKUP_TABLE = [ 0, 1, -1, 0, -1, 0, 0, 1, 1, 0, 0, -1, 0, -1, 1, 0 ]
3+ import machine
4+ import rp2
5+ import time
206
21-
22- def get_position (self ):
23- """
24- : return: The position of the encoded motor, in revolutions, relative to the last time reset was called.
25- : rtype: float
26- """
27- return self .currentPosition / self .ticks_per_rev
7+ class Encoder :
8+ gearRatio = (30 / 14 ) * (28 / 16 ) * (36 / 9 ) * (26 / 8 ) # 48.75
9+ ticksPerMotorRotation = 12
10+ ticksPerShaftRotation = ticksPerMotorRotation * gearRatio # 585
11+
12+ def __init__ (self , index , encAPin , encBPin ):
13+ if (abs (encAPin - encBPin ) != 1 ):
14+ raise Exception ("Encoder pins must be successive!" )
15+ basePin = machine .Pin (min (encAPin , encBPin ))
16+ self .sm = rp2 .StateMachine (index , self ._encoder , in_base = basePin )
17+ self .reset_encoder_position ()
18+ self .sm .active (1 )
2819
29- # return position in ticks
30- def get_position_ticks (self ):
31- """
32- : return: The position of the encoder, in ticks, relative to the last time reset was called.
33- : rtype: int
34- """
35- return self .currentPosition
36-
3720 def reset_encoder_position (self ):
38- """
39- Resets the encoder position back to zero.
40- """
41- self .currentPosition = 0
42-
43- def _isr (self ):
44- # aState = self._aPin.value()
45- bState = self ._bPin .value ()
46-
47- # At rising edge of A. If B is low, we are going forwards, if B is high, we are going backwards
48- if bState == 0 :
49- self .currentPosition -= 1
50- else :
51- self .currentPosition += 1
52-
53- #print(self.currentPosition)
21+ # It's possible for this to cause issues if in the middle of inrementing
22+ # or decrementing, but the result should only be off by 1. If that's a
23+ # problem, an alternative solution is to stop the state machine, then
24+ # reset both x and the program counter. But that's excessive.
25+ self .sm .exec ("set(x, 0)" )
26+
27+ def get_position_ticks (self ):
28+ ticks = self .sm .get ()
29+ if (ticks > 2 ** 31 ):
30+ ticks -= 2 ** 32
31+ return ticks
32+
33+ def get_position (self ):
34+ return self .getTicks () / self .ticksPerShaftRotation
5435
36+ @rp2 .asm_pio (in_shiftdir = rp2 .PIO .SHIFT_LEFT , out_shiftdir = rp2 .PIO .SHIFT_RIGHT )
37+ def _encoder ():
38+ # Register descriptions:
39+ # X - Encoder count, as a 32-bit number
40+ # OSR - Previous pin values, only last 2 bits are used
41+ # ISR - Push encoder count, and combine pin states together
5542
56- # index = self.prevAState*8 + self.prevBState*4 + aState*2 + bState
57-
58- # print(text, aState, bState, index)
59-
60- # self.currentPosition += self._ENCODER_LOOKUP_TABLE[index]
61-
62- # # DEBUG ONLY
63- # #print(f"{index}, {self._ENCODER_LOOKUP_TABLE[index]}")
64-
65- # self.prevAState = aState
66- # self.prevBState = bState
43+ # Jump table
44+ # The program counter is moved to memory address 0000 - 1111, based
45+ # on the previous (left 2 bits) and current (right bits) pin states
46+ jmp ("read" ) # 00 -> 00 No change, continue
47+ jmp ("decr" ) # 00 -> 01 Reverse, decrement count
48+ jmp ("incr" ) # 00 -> 10 Forward, increment count
49+ jmp ("read" ) # 00 -> 11 Impossible, continue
50+
51+ jmp ("incr" ) # 01 -> 00 Forward, increment count
52+ jmp ("read" ) # 01 -> 01 No change, continue
53+ jmp ("read" ) # 01 -> 10 Impossible, continue
54+ jmp ("decr" ) # 01 -> 11 Reverse, decrement count
55+
56+ jmp ("decr" ) # 10 -> 00 Reverse, decrement count
57+ jmp ("read" ) # 10 -> 01 Impossible, continue
58+ jmp ("read" ) # 10 -> 10 No change, continue
59+ jmp ("incr" ) # 10 -> 11 Forward, increment count
60+
61+ jmp ("read" ) # 11 -> 00 Impossible, continue
62+ jmp ("incr" ) # 11 -> 01 Forward, increment count
63+ jmp ("decr" ) # 11 -> 10 Reverse, decrement count
64+ jmp ("read" ) # 11 -> 11 No change, continue
65+
66+ label ("read" )
67+ mov (osr , isr ) # Store previous pin states in OSR
68+ mov (isr , x ) # Copy encoder count to ISR
69+ push (noblock ) # Push count to RX buffer, and reset ISR to 0
70+ out (isr , 2 ) # Shift previous pin states into ISR
71+ in_ (pins , 2 ) # Shift current pin states into ISR
72+ mov (pc , isr ) # Move PC to jump table to determine what to do next
73+
74+ label ("decr" ) # There is no explicite increment intruction, but X can be
75+ jmp (x_dec , "decr_nop" ) # decremented in the jump instruction. So we use that and jump
76+ label ("decr_nop" ) # to the next instruction, which is equivalent to just decrementing
77+ jmp ("read" )
78+
79+ label ("incr" ) # There is no explicite increment intruction, but X can be
80+ mov (x , invert (x )) # decremented in the jump instruction. So we invert X, decrement,
81+ jmp (x_dec , "incr_nop" ) # then invert again - this is equivalent to incrementing.
82+ label ("incr_nop" )
83+ mov (x , invert (x ))
84+ jmp ("read" )
85+
86+ # Fill remaining instruction memory with jumps to ensure nothing bad happens
87+ # For some reason, weird behavior happens if the instruction memory isn't full
88+ jmp ("read" )
89+ jmp ("read" )
90+ jmp ("read" )
91+ jmp ("read" )
0 commit comments