Wheels axel class that handles two motors at once, ended single rotary encoder class

Author: sorre97

coderbot.py

@@ -365,44 +365,44 @@ def adjust_power(self, power_delta):
     class TwinMotorsEncoder(object):
         def __init__(self, apigpio, pin_enable, pin_forward_left, pin_backward_left, pin_encoder_left, pin_forward_right, pin_backward_right, pin_encoder_right):
             self._straight = False
-            self._running = False
-            self._encoder_sem = threading.Condition()
-            self._motor_left = CoderBot.MotorEncoder(self, apigpio, pin_enable, pin_forward_left, pin_backward_left, pin_encoder_left)
-            self._motor_right = CoderBot.MotorEncoder(self, apigpio, pin_enable, pin_forward_right, pin_backward_right, pin_encoder_right)
-
-        def exit(self):
-            self._motor_left.exit()
-            self._motor_right.exit()
-
-        def control(self, power_left=100.0, power_right=100.0, elapse=-1, speed_left=-1, speed_right=-1, steps_left=-1, steps_right=-1):
-            self._straight = power_left == power_right and speed_left == speed_right and steps_left == steps_right
-
-            if steps_left >= 0 or steps_right >= 0:
-                self._encoder_sem.acquire()
-
-            self._motor_left.control(power=power_left, elapse=-1, speed=speed_left, steps=steps_left)
-            self._motor_right.control(power=power_right, elapse=-1, speed=speed_right, steps=steps_right)
-            self._running = True
-
-            if elapse > 0:
-                time.sleep(elapse)
-                self.stop()
-
-            if steps_left >= 0 or steps_right >= 0:
-                self._encoder_sem.wait()
-                self._encoder_sem.release()
-                self.stop()
-
-        def stop(self):
-            self._motor_left.stop()
-            self._motor_right.stop()
-            self._running = False
-
-        def distance(self):
-            return (self._motor_left.distance() + self._motor_right.distance()) / 2
-
-        def speed(self):
-            return (self._motor_left.speed() + self._motor_right.speed()) / 2
+            #self._running = False
+            #self._encoder_sem = threading.Condition()
+            #self._motor_left = CoderBot.MotorEncoder(self, apigpio, pin_enable, pin_forward_left, pin_backward_left, pin_encoder_left)
+            #self._motor_right = CoderBot.MotorEncoder(self, apigpio, pin_enable, pin_forward_right, pin_backward_right, pin_encoder_right)
+
+        #def exit(self):
+        #    self._motor_left.exit()
+        #    self._motor_right.exit()
+
+        # def control(self, power_left=100.0, power_right=100.0, elapse=-1, speed_left=-1, speed_right=-1, steps_left=-1, steps_right=-1):
+        #     self._straight = power_left == power_right and speed_left == speed_right and steps_left == steps_right
+        #
+        #     if steps_left >= 0 or steps_right >= 0:
+        #         self._encoder_sem.acquire()
+        #
+        #     self._motor_left.control(power=power_left, elapse=-1, speed=speed_left, steps=steps_left)
+        #     self._motor_right.control(power=power_right, elapse=-1, speed=speed_right, steps=steps_right)
+        #     self._running = True
+        #
+        #     if elapse > 0:
+        #         time.sleep(elapse)
+        #         self.stop()
+        #
+        #     if steps_left >= 0 or steps_right >= 0:
+        #         self._encoder_sem.wait()
+        #         self._encoder_sem.release()
+        #         self.stop()
+
+        #def stop(self):
+        #    self._motor_left.stop()
+        #    self._motor_right.stop()
+        #    self._running = False
+
+        #def distance(self):
+        #    return (self._motor_left.distance() + self._motor_right.distance()) / 2
+
+        #def speed(self):
+        #    return (self._motor_left.speed() + self._motor_right.speed()) / 2
 
         def _cb_encoder(self, motor, gpio, level, tick):
             if (self._straight and self._running and not self._motor_left.stopping() and not self._motor_right.stopping() and

rotary_encoder/motorencoder.py

@@ -15,9 +15,9 @@ class MotorEncoder:
         concurrency problems on GPIO READ/WRITE """
 
     # default constructor
-    def __init__(self, pigpio, enable_pin, forward_pin, backward_pin, encoder_feedback_pin):
+    def __init__(self, pi, enable_pin, forward_pin, backward_pin, encoder_feedback_pin):
         # setting pin variables
-        self._pigpio = pigpio
+        self._pi = pi
         self._enable_pin = enable_pin
         self._forward_pin = forward_pin
         self._backward_pin = backward_pin
@@ -33,7 +33,7 @@ def __init__(self, pigpio, enable_pin, forward_pin, backward_pin, encoder_feedba
 
         # other
         self._motor_lock = threading.RLock()
-        self._rotary_decoder = RotaryDecoder(pigpio, encoder_feedback_pin, self.rotary_callback)
+        self._rotary_decoder = RotaryDecoder(pi, encoder_feedback_pin, self.rotary_callback)
 
     # GETTERS
     # ticks
@@ -66,16 +66,20 @@ def is_moving(self):
 
     def control(self, power=100.0, time_elapse=0):
         self._motor_lock.acquire()  # acquiring lock
+
         self._direction = 1 if power > 0 else -1  # setting direction according to speed
         self._power = power
         self.stop()  # stopping motor to initialize new movement
+        self._pi.write(self._enable_pin, True)  # enabling motors
 
         # going forward
         if (self._direction):
-            self._pigpio.set_PWM_dutycycle(self._forward_pin, abs(power))
+            self._pi.set_PWM_dutycycle(self._forward_pin, abs(power))
+            self._direction = 1
         # going bacakward
         else:
-            self._pigpio.set_PWM_dutycycle(self._backward_pin, abs(power))
+            self._pi.set_PWM_dutycycle(self._backward_pin, abs(power))
+            self._direction = -1
 
         self._is_moving = True
 
@@ -95,8 +99,8 @@ def stop(self):
         self._motor_lock.acquire()
 
         # stopping motor
-        self._pigpio.write(self._backward_pin, 0)
-        self._pigpio.write(self._forward_pin, 0)
+        self._pi.write(self._backward_pin, 0)
+        self._pi.write(self._forward_pin, 0)
 
         # returning state variables to consistent state
         self._distance = 0       # resetting distance travelled
@@ -109,7 +113,7 @@ def stop(self):
         # releasing lock
         self._motor_lock.release()
 
-    # CALLBACKS
+    # CALLBACK
     """ The callback function rotary_callback is called on FALLING_EDGE by the
         rotary_decoder with a parameter value of 1 (1 new tick)
         - Gearbox ratio: 120:1 (1 wheel revolution = 120 motor revolution)
@@ -127,6 +131,7 @@ def rotary_callback(self, tick):
         self._ticks += tick  # updating ticks
         # self._encoder_speed = ?    # encoder speed (mm/s)
         self._distance = self._ticks * 0.196  # (mm) travelled
+        self._motor_lock.release()
 
     # callback cancelling
     def cancel_callback(self):

rotary_encoder/rotarydecoder.py

@@ -6,30 +6,30 @@ class RotaryDecoder:
 
    """ Class to decode mechanical rotary encoder pulses """
 
-   def __init__(self, pigpio, encoder_feedback_pin, callback):
+   def __init__(self, pi, encoder_feedback_pin, callback):
 
-      self._pigpio = pigpio
+      self._pi = pi
       self._encoder_feedback_pin = encoder_feedback_pin   # encoder feedback pin
       self._callback = callback   # callback function on event
       self._value = 0             # value of encoder feedback
 
       # setting up GPIO
-      self._pigpio.set_mode(encoder_feedback_pin, pigpio.INPUT)
-      self._pigpio.set_pull_up_down(encoder_feedback_pin, pigpio.PUD_UP)
+      self._pi.set_mode(encoder_feedback_pin, pigpio.INPUT)
+      self._pi.set_pull_up_down(encoder_feedback_pin, pigpio.PUD_UP)
 
       # callback function on EITHER_EDGE
-      self._callback_trigger = self._pigpio.callback(encoder_feedback_pin, pigpio.EITHER_EDGE, self._pulse)
+      self._callback_trigger = self._pi.callback(encoder_feedback_pin, pigpio.EITHER_EDGE, self._pulse)
 
    """ 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, level):
-      self.levB = level;
+   def _pulse(self, pi, level, tick):
+      self._value = level
 
       # RAISING EDGE
-      if self.levB == 1:
-          self.callback(1)
+      if self._value == 1:
+          self._callback(1)
 
 
    def cancel(self):
@@ -38,6 +38,6 @@ def cancel(self):
       Cancel the rotary encoder decoder.
       """
 
-      self.callback_trigger.cancel()
+      self._callback_trigger.cancel()
 
 

rotary_encoder/wheelsaxel.py

@@ -1,7 +1,77 @@
 import pigpio
+import threading
+from time import sleep
 
 from motorencoder import MotorEncoder
 
 class WheelsAxel:
-    pass
+    """ Class that handles both motor encoders, left and right
 
+        This class works like a wheels axle, coordinating left and right
+        wheels at the same time
+
+        It also tries to handle the inconsistent tension on wheels
+        that makes one wheel go slower than the other """
+
+    def __init__(self, pi, enable_pin,
+                 left_forward_pin, left_backward_pin, left_encoder_feedback_pin,
+                 right_forward_pin, right_backward_pin, right_encoder_feedback_pin):
+
+        # state variables
+        self._is_moving = False
+
+        # left motor
+        self._left_motor = MotorEncoder(pi,
+                                        enable_pin,
+                                        left_forward_pin,
+                                        left_backward_pin,
+                                        left_encoder_feedback_pin)
+        # right motor
+        self._right_motor = MotorEncoder(pi,
+                                         enable_pin,
+                                         right_forward_pin,
+                                         right_backward_pin,
+                                         right_encoder_feedback_pin)
+
+        # other
+        self._wheelsAxle_lock = threading.Condition() # race condition lock
+
+    # STATE GETTERS
+    """ Distance and speed are calculated by a mean of the feedback
+        from the two motors """
+    # distance
+    def distance(self):
+        l_dist = self._left_motor.distance()
+        r_dist = self._right_motor.distance()
+        return (l_dist + r_dist) / 2
+
+    #speed
+    def speed(self):
+        l_speed = self._left_motor.speed()
+        r_speed = self._right_motor.speed()
+        return (l_speed + r_speed) / 2
+
+    # MOVEMENT
+    def control(self, power_left = 100, power_right = 100, time_elapse = 0):
+
+        self._left_motor.control(power_left, time_elapse)
+        self._right_motor.control(power_right, time_elapse)
+        self._is_moving = True
+
+        if(time_elapse > 0):
+            sleep(time_elapse)
+            self.stop()
+
+    """ The stop function calls the two stop functions of the two
+        correspondent motors """
+    def stop(self):
+        self._left_motor.stop()
+        self._right_motor.stop()
+        self._is_moving = False
+
+    # CALLBACK
+
+
+    def cancel_callback(self):
+        self._right_motor.cancel_callback()
+        self._left_motor.cancel_callback()