Merge branch 'main' of github.com:RaspberryPiFoundation/build-hat

grega · grega · commit d659701ee1e3 · 2021-10-19T08:15:14.000+01:00
diff --git a/buildhat/__init__.py b/buildhat/__init__.py
@@ -4,5 +4,6 @@
 from .color import ColorSensor
 from .colordistance import ColorDistanceSensor
 from .matrix import Matrix
+from .hat import Hat
 from .serinterface import BuildHAT
 from .exc import *
diff --git a/buildhat/color.py b/buildhat/color.py
@@ -116,7 +116,7 @@ def get_color_rgbi(self):
         """Returns the color 
 
         :return: RGBI representation 
-        :rtype: tuple
+        :rtype: list
         """
         self.mode(5)
         reads = []
diff --git a/buildhat/colordistance.py b/buildhat/colordistance.py
@@ -116,7 +116,7 @@ def get_color_rgb(self):
         """Returns the color 
 
         :return: RGBI representation 
-        :rtype: tuple
+        :rtype: list
         """
         self.mode(6)
         reads = []
diff --git a/buildhat/hat.py b/buildhat/hat.py
@@ -0,0 +1,41 @@
+from .serinterface import BuildHAT
+from .devices import Device
+import os
+import sys
+import weakref
+
+class Hat:
+    """Allows enumeration of devices which are connected to the hat
+    """
+    def __init__(self):
+        if not Device._instance:
+            data = os.path.join(os.path.dirname(sys.modules["buildhat"].__file__),"data/")
+            firm = os.path.join(data,"firmware.bin")
+            sig = os.path.join(data,"signature.bin")
+            ver = os.path.join(data,"version")
+            vfile = open(ver)
+            v = int(vfile.read())
+            vfile.close()
+            Device._instance = BuildHAT(firm, sig, v)
+            weakref.finalize(self, self._close)
+
+    def get(self):
+        """Gets devices which are connected or disconnected 
+
+        :return: Dictionary of devices
+        :rtype: dict
+        """
+        devices = {}
+        for i in range(4):
+            name = "Other"
+            if Device._instance.connections[i].typeid in Device._device_names:
+                name = Device._device_names[Device._instance.connections[i].typeid]
+            elif Device._instance.connections[i].typeid == -1:
+                name = "Disconnected"
+            devices[chr(ord('A')+i)] = {"typeid" : Device._instance.connections[i].typeid,
+                                        "connected" : Device._instance.connections[i].connected,
+                                        "name" : name}
+        return devices
+
+    def _close(self):
+        Device._instance.shutdown()
diff --git a/buildhat/motors.py b/buildhat/motors.py
@@ -60,10 +60,50 @@ def run_for_rotations(self, rotations, speed=None, blocking=True):
                 raise MotorException("Invalid Speed")
             self.run_for_degrees(int(rotations * 360), speed, blocking)
 
-    def _run_for_degrees(self, newpos, origpos, speed):
-        dur = abs((newpos - origpos) / speed)
+    def _run_for_degrees(self, degrees, speed):
+        mul = 1
+        if speed < 0:
+            speed = abs(speed)
+            mul = -1
+        pos = self.get_position()
+        newpos = ((degrees*mul)+pos)/360.0
+        pos /= 360.0
+        speed *= 0.05
+        dur = abs((newpos - pos) / speed)
+        cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 1 s4 0.0027777778 0 5 0 .1 3 ; set ramp {} {} {} 0\r".format(self.port,
+        self.port, pos, newpos, dur)
+        self._write(cmd)
+        with self._hat.rampcond[self.port]:
+            self._hat.rampcond[self.port].wait()
+        if self._release:
+            time.sleep(0.2)
+            self.coast()
+
+    def _run_to_position(self, degrees, speed, direction):
+        data = self.get()
+        pos = data[1]
+        apos = data[2]
+        diff = (degrees-apos+180) % 360 - 180
+        newpos = (pos + diff)/360
+        v1 = (degrees - apos)%360
+        v2 = (apos - degrees)%360
+        mul = 1
+        if diff > 0:
+            mul = -1
+        diff = sorted([diff, mul * (v2 if abs(diff) == v1 else v1)])
+        if direction == "shortest":
+            pass
+        elif direction == "clockwise":
+            newpos = (pos + diff[1])/360
+        elif direction == "anticlockwise":
+            newpos = (pos + diff[0])/360
+        else:
+            raise DirectionInvalid("Invalid direction, should be: shortest, clockwise or anticlockwise")
+        pos /= 360.0
+        speed *= 0.05
+        dur = abs((newpos - pos) / speed)
         cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 1 s4 0.0027777778 0 5 0 .1 3 ; set ramp {} {} {} 0\r".format(self.port,
