Modified encoder motors to read both A and B signals (quadrature encoder)

Author: sorre97

rotary_encoder/rotarydecoder.py

@@ -6,38 +6,74 @@ class RotaryDecoder:
 
    """ Class to decode mechanical rotary encoder pulses """
 
-   def __init__(self, pi, encoder_feedback_pin, callback):
+   def __init__(self, pi, feedback_pin_A, feedback_pin_B, callback):
 
       self._pi = pi
-      self._encoder_feedback_pin = encoder_feedback_pin   # encoder feedback pin
+      self._feedback_pin_A = feedback_pin_A   # encoder feedback pin A
+      self._feedback_pin_B = feedback_pin_B   # encoder feedback pin B
       self._callback = callback   # callback function on event
-      self._value = 0             # value of encoder feedback
+      self._direction = 0         # direction, forward = 1, backward = -1, steady = 0
+
+      self._levelA = 0  # value of encoder feedback pin A
+      self._levelB = 0  # value of encoder feedback pin B
 
       # setting up GPIO
-      self._pi.set_mode(encoder_feedback_pin, pigpio.INPUT)
-      self._pi.set_pull_up_down(encoder_feedback_pin, pigpio.PUD_UP)
+      self._pi.set_mode(feedback_pin_A, pigpio.INPUT)
+      self._pi.set_mode(feedback_pin_B, pigpio.INPUT)
+      self._pi.set_pull_up_down(feedback_pin_A, pigpio.PUD_UP)
+      self._pi.set_pull_up_down(feedback_pin_B, pigpio.PUD_UP)
+
+      # callback function on EITHER_EDGE for each pin
+      self._callback_triggerA = self._pi.callback(feedback_pin_A, pigpio.EITHER_EDGE, self._pulse)
+      self._callback_triggerB = self._pi.callback(feedback_pin_B, pigpio.EITHER_EDGE, self._pulse)
 
-      # callback function on EITHER_EDGE
-      self._callback_trigger = self._pi.callback(encoder_feedback_pin, pigpio.EITHER_EDGE, self._pulse)
+      self._lastGpio = None
 
    """ pulse is the callback function on EITHER_EDGE
-       it returns a 1 if the feedback square wave is on a falling edge 
-       It simply returns a 1 on FALLING EDGE, i've detached this class in order
-       to make it easier in case of extension with PIN A attached too """
-   def _pulse(self, pi, level, tick):
-      self._value = level
+       We have two feedback input from pin A and B (two train waves)
+       it returns a 1 if the square waves have A leading B because we're moving forward
+       It returns a -1 if the square waves have B leading A because we're moving backwards
+       In either case, A is staggered from B by (+-)pi/2 radiants
+       
+                +---------+         +---------+            0
+                |         |         |         |
+            A   |         |         |         |
+                |         |         |         |
+            ----+         +---------+         +----------+ 1   # A leading B
+                      +---------+         +---------+      0   # forward
+                      |         |         |         |
+            B         |         |         |         |
+                      |         |         |         |
+            +---------+         +---------+         +----- 1
+   """
+   def _pulse(self, gpio, level, tick):
+      # interrupt comes from pin A
+      if (gpio == self._feedback_pin_A):
+         self._levelA = level   # set level of squared wave (0, 1) on A
+      # interrupt comes from pin B
+      else:
+         self._levelB = level   # set level of squared wave (0, 1) on B
+
+      if (gpio != self._lastGpio): # debounce
+         self._lastGpio = gpio
 
-      # RAISING EDGE
-      if self._value == 1:
-          self._callback(1)
+         if (gpio == self._feedback_pin_A and level == 1):
+            if (self._levelB == 1):
+               self._callback(1) # A leading B, moving forward
+               self._direction = 1 # forward
+
+         elif (gpio == self._feedback_pin_B and level == 1):
+            if (self._levelA == 1):
+               self._callback(-1)   # B leading A, moving forward
+               self._direction = -1 # backwards
 
 
    def cancel(self):
 
       """
