Test ordering of motor run_to_position commands (#176)

* Test ordering of motor run_to_position commands

* Restructure to use Future

* Use Future to obtain voltage of build HAT

* Support multiple non-blocking commands simultaneously

* Enable 'del' to function correctly

* Bump threshold temporarily

* Refactor variable names

Author: chrisruk

buildhat/devices.py

@@ -3,6 +3,7 @@
 import os
 import sys
 import weakref
+from concurrent.futures import Future
 
 from .exc import DeviceError
 from .serinterface import BuildHAT
@@ -202,11 +203,10 @@ def get(self):
             idx = self._combimode
         else:
             raise DeviceError("Not in simple or combimode")
+        ftr = Future()
+        self._hat.portftr[self.port].append(ftr)
         self._write(f"port {self.port} ; selonce {idx}\r")
-        # wait for data
-        with Device._instance.portcond[self.port]:
-            Device._instance.portcond[self.port].wait()
-        return self._conn.data
+        return ftr.result()
 
     def mode(self, modev):
         """Set combimode or simple mode

buildhat/hat.py

@@ -1,5 +1,7 @@
 """HAT handling functionality"""
 
+from concurrent.futures import Future
+
 from .devices import Device
 
 
@@ -45,11 +47,10 @@ def get_vin(self):
         :return: Voltage on the input power jack
         :rtype: float
         """
+        ftr = Future()
+        Device._instance.vinftr.append(ftr)
         Device._instance.write(b"vin\r")
-        with Device._instance.vincond:
-            Device._instance.vincond.wait()
-
-        return Device._instance.vin
+        return ftr.result()
 
     def _set_led(self, intmode):
         if isinstance(intmode, int) and intmode >= -1 and intmode <= 3:

buildhat/motors.py

@@ -3,6 +3,7 @@
 import threading
 import time
 from collections import deque
+from concurrent.futures import Future
 from enum import Enum
 from threading import Condition
 
@@ -205,9 +206,10 @@ def _run_positional_ramp(self, pos, newpos, speed):
         cmd = (f"port {self.port}; combi 0 {self._combi} ; select 0 ; selrate {self._interval}; "
                f"pid {self.port} 0 1 s4 0.0027777778 0 5 0 .1 3; "
                f"set ramp {pos} {newpos} {dur} 0\r")
+        ftr = Future()
+        self._hat.rampftr[self.port].append(ftr)
         self._write(cmd)
-        with self._hat.rampcond[self.port]:
-            self._hat.rampcond[self.port].wait()
+        ftr.result()
         if self._release:
             time.sleep(0.2)
             self.coast()
@@ -228,9 +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:
-            th = threading.Thread(target=self._run_for_degrees, args=(degrees, speed))
-            th.daemon = True
-            th.start()
+            self._queue((self._run_for_degrees, (degrees, speed)))
         else:
             self._run_for_degrees(degrees, speed)
 
@@ -251,9 +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:
-            th = threading.Thread(target=self._run_to_position, args=(degrees, speed, direction))
-            th.daemon = True
-            th.start()
+            self._queue((self._run_to_position, (degrees, speed, direction)))
         else:
             self._run_to_position(degrees, speed, direction)
 
@@ -262,9 +260,10 @@ def _run_for_seconds(self, seconds, speed):
         cmd = (f"port {self.port} ; combi 0 {self._combi} ; select 0 ; selrate {self._interval}; "
                f"pid {self.port} 0 0 s1 1 0 0.003 0.01 0 100; "
                f"set pulse {speed} 0.0 {seconds} 0\r")
+        ftr = Future()
+        self._hat.pulseftr[self.port].append(ftr)
         self._write(cmd)
-        with self._hat.pulsecond[self.port]:
-            self._hat.pulsecond[self.port].wait()
+        ftr.result()
         if self._release:
             self.coast()
         self._runmode = MotorRunmode.NONE
@@ -283,9 +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:
-            th = threading.Thread(target=self._run_for_seconds, args=(seconds, speed))
-            th.daemon = True
-            th.start()
+            self._queue((self._run_for_seconds, (seconds, speed)))
         else:
             self._run_for_seconds(seconds, speed)
 
@@ -444,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

@@ -84,23 +84,24 @@ def __init__(self, firmware, signature, version, device="/dev/serial0", debug=Fa
         self.cond = Condition()
         self.state = HatState.OTHER
         self.connections = []
-        self.portcond = []
-        self.pulsecond = []
-        self.rampcond = []
+        self.portftr = []
+        self.pulseftr = []
+        self.rampftr = []
+        self.vinftr = []
+        self.motorqueue = []
         self.fin = False
         self.running = True
-        self.vincond = Condition()
-        self.vin = None
         if debug:
             tmp = tempfile.NamedTemporaryFile(suffix=".log", prefix="buildhat-", delete=False)
             logging.basicConfig(filename=tmp.name, format='%(asctime)s %(message)s',
                                 level=logging.DEBUG)
 
         for _ in range(4):
             self.connections.append(Connection())
-            self.portcond.append(Condition())
-            self.pulsecond.append(Condition())
-            self.rampcond.append(Condition())
+            self.portftr.append([])
+            self.pulseftr.append([])
+            self.rampftr.append([])
+            self.motorqueue.append(queue.Queue())
 
         self.ser = serial.Serial(device, 115200, timeout=5)
         # Check if we're in the bootloader or the firmware
@@ -150,6 +151,11 @@ def __init__(self, firmware, signature, version, device="/dev/serial0", debug=Fa
         self.cb.daemon = True
         self.cb.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()
         self.ser.timeout = 1
@@ -266,6 +272,8 @@ def shutdown(self):
             self.running = False
             self.th.join()
             self.cbqueue.put(())
+            for q in self.motorqueue:
+                q.put((None, None))
             self.cb.join()
             turnoff = ""
             for p in range(4):
@@ -278,6 +286,20 @@ def shutdown(self):
             self.write(f"{turnoff}\r".encode())
             self.write(b"port 0 ; select ; port 1 ; select ; port 2 ; select ; port 3 ; select ; echo 0\r")
 
+    def motorloop(self, q):
+        """Event handling for non-blocking motor commands
+
+        :param q: Queue of motor functions
+        """
+        while self.running:
+            func, data = q.get()
+            if func is None:
+                break
+            else:
+                func(*data)
+                func = None  # Necessary for 'del' to function correctly on motor object
+                data = None
+
     def callbackloop(self, q):
         """Event handling for callbacks
 
@@ -330,11 +352,11 @@ def loop(self, cond, uselist, q, listevt):
                     if uselist and listevt.is_set():
                         count += 1
                 elif cmp(msg, BuildHAT.RAMPDONE):
-                    with self.rampcond[portid]:
-                        self.rampcond[portid].notify()
+                    ftr = self.rampftr[portid].pop()
+                    ftr.set_result(True)
                 elif cmp(msg, BuildHAT.PULSEDONE):
-                    with self.pulsecond[portid]:
-                        self.pulsecond[portid].notify()
+                    ftr = self.pulseftr[portid].pop()
+                    ftr.set_result(True)
 
             if uselist and count == 4:
                 with cond:
@@ -362,11 +384,13 @@ def runit():
                 if callit is not None:
                     q.put((callit, newdata))
                 self.connections[portid].data = newdata
-                with self.portcond[portid]:
-                    self.portcond[portid].notify()
+                try:
+                    ftr = self.portftr[portid].pop()
+                    ftr.set_result(newdata)
+                except IndexError:
+                    pass
 
             if len(line) >= 5 and line[1] == "." and line.endswith(" V"):
                 vin = float(line.split(" ")[0])
-                self.vin = vin
-                with self.vincond:
-                    self.vincond.notify()
+                ftr = self.vinftr.pop()
+                ftr.set_result(vin)

test/motors.py

@@ -10,6 +10,8 @@
 class TestMotor(unittest.TestCase):
     """Test motors"""
 
+    THRESHOLD_DISTANCE = 15
+
     def test_rotations(self):
         """Test motor rotating"""
         m = Motor('A')
@@ -19,18 +21,66 @@ def test_rotations(self):
         rotated = (pos2 - pos1) / 360
         self.assertLess(abs(rotated - 2), 0.5)
 
+    def test_nonblocking(self):
+        """Test motor nonblocking mode"""
+        m = Motor('A')
+        last = 0
+        for delay in [1, 0]:
+            for _ in range(3):
+                m.run_to_position(90, blocking=False)
+                time.sleep(delay)
+                m.run_to_position(90, blocking=False)
+                time.sleep(delay)
+                m.run_to_position(90, blocking=False)
+                time.sleep(delay)
+                m.run_to_position(last, blocking=False)
+                time.sleep(delay)
+                # Wait for a bit, before reading last position
+                time.sleep(7)
+                pos1 = m.get_aposition()
+                diff = abs((last - pos1 + 180) % 360 - 180)
+                self.assertLess(diff, self.THRESHOLD_DISTANCE)
+
+    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, self.THRESHOLD_DISTANCE)
+                pos2 = m2.get_aposition()
+                diff = abs((last - pos2 + 180) % 360 - 180)
+                self.assertLess(diff, self.THRESHOLD_DISTANCE)
+
     def test_position(self):
         """Test motor goes to desired position"""
         m = Motor('A')
         m.run_to_position(0)
         pos1 = m.get_aposition()
         diff = abs((0 - pos1 + 180) % 360 - 180)
-        self.assertLess(diff, 10)
+        self.assertLess(diff, self.THRESHOLD_DISTANCE)
 
         m.run_to_position(180)
         pos1 = m.get_aposition()
         diff = abs((180 - pos1 + 180) % 360 - 180)
-        self.assertLess(diff, 10)
+        self.assertLess(diff, self.THRESHOLD_DISTANCE)
 
     def test_time(self):
         """Test motor runs for correct duration"""