Improve sensor interval (#192)

* Initial rate improvements

* Add test for color sensor

* Add test for motors at low intervals

* Reduce default interval to 10ms to give 100Hz of data readings

* Add toggle to position/speed

This is so that we don't end up with a busy loop, which interfers with
processing serial data from another thread.  I think if there wasn't a GIL
this problem might not exist.

* Dont need to resend combi setting

Author: chrisruk

buildhat/devices.py

@@ -58,8 +58,9 @@ def __init__(self, port):
         Device._setup()
         self._simplemode = -1
         self._combimode = -1
+        self._modestr = ""
         self._typeid = self._conn.typeid
-        self._interval = 100
+        self._interval = 10
         if (
             self._typeid in Device._device_names
             and Device._device_names[self._typeid][0] != type(self).__name__  # noqa: W503
@@ -196,16 +197,10 @@ def get(self):
         :raises DeviceError: Occurs if device not in valid mode
         """
         self.isconnected()
-        idx = -1
-        if self._simplemode != -1:
-            idx = self._simplemode
-        elif self._combimode != -1:
-            idx = self._combimode
-        else:
+        if self._simplemode == -1 and self._combimode == -1:
             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")
         return ftr.result()
 
     def mode(self, modev):
@@ -215,18 +210,32 @@ def mode(self, modev):
         """
         self.isconnected()
         if isinstance(modev, list):
-            self._combimode = 0
             modestr = ""
             for t in modev:
                 modestr += f"{t[0]} {t[1]} "
-            self._write(f"port {self.port} ; combi {self._combimode} {modestr}\r")
+            if self._simplemode == -1 and self._combimode == 0 and self._modestr == modestr:
+                return
+            self._write(f"port {self.port}; select\r")
+            self._combimode = 0
+            self._write((f"port {self.port} ; combi {self._combimode} {modestr} ; "
+                         f"select {self._combimode} ; "
+                         f"selrate {self._interval}\r"))
             self._simplemode = -1
+            self._modestr = modestr
+            self._conn.combimode = 0
+            self._conn.simplemode = -1
         else:
+            if self._combimode == -1 and self._simplemode == int(modev):
+                return
             # Remove combi mode
             if self._combimode != -1:
                 self._write(f"port {self.port} ; combi {self._combimode}\r")
+            self._write(f"port {self.port}; select\r")
             self._combimode = -1
             self._simplemode = int(modev)
+            self._write(f"port {self.port} ; select {int(modev)} ; selrate {self._interval}\r")
+            self._conn.combimode = -1
+            self._conn.simplemode = int(modev)
 
     def select(self):
         """Request data from mode

buildhat/motors.py

@@ -203,7 +203,7 @@ def _run_positional_ramp(self, pos, newpos, speed):
         # Collapse speed range to -5 to 5
         speed *= 0.05
         dur = abs((newpos - pos) / speed)
-        cmd = (f"port {self.port}; combi 0 {self._combi} ; select 0 ; selrate {self._interval}; "
+        cmd = (f"port {self.port}; 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()
@@ -259,7 +259,7 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes
 
     def _run_for_seconds(self, seconds, speed):
         self._runmode = MotorRunmode.SECONDS
-        cmd = (f"port {self.port} ; combi 0 {self._combi} ; select 0 ; selrate {self._interval}; "
+        cmd = (f"port {self.port} ; 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()
@@ -311,7 +311,7 @@ def start(self, speed=None):
                 raise MotorError("Invalid Speed")
         cmd = f"port {self.port} ; set {speed}\r"
         if self._runmode == MotorRunmode.NONE:
-            cmd = (f"port {self.port} ; combi 0 {self._combi} ; select 0 ; selrate {self._interval}; "
+            cmd = (f"port {self.port} ; select 0 ; selrate {self._interval}; "
                    f"pid {self.port} 0 0 s1 1 0 0.003 0.01 0 100; "
                    f"set {speed}\r")
         self._runmode = MotorRunmode.FREE

buildhat/serinterface.py

@@ -31,6 +31,8 @@ def __init__(self):
         self.typeid = -1
         self.connected = False
         self.callit = None
+        self.simplemode = -1
+        self.combimode = -1
 
     def update(self, typeid, connected, callit=None):
         """Update connection information for port
@@ -381,6 +383,11 @@ def runit():
                     else:
                         if d != "":
                             newdata.append(int(d))
+                # Check data was for our current mode
+                if line[2] == "M" and self.connections[portid].simplemode != int(line[3]):
+                    continue
+                elif line[2] == "C" and self.connections[portid].combimode != int(line[3]):
+                    continue
                 callit = self.connections[portid].callit
                 if callit is not None:
                     q.put((callit, newdata))

test/color.py

@@ -0,0 +1,47 @@
+"""Test Color Sensor functionality"""
+import time
+import unittest
+
+from buildhat import ColorSensor
+
+
+class TestColor(unittest.TestCase):
+    """Test color sensor functions"""
+
+    def test_color_interval(self):
+        """Test color sensor interval"""
+        color = ColorSensor('A')
+        color.avg_reads = 1
+        color.interval = 10
+        count = 1000
+        expected_dur = count * color.interval * 1e-3
+
+        start = time.time()
+        for _ in range(count):
+            color.get_ambient_light()
+        end = time.time()
+        diff = abs((end - start) - expected_dur)
+        self.assertLess(diff, 0.25)
+
+        start = time.time()
+        for _ in range(count):
+            color.get_color_rgbi()
+        end = time.time()
+        diff = abs((end - start) - expected_dur)
+        self.assertLess(diff, 0.25)
+
+    def test_caching(self):
+        """Test to make sure we're not reading cached data"""
+        color = ColorSensor('A')
+        color.avg_reads = 1
+        color.interval = 1
+
+        for _ in range(100):
+            color.mode(2)
+            self.assertEqual(len(color.get()), 1)
+            color.mode(5)
+            self.assertEqual(len(color.get()), 4)
+
+
+if __name__ == '__main__':
+    unittest.main()

test/motors.py

@@ -185,22 +185,28 @@ def test_continuous_start(self):
         """Test starting motor for 5mins"""
         t = time.time() + (60 * 5)
         m = Motor('A')
+        toggle = 0
         while time.time() < t:
-            m.start(0)
+            m.start(toggle)
+            toggle ^= 1
 
     def test_continuous_degrees(self):
         """Test setting degrees for 5mins"""
         t = time.time() + (60 * 5)
         m = Motor('A')
+        toggle = 0
         while time.time() < t:
-            m.run_for_degrees(0)
+            m.run_for_degrees(toggle)
+            toggle ^= 1
 
     def test_continuous_position(self):
         """Test setting position of motor for 5mins"""
         t = time.time() + (60 * 5)
         m = Motor('A')
+        toggle = 0
         while time.time() < t:
-            m.run_to_position(0)
+            m.run_to_position(toggle)
+            toggle ^= 1
 
     def test_continuous_feedback(self):
         """Test feedback of motor for 30mins"""
@@ -211,6 +217,36 @@ def test_continuous_feedback(self):
         while time.time() < t:
             _ = (m.get_speed(), m.get_position(), m.get_aposition())
 
+    def test_interval(self):
+        """Test motor interval"""
+        m = Motor('A')
+        m.interval = 10
+        count = 1000
+        expected_dur = count * m.interval * 1e-3
+        start = time.time()
+        for _ in range(count):
+            m.get_position()
+        end = time.time()
+        diff = abs((end - start) - expected_dur)
+        self.assertLess(diff, expected_dur * 0.1)
+
+    def test_dual_interval(self):
+        """Test dual motor interval"""
+        m1 = Motor('A')
+        m2 = Motor('B')
+        for interval in [10, 5]:
+            m1.interval = interval
+            m2.interval = interval
+            count = 1000
+            expected_dur = count * m1.interval * 1e-3
+            start = time.time()
+            for _ in range(count):
+                m1.get_position()
+                m2.get_position()
+            end = time.time()
+            diff = abs((end - start) - expected_dur)
+            self.assertLess(diff, expected_dur * 0.1)
+
 
 if __name__ == '__main__':
     unittest.main()