-      Cancel the rotary encoder decoder.
+      Cancel the rotary encoder decoder callbacks.
       """
-
-      self._callback_trigger.cancel()
+      self._callback_triggerA.cancel()
+      self._callback_triggerB.cancel()
 
 

rotary_encoder_tries/reset_motors.py renamed to rotary_encoder_tests/reset_motors.py

@@ -79,13 +79,13 @@ def _pulse(self, gpio, level, tick):
              |         |         |         |
          B   |         |         |         |
              |         |         |         |
-         ----+         +---------+         +---------+  1
+         ----+         +---------+         +----------+ 1
       """
 
       if gpio == self.gpioA:
          self.levA = level
       else:
-         self.levB = level;
+         self.levB = level
 
       if gpio != self.lastGpio: # debounce
          self.lastGpio = gpio

rotary_encoder_tries/rotary_encoder_PWM_test.py renamed to rotary_encoder_tests/rotary_encoder_PWM_test.py

@@ -0,0 +1,27 @@
+PIN_ENCODER_LEFT_A = 14
+PIN_ENCODER_LEFT_B = 6
+PIN_ENCODER_RIGHT_A = 15
+PIN_ENCODER_RIGHT_B = 12
+
+from time import sleep
+import pigpio
+
+pi = pigpio.pi()
+
+print("connected")
+
+pi.set_mode(PIN_ENCODER_LEFT_A, pigpio.INPUT)
+pi.set_mode(PIN_ENCODER_LEFT_B, pigpio.INPUT)
+pi.set_mode(PIN_ENCODER_RIGHT_A, pigpio.INPUT)
+pi.set_mode(PIN_ENCODER_RIGHT_B, pigpio.INPUT)
+
+pi.set_pull_up_down(PIN_ENCODER_LEFT_A, pigpio.PUD_UP)
+pi.set_pull_up_down(PIN_ENCODER_LEFT_B, pigpio.PUD_UP)
+pi.set_pull_up_down(PIN_ENCODER_RIGHT_A, pigpio.PUD_UP)
+pi.set_pull_up_down(PIN_ENCODER_RIGHT_B, pigpio.PUD_UP)
+
+
+while(True):
+	print("LEFT: A: %d, B: %d" % (pi.read(PIN_ENCODER_LEFT_A), pi.read(PIN_ENCODER_LEFT_B)))
+	print("RIGHT: A: %d, B: %d" % (pi.read(PIN_ENCODER_RIGHT_A), pi.read(PIN_ENCODER_RIGHT_B)))
+	sleep(.05)

rotary_encoder_tries/rotary_encoder_fixed_distance_test.py renamed to rotary_encoder_tests/rotary_encoder_fixed_distance_test.py

@@ -0,0 +1,45 @@
+PIN_ENCODER_LEFT_A = 14
+PIN_ENCODER_LEFT_B = 6
+PIN_ENCODER_RIGHT_A = 15
+PIN_ENCODER_RIGHT_B = 12
+
+
+if __name__ == "__main__":
+
+   import time
+   import pigpio
+
+   from rotarydecoder import RotaryDecoder
+
+   pos_left = 0
+   pos_right = 0
+
+   def callback_left(way):
+
+      global pos_left
+
+      pos_left += way
+
+      print("pos_LEFT={}".format(pos_left))
+
+   def callback_right(way):
+
+      global pos_right
+
+      pos_right += way
+
+      print("pos_RIGHT={}".format(pos_right))
+
+
+   # pi = pigpio.pi('coderbot.local')
+   pi = pigpio.pi()
+
+   decoder_left = RotaryDecoder(pi, PIN_ENCODER_LEFT_A, PIN_ENCODER_LEFT_B, callback_left)
+   decoder_right = RotaryDecoder(pi, PIN_ENCODER_RIGHT_A, PIN_ENCODER_RIGHT_B, callback_right)
+
+   time.sleep(1000)
+
+   decoder_left.cancel()
+   decoder_right.cancel()
+
+   pi.stop()