add new motor class for tracking angles without queuing or threading (see issue #195)

Author: iwanimsand

buildhat/__init__.py

@@ -8,6 +8,6 @@
 from .hat import Hat
 from .light import Light
 from .matrix import Matrix
-from .motors import Motor, MotorPair, PassiveMotor
+from .motors import Motor, MotorPair, PassiveMotor, TargetTrackerMotor
 from .serinterface import BuildHAT
 from .wedo import MotionSensor, TiltSensor

buildhat/devices.py

@@ -24,14 +24,14 @@ class Device:
                      62: ("DistanceSensor", "Distance Sensor"),               # 45604
                      63: ("ForceSensor", "Force Sensor"),                     # 45606
                      64: ("Matrix", "3x3 Color Light Matrix"),                # 45608
-                     38: ("Motor", "Medium Linear Motor"),                    # 88008
-                     46: ("Motor", "Large Motor"),                            # 88013
-                     47: ("Motor", "XL Motor"),                               # 88014
-                     48: ("Motor", "Medium Angular Motor (Cyan)"),            # 45603
-                     49: ("Motor", "Large Angular Motor (Cyan)"),             # 45602
-                     65: ("Motor", "Small Angular Motor"),                    # 45607
-                     75: ("Motor", "Medium Angular Motor (Grey)"),            # 88018
-                     76: ("Motor", "Large Angular Motor (Grey)")}             # 88017
+                     38: (["Motor", "TargetTrackerMotor"], "Medium Linear Motor"),                    # 88008
+                     46: (["Motor", "TargetTrackerMotor"], "Large Motor"),                            # 88013
+                     47: (["Motor", "TargetTrackerMotor"], "XL Motor"),                               # 88014
+                     48: (["Motor", "TargetTrackerMotor"], "Medium Angular Motor (Cyan)"),            # 45603
+                     49: (["Motor", "TargetTrackerMotor"], "Large Angular Motor (Cyan)"),             # 45602
+                     65: (["Motor", "TargetTrackerMotor"], "Small Angular Motor"),                    # 45607
+                     75: (["Motor", "TargetTrackerMotor"], "Medium Angular Motor (Grey)"),            # 88018
+                     76: (["Motor", "TargetTrackerMotor"], "Large Angular Motor (Grey)")}             # 88017
 
     _used = {0: False,
              1: False,
@@ -63,7 +63,7 @@ def __init__(self, port):
         self._interval = 10
         if (
             self._typeid in Device._device_names
-            and Device._device_names[self._typeid][0] != type(self).__name__  # noqa: W503
+            and type(self).__name__ not in Device._device_names[self._typeid][0]  # noqa: W503
         ) or self._typeid == -1:
             raise DeviceError(f'There is not a {type(self).__name__} connected to port {port} (Found {self.name})')
         Device._used[p] = True

buildhat/motors.py

@@ -596,3 +596,93 @@ def release(self, value):
         self._release = value
         self._leftmotor.release = value
         self._rightmotor.release = value
+
+
+class TargetTrackerMotor(Device):
+    """Motor device which can be used to track a target.
+
+    This type of motor is non-blocking by default and doesn't wait for a command to be finished. An actual running
+    command will be overwritten. If you need sequential execution which waits for a movement to finish, then use the
+    ~Motor class.
+
+    :param port: Port of device
+    :raises DeviceError: Occurs if there is no motor attached to port
+    """
+
+    def __init__(self, port):
+        """Initialise motor
+
+        :param port:
+        """
+        super().__init__(port)
+        if self._typeid in {38}:
+            self.mode([(1, 0), (2, 0)])
+            self._combi = "1 0 2 0"
+            self._noapos = True
+        else:
+            self.mode([(1, 0), (2, 0), (3, 0)])
+            self._combi = "1 0 2 0 3 0"
+            self._noapos = False
+        self.plimit(0.7)
+        self.bias(0.3)
+        self._init_pid()
+
+    def _init_pid(self):
+        """Initialize the pid controller"""
+        cmd = f"pid {self.port} 0 5 s2 0.0027777778 1 5 0 .1 3\r "
+        self._write(cmd)
+
+    def plimit(self, plimit):
+        """Limit power
+
+        :param plimit: Value 0 to 1
+        :raises MotorError: Occurs if invalid plimit value passed
+        """
+        if not (0 <= plimit <= 1):
+            raise MotorError("plimit should be 0 to 1")
+        self._write(f"port {self.port} ; plimit {plimit}\r")
+
+    def bias(self, bias):
+        """Bias motor
+
+        :param bias: Value 0 to 1
+        :raises MotorError: Occurs if invalid bias value passed
+        """
+        if not (0 <= bias <= 1):
+            raise MotorError("bias should be 0 to 1")
+        self._write(f"port {self.port} ; bias {bias}\r")
+
+    def run_to_position(self, degrees):
+        """Run motor to position (in degrees)
+
+        :param degrees: Position in degrees from -180 to 180
+        """
+        cmd = f"port {self.port}; set {1 / 360 * degrees}\r"
+        self._write(cmd)
+
+    def get_position(self):
+        """Get position of motor with relation to preset position (can be negative or positive)
+
+        :return: Position of motor in degrees from preset position
+        :rtype: int
+        """
+        return self.get()[1]
+
+    def get_aposition(self):
+        """Get absolute position of motor
+
+        :return: Absolute position of motor from -180 to 180
+        :rtype: int
+        """
+        if self._noapos:
+            raise MotorError("No absolute position with this motor")
+        else:
+            return self.get()[2]
+
+    def get_speed(self):
+        """Get speed of motor
+
+        :return: Speed of motor
+        :rtype: int
+        """
+        return self.get()[0]

test/track_target_motor.py

@@ -0,0 +1,42 @@
+"""Test motors with ~TargetTrackerMotor"""
+
+import time
+import unittest
+
+from buildhat import TargetTrackerMotor
+
+
+class TestTargetTrackerMotor(unittest.TestCase):
+    """Test ~TargetTrackerMotor"""
+
+    def test_command_overwrites(self):
+        """Test motor rotating"""
+        m = TargetTrackerMotor('A')
+
+        # position motor at 0 degrees
+        pos1 = m.get_aposition()
+        t0 = time.time()
+        while pos1 < -5 or pos1 > 5:
+            m.run_to_position(0)
+            time.sleep(0.1)
+            pos1 = m.get_aposition()
+            if time.time() - t0 > 3:
+                self.fail("Motor could not reach start position in given time!")
+
+        angles = [0, 90, 0, 90, 0]
+
+        max_angle_reached = 0
+
+        for _ in range(5):
+            for angle in angles:
+                m.run_to_position(angle)
+                time.sleep(0.1)
+                pos2 = m.get_aposition()
+                if max_angle_reached < pos2:
+                    max_angle_reached = pos2
+
+        self.assertLess(max_angle_reached, 45)
+
+
+if __name__ == '__main__':
+    unittest.main()