diff --git a/buildhat/__init__.py b/buildhat/__init__.py index da3a147..5a51e7d 100644 --- a/buildhat/__init__.py +++ b/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 diff --git a/buildhat/devices.py b/buildhat/devices.py index e8bf086..cb34ac7 100644 --- a/buildhat/devices.py +++ b/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 diff --git a/buildhat/motors.py b/buildhat/motors.py index 0027fff..8fc4ff9 100644 --- a/buildhat/motors.py +++ b/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] diff --git a/test/track_target_motor.py b/test/track_target_motor.py new file mode 100644 index 0000000..4aef8fd --- /dev/null +++ b/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()