-        self.port, origpos, newpos, dur)
+        self.port, pos, newpos, dur)
         self._write(cmd)
         with self._hat.rampcond[self.port]:
             self._hat.rampcond[self.port].wait()
@@ -83,20 +123,12 @@ def run_for_degrees(self, degrees, speed=None, blocking=True):
             speed = self.default_speed
         if not (speed >= -100 and speed <= 100):
             raise MotorException("Invalid Speed")
-        mul = 1
-        if speed < 0:
-            speed = abs(speed)
-            mul = -1
-        origpos = self.get_position()
-        newpos = ((degrees*mul)+origpos)/360.0
-        origpos /= 360.0
-        speed *= 0.05
         if not blocking:
-            th = threading.Thread(target=self._run_for_degrees, args=(newpos, origpos, speed))
+            th = threading.Thread(target=self._run_for_degrees, args=(degrees, speed))
             th.daemon = True
             th.start()
         else:
-            self._run_for_degrees(newpos, origpos, speed)
+            self._run_for_degrees(degrees, speed)
 
     def run_to_position(self, degrees, speed=None, blocking=True, direction="shortest"):
         """Runs motor to position (in degrees)
@@ -110,35 +142,12 @@ def run_to_position(self, degrees, speed=None, blocking=True, direction="shortes
             raise MotorException("Invalid Speed")
         if degrees < -180 or degrees > 180:
             raise MotorException("Invalid angle")
-        pos = self.get_position()
-        apos = self.get_aposition()
-        diff = (degrees-apos+180) % 360 - 180
-        newpos = (pos + diff)/360
-
-        v1 = (degrees - apos)%360
-        v2 = (apos - degrees)%360
-        mul = 1
-        if diff > 0:
-            mul = -1
-        diff = sorted([diff, mul * (v2 if abs(diff) == v1 else v1)])
-
-        if direction == "shortest":
-            pass
-        elif direction == "clockwise":
-            newpos = (pos + diff[1])/360
-        elif direction == "anticlockwise":
-            newpos = (pos + diff[0])/360
-        else:
-            raise DirectionInvalid("Invalid direction, should be: shortest, clockwise or anticlockwise")
-
-        pos /= 360.0
-        speed *= 0.05
         if not blocking:
-            th = threading.Thread(target=self._run_for_degrees, args=(newpos, pos, speed))
+            th = threading.Thread(target=self._run_to_position, args=(degrees, speed, direction))
             th.daemon = True
             th.start()
         else:
-            self._run_for_degrees(newpos, pos, speed)
+            self._run_to_position(degrees, speed, direction)
 
     def _run_for_seconds(self, seconds, speed):
         cmd = "port {} ; combi 0 1 0 2 0 3 0 ; select 0 ; pid {} 0 0 s1 1 0 0.003 0.01 0 100; set pulse {} 0.0 {} 0\r".format(self.port, self.port, speed, seconds);
@@ -298,9 +307,9 @@ def run_for_degrees(self, degrees, speedl=None, speedr=None):
             speedl = self.default_speed
         if speedr is None:
             speedr = self.default_speed
-        th1 = threading.Thread(target=self._leftmotor.run_for_degrees, args=(degrees,), kwargs={'speed': speedl, 'blocking': True})
+        th1 = threading.Thread(target=self._leftmotor._run_for_degrees, args=(degrees, speedl))
+        th2 = threading.Thread(target=self._rightmotor._run_for_degrees, args=(degrees, speedr))
         th1.daemon = True
-        th2 = threading.Thread(target=self._rightmotor.run_for_degrees, args=(degrees,), kwargs={'speed': speedr, 'blocking': True})
         th2.daemon = True
         th1.start()
         th2.start()
@@ -318,9 +327,9 @@ def run_for_seconds(self, seconds, speedl=None, speedr=None):
             speedl = self.default_speed
         if speedr is None:
             speedr = self.default_speed
-        th1 = threading.Thread(target=self._leftmotor._run_for_seconds, args=(seconds,), kwargs={'speed': speedl})
+        th1 = threading.Thread(target=self._leftmotor._run_for_seconds, args=(seconds, speedl))
+        th2 = threading.Thread(target=self._rightmotor._run_for_seconds, args=(seconds, speedr))
         th1.daemon = True
-        th2 = threading.Thread(target=self._rightmotor._run_for_seconds, args=(seconds,), kwargs={'speed': speedr})
         th2.daemon = True
         th1.start()
         th2.start()
@@ -344,16 +353,22 @@ def stop(self):
         self._leftmotor.stop()
         self._rightmotor.stop()
 
-    def run_to_position(self, degreesl, degreesr, speed=None):
+    def run_to_position(self, degreesl, degreesr, speed=None, direction="shortest"):
         """Runs pair to position (in degrees)
 
            :param degreesl: Position in degrees for left motor
            :param degreesr: Position in degrees for right motor
            :param speed: Speed ranging from -100 to 100
         """
         if speed is None:
-            self._leftmotor.run_to_position(degreesl, self.default_speed)
-            self._rightmotor.run_to_position(degreesr, self.default_speed)
+            th1 = threading.Thread(target=self._leftmotor._run_to_position, args=(degreesl, self.default_speed, direction))
+            th2 = threading.Thread(target=self._rightmotor._run_to_position, args=(degreesr, self.default_speed, direction))
         else:
-            self._leftmotor.run_to_position(degreesl, speed)
-            self._rightmotor.run_to_position(degreesr, speed)
+            th1 = threading.Thread(target=self._leftmotor._run_to_position, args=(degreesl, speed, direction))
+            th2 = threading.Thread(target=self._rightmotor._run_to_position, args=(degreesr, speed, direction))
+        th1.daemon = True
+        th2.daemon = True
+        th1.start()
+        th2.start()
+        th1.join()
+        th2.join()
diff --git a/docs/buildhat/colordistancesensor.rst b/docs/buildhat/colordistancesensor.rst
@@ -1,6 +1,17 @@
 ColorDistanceSensor
 ===================
 
+|location_link|
+
+.. |location_link| raw:: html
+
+   <a href="https://www.lego.com/en-gb/product/color-distance-sensor-88007" target="_blank">LEGO Color and Distance Sensor 45605</a>
+
+
+The LEGO® Color and Distance Sensor can sort between six different colors and objects within 5 to 10 cm range
+
+NOTE: Support for this device is experimental and not all features are available yet. 
+
 .. autoclass:: buildhat.ColorDistanceSensor
    :members:
    :inherited-members:
diff --git a/docs/buildhat/hat.py b/docs/buildhat/hat.py
@@ -0,0 +1,4 @@
+from buildhat import Hat
+
+hat = Hat()
+print(hat.get())
diff --git a/docs/buildhat/hat.rst b/docs/buildhat/hat.rst
@@ -0,0 +1,13 @@
+Hat
+===
+
+Gets list of devices connected to the hat
+
+.. autoclass:: buildhat.Hat
+   :members:
+   :inherited-members:
+
+Example
+-------
+
+.. literalinclude:: hat.py
diff --git a/docs/buildhat/index.rst b/docs/buildhat/index.rst
@@ -3,8 +3,10 @@
 Library
 =======
 
-The Build HAT library has been created to support the Raspberry Pi Build HAT, 
-an add-on board for the Raspberry Pi computer, which allows control of up to four LEGO® Technic™ motors and sensors included in the SPIKE™ Portfolio.
+The Build HAT library has been created to support the `Raspberry Pi Build HAT`_, 
+an add-on board for the Raspberry Pi computer, which allows control of up to four LEGO® TECHNIC™ motors and sensors included in the SPIKE™ Portfolio.
+
+.. _Raspberry Pi Build HAT: http://raspberrypi.com/products/build-hat
 
 .. image:: images/BuildHAT_closeup.jpg
    :width: 300
@@ -18,7 +20,9 @@ Other LEGO® devices may be supported if they use the LPF2 connector:
    :alt: The LEGO LPF2 connector
 
 In order to drive motors, your Raspberry Pi and Build HAT will need an external 7.5V
-power supply. For best results, use the official Raspberry Pi Build HAT power supply. 
+power supply. For best results, use the `official Raspberry Pi Build HAT power supply`_. 
+
+.. _official Raspberry Pi Build HAT power supply: http://raspberrypi.com/products/build-hat-power-supply
 
 .. warning::
 
@@ -35,3 +39,4 @@ power supply. For best results, use the official Raspberry Pi Build HAT power su
    matrix.rst
    motor.rst
    motorpair.rst
+   hat.rst
diff --git a/docs/buildhat/motor.rst b/docs/buildhat/motor.rst
@@ -1,8 +1,7 @@
 Motor
 =====
 
-LEGO® Technic™ motors from the LEGO® Education SPIKE™ portfolio have an integrated rotation sensor (encoder) and can be positioned with 1-degree accuracy. The encoders can be queried to find the current 
-position of the motor with respect to a 'zero' mark shown on the motor itself. Other motors without encoders will report a 0 value if queried.
+LEGO® Technic™ motors from the LEGO® Education SPIKE™ portfolio have an integrated rotation sensor (encoder) and can be positioned with 1-degree accuracy. The encoders can be queried to find the current position of the motor with respect to a 'zero' mark shown on the motor itself. 
 
 |location_link1|
 
@@ -16,7 +15,25 @@ position of the motor with respect to a 'zero' mark shown on the motor itself. O
 
    <a href="https://education.lego.com/en-gb/products/lego-technic-medium-angular-motor/45603" target="_blank">LEGO® Medium angular motor 45603</a>
 
+Other motors without encoders will report a 0 value if queried.
 
+|location_link3|
+
+.. |location_link3| raw:: html
+
+   <a href="https://www.lego.com/en-gb/product/medium-linear-motor-88008" target="_blank">LEGO® Medium Linear motor 88008</a>
+
+|location_link4|
+
+.. |location_link4| raw:: html
+
+   <a href="https://www.lego.com/en-gb/product/technic-large-motor-88013" target="_blank">Technic™ Large Motor 88013</a>
+
+|location_link5|
+
+.. |location_link5| raw:: html
+
+   <a href="https://www.lego.com/en-gb/product/technic-xl-motor-88014" target="_blank">Technic™ XL Motor 88014</a>
 
 .. autoclass:: buildhat.Motor
    :members:
diff --git a/test/motors.py b/test/motors.py
@@ -0,0 +1,35 @@
+import unittest
+import time
+from buildhat import Motor
+
+class TestMotorMethods(unittest.TestCase):
+
+    def test_rotations(self):
+        m = Motor('A')
+        pos1 = m.get_position()
+        m.run_for_rotations(2) 
+        pos2 = m.get_position()
+        rotated = (pos2 - pos1)/360
+        self.assertLess(abs(rotated - 2), 0.5)
+
+    def test_position(self):
+        m = Motor('A')
+        m.run_to_position(0)
+        pos1 = m.get_aposition()
+        diff = abs((0-pos1+180) % 360 - 180)
+        self.assertLess(diff, 10)
+
+        m.run_to_position(180)
+        pos1 = m.get_aposition()
+        diff = abs((180-pos1+180) % 360 - 180)
+        self.assertLess(diff, 10)
+
+    def test_time(self):
+        m = Motor('A')
+        t1 = time.time()
+        m.run_for_seconds(5)
+        t2 = time.time()
+        self.assertEqual(int(t2 - t1), 5)
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/test/resources/FakeHat b/test/resources/FakeHat
diff --git a/test/test_manual.py b/test/test_manual.py
diff --git a/test/test_script.py b/test/test_script.py
