Support multiple non-blocking commands simultaneously

Author: chrisruk

buildhat/motors.py

@@ -230,7 +230,7 @@ def run_for_degrees(self, degrees, speed=None, blocking=True):
         if not (speed >= -100 and speed <= 100):
             raise MotorError("Invalid Speed")
         if not blocking:
-            Device._instance.mqueue.put((self._run_for_degrees, (degrees, speed)))
+            self._queue((self._run_for_degrees, (degrees, speed)))
         else:
             self._run_for_degrees(degrees, speed)
 
@@ -251,7 +251,7 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes
         if degrees < -180 or degrees > 180:
             raise MotorError("Invalid angle")
         if not blocking:
-            Device._instance.mqueue.put((self._run_to_position, (degrees, speed, direction)))
+            self._queue((self._run_to_position, (degrees, speed, direction)))
         else:
             self._run_to_position(degrees, speed, direction)
 
@@ -282,7 +282,7 @@ def run_for_seconds(self, seconds, speed=None, blocking=True):
         if not (speed >= -100 and speed <= 100):
             raise MotorError("Invalid Speed")
         if not blocking:
-            Device._instance.mqueue.put((self._run_for_seconds, (seconds, speed)))
+            self._queue((self._run_for_seconds, (seconds, speed)))
         else:
             self._run_for_seconds(seconds, speed)
 
@@ -441,6 +441,9 @@ def release(self, value):
             raise MotorError("Must pass boolean")
         self._release = value
 
+    def _queue(self, cmd):
+        Device._instance.motorqueue[self.port].put(cmd)
+
 
 class MotorPair:
     """Pair of motors

buildhat/serinterface.py

@@ -88,6 +88,7 @@ def __init__(self, firmware, signature, version, device="/dev/serial0", debug=Fa
         self.pulsecond = []
         self.rampcond = []
         self.vincond = []
+        self.motorqueue = []
         self.fin = False
         self.running = True
         if debug:
@@ -100,6 +101,7 @@ def __init__(self, firmware, signature, version, device="/dev/serial0", debug=Fa
             self.portcond.append([])
             self.pulsecond.append([])
             self.rampcond.append([])
+            self.motorqueue.append(queue.Queue())
 
         self.ser = serial.Serial(device, 115200, timeout=5)
         # Check if we're in the bootloader or the firmware
@@ -149,10 +151,10 @@ def __init__(self, firmware, signature, version, device="/dev/serial0", debug=Fa
         self.cb.daemon = True
         self.cb.start()
 
-        self.mqueue = queue.Queue()
-        self.ml = threading.Thread(target=self.motorloop, args=(self.mqueue,))
-        self.ml.daemon = True
-        self.ml.start()
+        for q in self.motorqueue:
+            ml = threading.Thread(target=self.motorloop, args=(q,))
+            ml.daemon = True
+            ml.start()
 
         # Drop timeout value to 1s
         listevt = threading.Event()
@@ -270,7 +272,8 @@ def shutdown(self):
             self.running = False
             self.th.join()
             self.cbqueue.put(())
-            self.mqueue.put((None, None))
+            for q in self.motorqueue:
+                q.put((None, None))
             self.cb.join()
             turnoff = ""
             for p in range(4):

test/motors.py

@@ -39,6 +39,34 @@ def test_nonblocking(self):
                 diff = abs((last - pos1 + 180) % 360 - 180)
                 self.assertLess(diff, 10)
 
+    def test_nonblocking_multiple(self):
+        """Test motor nonblocking mode"""
+        m1 = Motor('A')
+        m2 = Motor('B')
+        last = 0
+        for delay in [1, 0]:
+            for _ in range(3):
+                m1.run_to_position(90, blocking=False)
+                m2.run_to_position(90, blocking=False)
+                time.sleep(delay)
+                m1.run_to_position(90, blocking=False)
+                m2.run_to_position(90, blocking=False)
+                time.sleep(delay)
+                m1.run_to_position(90, blocking=False)
+                m2.run_to_position(90, blocking=False)
+                time.sleep(delay)
+                m1.run_to_position(last, blocking=False)
+                m2.run_to_position(last, blocking=False)
+                time.sleep(delay)
+                # Wait for a bit, before reading last position
+                time.sleep(7)
+                pos1 = m1.get_aposition()
+                diff = abs((last - pos1 + 180) % 360 - 180)
+                self.assertLess(diff, 10)
+                pos2 = m2.get_aposition()
+                diff = abs((last - pos2 + 180) % 360 - 180)
+                self.assertLess(diff, 10)
+
     def test_position(self):
         """Test motor goes to desired position"""
         m = Motor('A')