From 8d6fbf36318f38226ba30096b428ce1e3b8575ae Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 6 Apr 2023 12:29:31 +0200 Subject: [PATCH 01/11] pybricks._common.IMU: Add rotation and orientation. Also update implementation status for heading and reset_heading. --- CHANGELOG.md | 5 +++ doc/main/hubs/essentialhub.rst | 4 ++ doc/main/hubs/primehub.rst | 4 ++ doc/main/hubs/technichub.rst | 4 ++ jedi/tests/test_complete_essential_hub.py | 2 + jedi/tests/test_complete_prime_hub.py | 2 + jedi/tests/test_complete_technic_hub.py | 2 + jedi/tests/test_get_signature.py | 21 +++++++++ src/pybricks/_common.py | 52 +++++++++++++++++++++-- 9 files changed, 92 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f3504094..8d849bc3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,11 @@ - Documented ``integral_deadzone`` in ``Control.pid()``. - Documented ``Motor.model``. This can be used to view the estimated motor state and change its settings. +- Added `rotation` and `orientation` methods to `IMU` class. + +### Changed +- Change implementation status of `IMU.heading` and `IMU.reset_heading`. They + are now implemented, with some limitations as noted in a note box. ## 3.2.0 - 2022-12-20 diff --git a/doc/main/hubs/essentialhub.rst b/doc/main/hubs/essentialhub.rst index 74ac7bda..595ceeb3 100644 --- a/doc/main/hubs/essentialhub.rst +++ b/doc/main/hubs/essentialhub.rst @@ -37,6 +37,10 @@ Essential Hub .. automethod:: pybricks.hubs::EssentialHub.imu.reset_heading + .. automethod:: pybricks.hubs::EssentialHub.imu.rotation + + .. automethod:: pybricks.hubs::EssentialHub.imu.orientation + .. rubric:: Using the battery .. automethod:: pybricks.hubs::EssentialHub.battery.voltage diff --git a/doc/main/hubs/primehub.rst b/doc/main/hubs/primehub.rst index c1abcccf..cf34c53b 100644 --- a/doc/main/hubs/primehub.rst +++ b/doc/main/hubs/primehub.rst @@ -71,6 +71,10 @@ Prime Hub / Inventor Hub .. automethod:: pybricks.hubs::PrimeHub.imu.reset_heading + .. automethod:: pybricks.hubs::PrimeHub.imu.rotation + + .. automethod:: pybricks.hubs::PrimeHub.imu.orientation + .. rubric:: Using the speaker .. automethod:: pybricks.hubs::PrimeHub.speaker.volume diff --git a/doc/main/hubs/technichub.rst b/doc/main/hubs/technichub.rst index 761eede3..ebdd5cde 100644 --- a/doc/main/hubs/technichub.rst +++ b/doc/main/hubs/technichub.rst @@ -33,6 +33,10 @@ Technic Hub .. automethod:: pybricks.hubs::TechnicHub.imu.reset_heading + .. automethod:: pybricks.hubs::TechnicHub.imu.rotation + + .. automethod:: pybricks.hubs::TechnicHub.imu.orientation + .. rubric:: Using the battery .. automethod:: pybricks.hubs::TechnicHub.battery.voltage diff --git a/jedi/tests/test_complete_essential_hub.py b/jedi/tests/test_complete_essential_hub.py index f8852d21..8b5723cd 100644 --- a/jedi/tests/test_complete_essential_hub.py +++ b/jedi/tests/test_complete_essential_hub.py @@ -79,7 +79,9 @@ def test_hub_dot_imu_dot(): "acceleration", "angular_velocity", "heading", + "orientation", "reset_heading", + "rotation", "tilt", "up", ] diff --git a/jedi/tests/test_complete_prime_hub.py b/jedi/tests/test_complete_prime_hub.py index 3e977d30..d48b0cca 100644 --- a/jedi/tests/test_complete_prime_hub.py +++ b/jedi/tests/test_complete_prime_hub.py @@ -97,7 +97,9 @@ def test_hub_dot_imu_dot(): "acceleration", "angular_velocity", "heading", + "orientation", "reset_heading", + "rotation", "tilt", "up", ] diff --git a/jedi/tests/test_complete_technic_hub.py b/jedi/tests/test_complete_technic_hub.py index 2853d223..db0e5ea8 100644 --- a/jedi/tests/test_complete_technic_hub.py +++ b/jedi/tests/test_complete_technic_hub.py @@ -67,7 +67,9 @@ def test_hub_dot_imu_dot(): "acceleration", "angular_velocity", "heading", + "orientation", "reset_heading", + "rotation", "tilt", "up", ] diff --git a/jedi/tests/test_get_signature.py b/jedi/tests/test_get_signature.py index b64508f9..24776060 100644 --- a/jedi/tests/test_get_signature.py +++ b/jedi/tests/test_get_signature.py @@ -308,12 +308,19 @@ def _get_method_signature(module: str, type: str, method: str) -> SignatureHelp: [(["axis: Axis"], "float"), ([], "Matrix")], ), pytest.param("pybricks.hubs", "TechnicHub", "imu.heading", [([], "float")]), + pytest.param("pybricks.hubs", "TechnicHub", "imu.orientation", [([], "Matrix")]), pytest.param( "pybricks.hubs", "TechnicHub", "imu.reset_heading", [(["angle: Number"], "None")], ), + pytest.param( + "pybricks.hubs", + "TechnicHub", + "imu.rotation", + [(["axis: Axis"], "float")], + ), pytest.param("pybricks.hubs", "TechnicHub", "battery.voltage", [([], "int")]), pytest.param("pybricks.hubs", "TechnicHub", "battery.current", [([], "int")]), pytest.param( @@ -400,12 +407,19 @@ def _get_method_signature(module: str, type: str, method: str) -> SignatureHelp: [(["axis: Axis"], "float"), ([], "Matrix")], ), pytest.param("pybricks.hubs", "PrimeHub", "imu.heading", [([], "float")]), + pytest.param("pybricks.hubs", "PrimeHub", "imu.orientation", [([], "Matrix")]), pytest.param( "pybricks.hubs", "PrimeHub", "imu.reset_heading", [(["angle: Number"], "None")], ), + pytest.param( + "pybricks.hubs", + "PrimeHub", + "imu.rotation", + [(["axis: Axis"], "float")], + ), pytest.param( "pybricks.hubs", "PrimeHub", @@ -483,12 +497,19 @@ def _get_method_signature(module: str, type: str, method: str) -> SignatureHelp: [(["axis: Axis"], "float"), ([], "Matrix")], ), pytest.param("pybricks.hubs", "EssentialHub", "imu.heading", [([], "float")]), + pytest.param("pybricks.hubs", "EssentialHub", "imu.orientation", [([], "Matrix")]), pytest.param( "pybricks.hubs", "EssentialHub", "imu.reset_heading", [(["angle: Number"], "None")], ), + pytest.param( + "pybricks.hubs", + "EssentialHub", + "imu.rotation", + [(["axis: Axis"], "float")], + ), pytest.param("pybricks.hubs", "EssentialHub", "battery.voltage", [([], "int")]), pytest.param("pybricks.hubs", "EssentialHub", "battery.current", [([], "int")]), pytest.param("pybricks.hubs", "EssentialHub", "charger.connected", [([], "bool")]), diff --git a/src/pybricks/_common.py b/src/pybricks/_common.py index 7fceb01c..7394e629 100644 --- a/src/pybricks/_common.py +++ b/src/pybricks/_common.py @@ -983,7 +983,7 @@ def tilt(self) -> Tuple[int, int]: along the x-axis. Returns: - Tuple of pitch and roll angles. + Tuple of pitch and roll angles in degrees. """ @@ -998,7 +998,18 @@ def heading(self) -> float: For a vehicle viewed from the top, this means that a positive heading value corresponds to a counterclockwise rotation. - .. note:: This method is not yet implemented. + The heading value continously increments as the robot keeps turning. It + does *not* wrap around after reaching 180 degrees. + + .. note:: *For now, this method only keeps track of the heading while + the robot is on a flat surface.* + + This means that the value is + no longer correct if you lift it from the table. To solve + this, you can call ``reset_heading`` to reset the heading to + a known value *after* you put it back down. For example, you + could align your robot with the side of the competition table + and reset the heading 90 degrees as the new starting point. Returns: Heading angle relative to starting orientation. @@ -1010,8 +1021,6 @@ def reset_heading(self, angle: Number) -> None: Resets the accumulated heading angle of the robot. - .. note:: This method is not yet implemented. - Arguments: angle (Number, deg): Value to which the heading should be reset. """ @@ -1040,6 +1049,41 @@ def angular_velocity(self, *args): this returns a vector of accelerations along all axes. """ + def rotation(self, axis: Axis) -> float: + """ + rotation(axis) -> float: deg + + Gets the rotation of the device along a given axis in + the :ref:`robot reference frame `. + + This value is useful if your robot *only* rotates along the requested + axis. For general three-dimensional motion, use the + ``orientation()`` method instead. + + The value starts counting from ``0`` when you initialize this class. + + Arguments: + axis (Axis): Axis along which the rotation should be measured. + Returns: + The rotation angle. + """ + + def orientation(self) -> Matrix: + """ + orientation() -> Matrix + + Gets the three-dimensional orientation of the robot in + the :ref:`robot reference frame `. + + It returns a rotation matrix whose columns represent the ``X``, ``Y``, + and ``Z`` axis of the robot. + + .. note:: This method is not yet implemented. + + Returns: + The rotation matrix. + """ + class CommonColorSensor: """Generic color sensor that supports Pybricks color calibration.""" From 64ce3e56b8c0b9821abeeb146e4053ddf85c066d Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 7 Apr 2023 15:45:54 +0200 Subject: [PATCH 02/11] pybricks._common.IMU: Add imu status. --- CHANGELOG.md | 3 +- doc/main/hubs/essentialhub.rst | 6 +++ doc/main/hubs/primehub.rst | 6 +++ doc/main/hubs/technichub.rst | 6 +++ jedi/tests/test_complete_essential_hub.py | 3 ++ jedi/tests/test_complete_prime_hub.py | 3 ++ jedi/tests/test_complete_technic_hub.py | 3 ++ src/pybricks/_common.py | 48 +++++++++++++++++++++++ 8 files changed, 77 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8d849bc3..805a30dc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,7 +10,8 @@ - Documented ``integral_deadzone`` in ``Control.pid()``. - Documented ``Motor.model``. This can be used to view the estimated motor state and change its settings. -- Added `rotation` and `orientation` methods to `IMU` class. +- Added `rotation`, `orientation`, `ready`, `stationary` + and `set_stationary_thresholds` methods to `IMU` class. ### Changed - Change implementation status of `IMU.heading` and `IMU.reset_heading`. They diff --git a/doc/main/hubs/essentialhub.rst b/doc/main/hubs/essentialhub.rst index 595ceeb3..9f27ccc1 100644 --- a/doc/main/hubs/essentialhub.rst +++ b/doc/main/hubs/essentialhub.rst @@ -25,6 +25,12 @@ Essential Hub .. rubric:: Using the IMU + .. automethod:: pybricks.hubs::EssentialHub.imu.ready + + .. automethod:: pybricks.hubs::EssentialHub.imu.stationary + + .. automethod:: pybricks.hubs::EssentialHub.imu.set_stationary_thresholds + .. automethod:: pybricks.hubs::EssentialHub.imu.up .. automethod:: pybricks.hubs::EssentialHub.imu.tilt diff --git a/doc/main/hubs/primehub.rst b/doc/main/hubs/primehub.rst index cf34c53b..3787b0d3 100644 --- a/doc/main/hubs/primehub.rst +++ b/doc/main/hubs/primehub.rst @@ -59,6 +59,12 @@ Prime Hub / Inventor Hub .. rubric:: Using the IMU + .. automethod:: pybricks.hubs::PrimeHub.imu.ready + + .. automethod:: pybricks.hubs::PrimeHub.imu.stationary + + .. automethod:: pybricks.hubs::PrimeHub.imu.set_stationary_thresholds + .. automethod:: pybricks.hubs::PrimeHub.imu.up .. automethod:: pybricks.hubs::PrimeHub.imu.tilt diff --git a/doc/main/hubs/technichub.rst b/doc/main/hubs/technichub.rst index ebdd5cde..91baf1aa 100644 --- a/doc/main/hubs/technichub.rst +++ b/doc/main/hubs/technichub.rst @@ -21,6 +21,12 @@ Technic Hub .. rubric:: Using the IMU + .. automethod:: pybricks.hubs::TechnicHub.imu.ready + + .. automethod:: pybricks.hubs::TechnicHub.imu.stationary + + .. automethod:: pybricks.hubs::TechnicHub.imu.set_stationary_thresholds + .. automethod:: pybricks.hubs::TechnicHub.imu.up .. automethod:: pybricks.hubs::TechnicHub.imu.tilt diff --git a/jedi/tests/test_complete_essential_hub.py b/jedi/tests/test_complete_essential_hub.py index 8b5723cd..8fc2d117 100644 --- a/jedi/tests/test_complete_essential_hub.py +++ b/jedi/tests/test_complete_essential_hub.py @@ -80,8 +80,11 @@ def test_hub_dot_imu_dot(): "angular_velocity", "heading", "orientation", + "ready", "reset_heading", "rotation", + "set_stationary_thresholds", + "stationary", "tilt", "up", ] diff --git a/jedi/tests/test_complete_prime_hub.py b/jedi/tests/test_complete_prime_hub.py index d48b0cca..2075cd92 100644 --- a/jedi/tests/test_complete_prime_hub.py +++ b/jedi/tests/test_complete_prime_hub.py @@ -98,8 +98,11 @@ def test_hub_dot_imu_dot(): "angular_velocity", "heading", "orientation", + "ready", "reset_heading", "rotation", + "set_stationary_thresholds", + "stationary", "tilt", "up", ] diff --git a/jedi/tests/test_complete_technic_hub.py b/jedi/tests/test_complete_technic_hub.py index db0e5ea8..dc58388f 100644 --- a/jedi/tests/test_complete_technic_hub.py +++ b/jedi/tests/test_complete_technic_hub.py @@ -68,8 +68,11 @@ def test_hub_dot_imu_dot(): "angular_velocity", "heading", "orientation", + "ready", "reset_heading", "rotation", + "set_stationary_thresholds", + "stationary", "tilt", "up", ] diff --git a/src/pybricks/_common.py b/src/pybricks/_common.py index 7394e629..bcd58526 100644 --- a/src/pybricks/_common.py +++ b/src/pybricks/_common.py @@ -988,6 +988,54 @@ def tilt(self) -> Tuple[int, int]: class IMU(Accelerometer): + def ready(self) -> bool: + """ready() -> bool + + Checks if the device is calibrated and ready for use. + + This becomes ``True`` when the robot has been sitting stationary for a + few seconds, which allows the device to re-calibrate. It is ``False`` + if the hub has just been started, or if it hasn't had a chance to + calibrate for more than 10 minutes. + + Returns: + ``True`` if it is ready for use, ``False`` if not. + """ + + def stationary(self) -> bool: + """stationary() -> bool + + Checks if the device is currently stationary (not moving). + + Returns: + ``True`` if stationary for at least a second, ``False`` if it is + moving. + """ + + def set_stationary_thresholds( + self, angular_velocity: float, acceleration: float + ) -> None: + """set_stationary_thresholds(angular_velocity, acceleration) + + When the angular velocity and acceleration measurements are below the + given threshold values for at least one second, the sensor is + considered stationary. This is when the sensor recalibrates itself. + + If you are in a noisy room with high ambient vibrations (such as a + robot competition hall), it is recommended to increase these values + slightly to give your robot the chance to calibrate properly. + + To verify that your settings are working, test that + the ``stationary()`` method gives ``False`` if your robot is moving, + and ``True`` if it is sitting still for at least a second. + + Arguments: + angular_velocity (Number, deg/s): The threshold for angular + velocity. The default value is 1.5 deg/s. + acceleration (Number, mm/s²): The threshold for angular + velocity. The default value is 250 mm/s². + """ + def heading(self) -> float: """heading() -> float: deg From 28ecc060b72091472e9803ec8e3883c6d05e8077 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 7 Apr 2023 20:28:33 +0200 Subject: [PATCH 03/11] pybricks._common.IMU: Change settings setter. --- CHANGELOG.md | 2 +- doc/main/hubs/essentialhub.rst | 4 +-- doc/main/hubs/primehub.rst | 4 +-- doc/main/hubs/technichub.rst | 4 +-- jedi/tests/test_complete_essential_hub.py | 2 +- jedi/tests/test_complete_prime_hub.py | 2 +- jedi/tests/test_complete_technic_hub.py | 2 +- src/pybricks/_common.py | 41 ++++++++++++++++------- 8 files changed, 38 insertions(+), 23 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 805a30dc..dad9546b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,7 +11,7 @@ - Documented ``Motor.model``. This can be used to view the estimated motor state and change its settings. - Added `rotation`, `orientation`, `ready`, `stationary` - and `set_stationary_thresholds` methods to `IMU` class. + and `settings` methods to `IMU` class. ### Changed - Change implementation status of `IMU.heading` and `IMU.reset_heading`. They diff --git a/doc/main/hubs/essentialhub.rst b/doc/main/hubs/essentialhub.rst index 9f27ccc1..a24ac1d0 100644 --- a/doc/main/hubs/essentialhub.rst +++ b/doc/main/hubs/essentialhub.rst @@ -29,8 +29,6 @@ Essential Hub .. automethod:: pybricks.hubs::EssentialHub.imu.stationary - .. automethod:: pybricks.hubs::EssentialHub.imu.set_stationary_thresholds - .. automethod:: pybricks.hubs::EssentialHub.imu.up .. automethod:: pybricks.hubs::EssentialHub.imu.tilt @@ -47,6 +45,8 @@ Essential Hub .. automethod:: pybricks.hubs::EssentialHub.imu.orientation + .. automethod:: pybricks.hubs::EssentialHub.imu.settings + .. rubric:: Using the battery .. automethod:: pybricks.hubs::EssentialHub.battery.voltage diff --git a/doc/main/hubs/primehub.rst b/doc/main/hubs/primehub.rst index 3787b0d3..1390f9ac 100644 --- a/doc/main/hubs/primehub.rst +++ b/doc/main/hubs/primehub.rst @@ -63,8 +63,6 @@ Prime Hub / Inventor Hub .. automethod:: pybricks.hubs::PrimeHub.imu.stationary - .. automethod:: pybricks.hubs::PrimeHub.imu.set_stationary_thresholds - .. automethod:: pybricks.hubs::PrimeHub.imu.up .. automethod:: pybricks.hubs::PrimeHub.imu.tilt @@ -81,6 +79,8 @@ Prime Hub / Inventor Hub .. automethod:: pybricks.hubs::PrimeHub.imu.orientation + .. automethod:: pybricks.hubs::PrimeHub.imu.settings + .. rubric:: Using the speaker .. automethod:: pybricks.hubs::PrimeHub.speaker.volume diff --git a/doc/main/hubs/technichub.rst b/doc/main/hubs/technichub.rst index 91baf1aa..ab6c115f 100644 --- a/doc/main/hubs/technichub.rst +++ b/doc/main/hubs/technichub.rst @@ -25,8 +25,6 @@ Technic Hub .. automethod:: pybricks.hubs::TechnicHub.imu.stationary - .. automethod:: pybricks.hubs::TechnicHub.imu.set_stationary_thresholds - .. automethod:: pybricks.hubs::TechnicHub.imu.up .. automethod:: pybricks.hubs::TechnicHub.imu.tilt @@ -43,6 +41,8 @@ Technic Hub .. automethod:: pybricks.hubs::TechnicHub.imu.orientation + .. automethod:: pybricks.hubs::TechnicHub.imu.settings + .. rubric:: Using the battery .. automethod:: pybricks.hubs::TechnicHub.battery.voltage diff --git a/jedi/tests/test_complete_essential_hub.py b/jedi/tests/test_complete_essential_hub.py index 8fc2d117..37d9d9dd 100644 --- a/jedi/tests/test_complete_essential_hub.py +++ b/jedi/tests/test_complete_essential_hub.py @@ -83,7 +83,7 @@ def test_hub_dot_imu_dot(): "ready", "reset_heading", "rotation", - "set_stationary_thresholds", + "settings", "stationary", "tilt", "up", diff --git a/jedi/tests/test_complete_prime_hub.py b/jedi/tests/test_complete_prime_hub.py index 2075cd92..7f760640 100644 --- a/jedi/tests/test_complete_prime_hub.py +++ b/jedi/tests/test_complete_prime_hub.py @@ -101,7 +101,7 @@ def test_hub_dot_imu_dot(): "ready", "reset_heading", "rotation", - "set_stationary_thresholds", + "settings", "stationary", "tilt", "up", diff --git a/jedi/tests/test_complete_technic_hub.py b/jedi/tests/test_complete_technic_hub.py index dc58388f..2eb03ef9 100644 --- a/jedi/tests/test_complete_technic_hub.py +++ b/jedi/tests/test_complete_technic_hub.py @@ -71,7 +71,7 @@ def test_hub_dot_imu_dot(): "ready", "reset_heading", "rotation", - "set_stationary_thresholds", + "settings", "stationary", "tilt", "up", diff --git a/src/pybricks/_common.py b/src/pybricks/_common.py index bcd58526..cf1cad06 100644 --- a/src/pybricks/_common.py +++ b/src/pybricks/_common.py @@ -1012,27 +1012,42 @@ def stationary(self) -> bool: moving. """ - def set_stationary_thresholds( - self, angular_velocity: float, acceleration: float + @overload + def settings( + self, + angular_velocity_threshold: float = None, + acceleration_threshold: float = None, ) -> None: - """set_stationary_thresholds(angular_velocity, acceleration) + ... + + @overload + def settings(self) -> Tuple[float, float]: + ... - When the angular velocity and acceleration measurements are below the - given threshold values for at least one second, the sensor is - considered stationary. This is when the sensor recalibrates itself. + def settings(self, *args): + """ + settings(angular_velocity_threshold, acceleration_threshold) + settings() -> Tuple[float, float] + + Configures the IMU settings. If no arguments are given, + this returns the current values. - If you are in a noisy room with high ambient vibrations (such as a - robot competition hall), it is recommended to increase these values - slightly to give your robot the chance to calibrate properly. + The ``angular_velocity_threshold`` and ``acceleration_threshold`` + define when the hub is considered stationary. If all + measurements stay below these thresholds for one second, the IMU + will recalibrate itself. - To verify that your settings are working, test that + In a noisy room with high ambient vibrations (such as a + competition hall), it is recommended to increase the thresholds + slightly to give your robot the chance to calibrate. + To verify that your settings are working as expected, test that the ``stationary()`` method gives ``False`` if your robot is moving, and ``True`` if it is sitting still for at least a second. Arguments: - angular_velocity (Number, deg/s): The threshold for angular - velocity. The default value is 1.5 deg/s. - acceleration (Number, mm/s²): The threshold for angular + angular_velocity_threshold (Number, deg/s): The threshold for + angular velocity. The default value is 1.5 deg/s. + acceleration_threshold (Number, mm/s²): The threshold for angular velocity. The default value is 250 mm/s². """ From 6b3808f91d5b2da45db885ffd90a727e7bad7a7b Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 7 Apr 2023 20:47:31 +0200 Subject: [PATCH 04/11] pybricks.pupdevices.Motor: Document speed time window. --- doc/main/ev3devices.rst | 4 ++-- doc/main/pupdevices/motor.rst | 4 ++-- jedi/tests/test_get_signature.py | 7 ++++++- src/pybricks/_common.py | 12 ++++++++++-- 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/doc/main/ev3devices.rst b/doc/main/ev3devices.rst index 0d9d9c4e..2817349e 100644 --- a/doc/main/ev3devices.rst +++ b/doc/main/ev3devices.rst @@ -20,12 +20,12 @@ Motors .. rubric:: Measuring - .. automethod:: pybricks.ev3devices.Motor.speed - .. automethod:: pybricks.ev3devices.Motor.angle .. automethod:: pybricks.ev3devices.Motor.reset_angle + .. automethod:: pybricks.ev3devices.Motor.speed + .. automethod:: pybricks.ev3devices.Motor.load .. automethod:: pybricks.ev3devices.Motor.stalled diff --git a/doc/main/pupdevices/motor.rst b/doc/main/pupdevices/motor.rst index ad8de136..89cbc706 100644 --- a/doc/main/pupdevices/motor.rst +++ b/doc/main/pupdevices/motor.rst @@ -18,12 +18,12 @@ Motors with rotation sensors .. rubric:: Measuring - .. automethod:: pybricks.pupdevices.Motor.speed - .. automethod:: pybricks.pupdevices.Motor.angle .. automethod:: pybricks.pupdevices.Motor.reset_angle + .. automethod:: pybricks.pupdevices.Motor.speed + .. automethod:: pybricks.pupdevices.Motor.load .. automethod:: pybricks.pupdevices.Motor.stalled diff --git a/jedi/tests/test_get_signature.py b/jedi/tests/test_get_signature.py index 24776060..7f82bc31 100644 --- a/jedi/tests/test_get_signature.py +++ b/jedi/tests/test_get_signature.py @@ -543,7 +543,12 @@ def _get_method_signature(module: str, type: str, method: str) -> SignatureHelp: "settings", [(["max_voltage: Number"], "None"), ([], "Tuple[int]")], ), - pytest.param("pybricks.pupdevices", "Motor", "speed", [([], "int")]), + pytest.param( + "pybricks.pupdevices", + "Motor", + "speed", + [(["window: Number=100"], "int")], + ), pytest.param("pybricks.pupdevices", "Motor", "angle", [([], "int")]), pytest.param( "pybricks.pupdevices", diff --git a/src/pybricks/_common.py b/src/pybricks/_common.py index cf1cad06..d6193c59 100644 --- a/src/pybricks/_common.py +++ b/src/pybricks/_common.py @@ -408,11 +408,19 @@ def angle(self) -> int: Motor angle. """ - def speed(self) -> int: - """speed() -> int: deg/s + def speed(self, window: Number = 100) -> int: + """speed(window=100) -> int: deg/s Gets the speed of the motor. + The speed is measured as the change in the motor angle during the + given time window. A short window makes the speed value more + responsive to motor movement, but less steady. A long window makes the + speed value less responsive, but more steady. + + Arguments: + window (Number, ms): The time window used to determine the speed. + Returns: Motor speed. From dadaab6f61e38dc92f2040a08800cd7659c717b5 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 7 Apr 2023 20:54:32 +0200 Subject: [PATCH 05/11] pybricks.robotics.DriveBase: Document use of gyro. --- jedi/tests/test_get_signature.py | 1 + src/pybricks/robotics.py | 9 ++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/jedi/tests/test_get_signature.py b/jedi/tests/test_get_signature.py index 7f82bc31..804f55ac 100644 --- a/jedi/tests/test_get_signature.py +++ b/jedi/tests/test_get_signature.py @@ -149,6 +149,7 @@ def _get_constructor_signature(module: str, type: str) -> SignatureHelp: "wheel_diameter: Number", "axle_track: Number", "positive_direction: Direction=Direction.CLOCKWISE", + "use_gyro: bool=False", ] ], ), diff --git a/src/pybricks/robotics.py b/src/pybricks/robotics.py index adc543cb..c496971e 100644 --- a/src/pybricks/robotics.py +++ b/src/pybricks/robotics.py @@ -59,8 +59,9 @@ def __init__( wheel_diameter: Number, axle_track: Number, positive_direction: Direction = Direction.CLOCKWISE, + use_gyro: bool = False, ): - """DriveBase(left_motor, right_motor, wheel_diameter, axle_track, positive_direction=Direction.CLOCKWISE) + """DriveBase(left_motor, right_motor, wheel_diameter, axle_track, positive_direction=Direction.CLOCKWISE, use_gyro=False) Arguments: left_motor (Motor): @@ -73,6 +74,12 @@ def __init__( positive_direction (Direction): Which direction the drive base should turn when you give a positive turn rate or turn angle, viewed from the top. + use_gyro (bool): Choose ``True`` to use the internal gyrosope for + more accurate turns. This can only be used when + the ``positive_direction`` is set to counterclockwise. This is + only supported on hubs with an inertial measurement unit (IMU). + If you choose ``False`` (the default), turns are measured + using the wheel angles. """ def drive(self, speed: Number, turn_rate: Number) -> None: From b91afc9f888511d8a9bd539a75cdb2a04ba0c824 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 21 Apr 2023 09:14:36 +0200 Subject: [PATCH 06/11] pybricks.robotics.DriveBase: Revert gyro use via init. This reverts commit dadaab6f61e38dc92f2040a08800cd7659c717b5. We will introduce a separate class instead. --- jedi/tests/test_get_signature.py | 1 - src/pybricks/robotics.py | 9 +-------- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/jedi/tests/test_get_signature.py b/jedi/tests/test_get_signature.py index 804f55ac..7f82bc31 100644 --- a/jedi/tests/test_get_signature.py +++ b/jedi/tests/test_get_signature.py @@ -149,7 +149,6 @@ def _get_constructor_signature(module: str, type: str) -> SignatureHelp: "wheel_diameter: Number", "axle_track: Number", "positive_direction: Direction=Direction.CLOCKWISE", - "use_gyro: bool=False", ] ], ), diff --git a/src/pybricks/robotics.py b/src/pybricks/robotics.py index c496971e..adc543cb 100644 --- a/src/pybricks/robotics.py +++ b/src/pybricks/robotics.py @@ -59,9 +59,8 @@ def __init__( wheel_diameter: Number, axle_track: Number, positive_direction: Direction = Direction.CLOCKWISE, - use_gyro: bool = False, ): - """DriveBase(left_motor, right_motor, wheel_diameter, axle_track, positive_direction=Direction.CLOCKWISE, use_gyro=False) + """DriveBase(left_motor, right_motor, wheel_diameter, axle_track, positive_direction=Direction.CLOCKWISE) Arguments: left_motor (Motor): @@ -74,12 +73,6 @@ def __init__( positive_direction (Direction): Which direction the drive base should turn when you give a positive turn rate or turn angle, viewed from the top. - use_gyro (bool): Choose ``True`` to use the internal gyrosope for - more accurate turns. This can only be used when - the ``positive_direction`` is set to counterclockwise. This is - only supported on hubs with an inertial measurement unit (IMU). - If you choose ``False`` (the default), turns are measured - using the wheel angles. """ def drive(self, speed: Number, turn_rate: Number) -> None: From 052ca1592d665e7eede7b69bd38853d085930cac Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 21 Apr 2023 09:26:45 +0200 Subject: [PATCH 07/11] pybricks._common.IMU: Heading is clockwise positive. This is not a breaking change because heading() was never implemented until now. --- src/pybricks/_common.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/pybricks/_common.py b/src/pybricks/_common.py index d6193c59..97df1f97 100644 --- a/src/pybricks/_common.py +++ b/src/pybricks/_common.py @@ -1062,15 +1062,13 @@ def settings(self, *args): def heading(self) -> float: """heading() -> float: deg - Gets the heading angle relative to the starting orientation. It is a - positive rotation around the :ref:`z-axis in the robot - frame `, prior to applying any tilt rotation. + Gets the heading angle of your robot. A positive value means a + clockwise turn. - For a vehicle viewed from the top, this means that - a positive heading value corresponds to a counterclockwise rotation. + The heading is 0 when your program starts. The value continues to grow + even as the robot turns more than 180 degrees. It does not wrap around + to -180 like it does in some apps. - The heading value continously increments as the robot keeps turning. It - does *not* wrap around after reaching 180 degrees. .. note:: *For now, this method only keeps track of the heading while the robot is on a flat surface.* From da472ff317bdb9d1c2fcf1b097e237be7c06e62a Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 21 Apr 2023 09:34:04 +0200 Subject: [PATCH 08/11] pybricks.robotics.DriveBase: Drop positive direction. This reverts commit e4650cb1c96d8827455ab5e4c20488438b0f1846. This is not needed for gyro support, so it is better to remove it before it is ever released. --- CHANGELOG.md | 2 -- jedi/tests/test_complete_import.py | 4 +++- jedi/tests/test_get_signature.py | 1 - src/pybricks/robotics.py | 12 ++++-------- 4 files changed, 7 insertions(+), 12 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dad9546b..4806e72d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,8 +5,6 @@ ## Unreleased ### Added -- Documented `positive_direction` in `DriveBase`. -- Document deceleration setter in `DriveBase`. - Documented ``integral_deadzone`` in ``Control.pid()``. - Documented ``Motor.model``. This can be used to view the estimated motor state and change its settings. diff --git a/jedi/tests/test_complete_import.py b/jedi/tests/test_complete_import.py index cd360584..40af678b 100644 --- a/jedi/tests/test_complete_import.py +++ b/jedi/tests/test_complete_import.py @@ -159,7 +159,9 @@ def test_from_pybricks_pupdevices_import(): def test_from_pybricks_robotics_import(): code = "from pybricks.robotics import " completions: list[CompletionItem] = json.loads(complete(code, 1, len(code) + 1)) - assert [c["insertText"] for c in completions] == ["DriveBase"] + assert [c["insertText"] for c in completions] == [ + "DriveBase", + ] def test_from_pybricks_tools_import(): diff --git a/jedi/tests/test_get_signature.py b/jedi/tests/test_get_signature.py index 7f82bc31..e8ec4ee4 100644 --- a/jedi/tests/test_get_signature.py +++ b/jedi/tests/test_get_signature.py @@ -148,7 +148,6 @@ def _get_constructor_signature(module: str, type: str) -> SignatureHelp: "right_motor: Motor", "wheel_diameter: Number", "axle_track: Number", - "positive_direction: Direction=Direction.CLOCKWISE", ] ], ), diff --git a/src/pybricks/robotics.py b/src/pybricks/robotics.py index adc543cb..d97e8938 100644 --- a/src/pybricks/robotics.py +++ b/src/pybricks/robotics.py @@ -8,7 +8,7 @@ from typing import Tuple, Optional, overload, TYPE_CHECKING from . import _common -from .parameters import Stop, Direction +from .parameters import Stop if TYPE_CHECKING: from ._common import Motor @@ -29,7 +29,8 @@ class DriveBase: **Positive** angles and turn rates mean turning **right**. **Negative** means **left**. So when viewed from the top, positive means clockwise and negative means counterclockwise. If desired, - you can reverse this behavior with the ``positive_direction`` parameter. + you can flip this convention by reversing the ``left_motor`` and + ``right_motor`` when you initialize this class. See the `measuring`_ section for tips to measure and adjust the diameter and axle track values. @@ -58,9 +59,8 @@ def __init__( right_motor: Motor, wheel_diameter: Number, axle_track: Number, - positive_direction: Direction = Direction.CLOCKWISE, ): - """DriveBase(left_motor, right_motor, wheel_diameter, axle_track, positive_direction=Direction.CLOCKWISE) + """DriveBase(left_motor, right_motor, wheel_diameter, axle_track) Arguments: left_motor (Motor): @@ -70,9 +70,6 @@ def __init__( wheel_diameter (Number, mm): Diameter of the wheels. axle_track (Number, mm): Distance between the points where both wheels touch the ground. - positive_direction (Direction): Which direction the drive base - should turn when you give a positive turn rate or turn - angle, viewed from the top. """ def drive(self, speed: Number, turn_rate: Number) -> None: @@ -229,7 +226,6 @@ def stalled(self) -> bool: # HACK: hide from jedi if TYPE_CHECKING: - del Direction del Motor del Number del Stop From 65e734efc4bfa119cba932bb3cdca93a231bf086 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 21 Apr 2023 09:37:29 +0200 Subject: [PATCH 09/11] pybricks.robotics: Drop note restricting when to change settings. --- doc/main/robotics.rst | 3 --- 1 file changed, 3 deletions(-) diff --git a/doc/main/robotics.rst b/doc/main/robotics.rst index 7d7128ca..d60b9c03 100644 --- a/doc/main/robotics.rst +++ b/doc/main/robotics.rst @@ -103,9 +103,6 @@ the default speed and acceleration for straight maneuvers and turns. Use the following attributes to adjust more advanced control settings. - You can only change the settings while the robot is stopped. This is - either before you begin driving or after you call :meth:`.stop`. - .. autoattribute:: pybricks.robotics.DriveBase.distance_control :annotation: From 1a79dd13797026b53f3b8e726abb0a418b7da67e Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 21 Apr 2023 09:38:10 +0200 Subject: [PATCH 10/11] pybricks.robotics: Drop note suggesting to flip motors. This will still work, but it is asking for trouble when including the gyro. So it is probably better not to mention this at all. --- src/pybricks/robotics.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/pybricks/robotics.py b/src/pybricks/robotics.py index d97e8938..15d1aef6 100644 --- a/src/pybricks/robotics.py +++ b/src/pybricks/robotics.py @@ -28,9 +28,7 @@ class DriveBase: **Positive** angles and turn rates mean turning **right**. **Negative** means **left**. So when viewed from the top, - positive means clockwise and negative means counterclockwise. If desired, - you can flip this convention by reversing the ``left_motor`` and - ``right_motor`` when you initialize this class. + positive means clockwise and negative means counterclockwise. See the `measuring`_ section for tips to measure and adjust the diameter and axle track values. From 16ef9f52e61f9cb2f656adac5f6010af3a58e305 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 21 Apr 2023 10:32:57 +0200 Subject: [PATCH 11/11] pybricks.robotics: Add GyroDriveBase. --- CHANGELOG.md | 1 + doc/common/extensions/requirements-static.py | 8 ++-- doc/main/robotics.rst | 39 +++++++++++++++++++- examples/pup/robotics/drivebase_basics.py | 4 +- jedi/tests/test_complete_import.py | 4 +- src/pybricks/robotics.py | 6 +++ 6 files changed, 51 insertions(+), 11 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4806e72d..809f60ed 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ state and change its settings. - Added `rotation`, `orientation`, `ready`, `stationary` and `settings` methods to `IMU` class. +- Added `GyroDriveBase` class to `pybricks.robotics`. ### Changed - Change implementation status of `IMU.heading` and `IMU.reset_heading`. They diff --git a/doc/common/extensions/requirements-static.py b/doc/common/extensions/requirements-static.py index 755e0d41..991dd3c8 100644 --- a/doc/common/extensions/requirements-static.py +++ b/doc/common/extensions/requirements-static.py @@ -24,10 +24,10 @@ HUB_FEATURES = { "movehub": {"movehub"} | FEATURES_SMALL, "cityhub": {"cityhub"} | FEATURES_MEDIUM, - "technichub": {"technichub"} | FEATURES_MEDIUM, - "primehub": {"primehub", "inventorhub", "light-matrix"} | FEATURES_LARGE, - "inventorhub": {"primehub", "inventorhub", "light-matrix"} | FEATURES_LARGE, - "essentialhub": {"essentialhub"} | FEATURES_LARGE, + "technichub": {"technichub", "gyro"} | FEATURES_MEDIUM, + "primehub": {"primehub", "inventorhub", "light-matrix", "gyro"} | FEATURES_LARGE, + "inventorhub": {"primehub", "inventorhub", "light-matrix", "gyro"} | FEATURES_LARGE, + "essentialhub": {"essentialhub", "gyro"} | FEATURES_LARGE, } diff --git a/doc/main/robotics.rst b/doc/main/robotics.rst index d60b9c03..98c93aeb 100644 --- a/doc/main/robotics.rst +++ b/doc/main/robotics.rst @@ -1,11 +1,10 @@ -.. pybricks-requirements:: - :mod:`robotics ` -- Robotics and drive bases =============================================================== .. automodule:: pybricks.robotics :no-members: +.. pybricks-requirements:: .. autoclass:: pybricks.robotics.DriveBase :no-members: @@ -113,11 +112,47 @@ The :meth:`done` and :meth:`stalled` methods have been moved. + +.. pybricks-requirements:: gyro + +.. class:: GyroDriveBase + + This class works just like the :class:`DriveBase`, but it uses the hub's + built-in gyroscope to drive straight and turn more accurately. + + If your hub is not mounted flat in your robot, make sure to specify + the ``top_side`` and ``front_side`` parameters when you initialize the + :class:`PrimeHub() `, + :class:`InventorHub() `, + :class:`EssentialHub() `, or + :class:`TechnicHub() `. This way your robot + knows which rotation to measure when turning. + + The gyro in each hub is a bit different, which can cause it to be a few + degrees off for big turns, or many small turns in the same + direction. For example, you may need to use + :meth:`turn(357) ` or + :meth:`turn(362) ` + on your robot to make a full turn. + + By default, this class tries to maintain the robot's position after a move + completes. This means the wheels will spin if you pick the robot up, in an + effort to maintain its heading angle. To avoid this, you can choose + ``then=Stop.COAST`` in your last + :meth:`straight `, + :meth:`turn `, or + :meth:`curve ` command. + Examples ------------------- Driving straight and turning in place ********************************************** +The following program shows the basics of driving and turning. + +To use the built-in gyro, just replace the two occurences of +:class:`DriveBase` with :class:`GyroDriveBase`. + .. literalinclude:: ../../examples/pup/robotics/drivebase_basics.py diff --git a/examples/pup/robotics/drivebase_basics.py b/examples/pup/robotics/drivebase_basics.py index ddb4eb26..66e189fc 100644 --- a/examples/pup/robotics/drivebase_basics.py +++ b/examples/pup/robotics/drivebase_basics.py @@ -14,10 +14,10 @@ # Drive forward by 500mm (half a meter). drive_base.straight(500) -# Turn around clockwise (180 degrees) +# Turn around clockwise by 180 degrees. drive_base.turn(180) -# Drive forward again to drive back. +# Drive forward again to get back to the start. drive_base.straight(500) # Turn around counterclockwise. diff --git a/jedi/tests/test_complete_import.py b/jedi/tests/test_complete_import.py index 40af678b..e69a424b 100644 --- a/jedi/tests/test_complete_import.py +++ b/jedi/tests/test_complete_import.py @@ -159,9 +159,7 @@ def test_from_pybricks_pupdevices_import(): def test_from_pybricks_robotics_import(): code = "from pybricks.robotics import " completions: list[CompletionItem] = json.loads(complete(code, 1, len(code) + 1)) - assert [c["insertText"] for c in completions] == [ - "DriveBase", - ] + assert [c["insertText"] for c in completions] == ["DriveBase", "GyroDriveBase"] def test_from_pybricks_tools_import(): diff --git a/src/pybricks/robotics.py b/src/pybricks/robotics.py index 15d1aef6..65ef209a 100644 --- a/src/pybricks/robotics.py +++ b/src/pybricks/robotics.py @@ -222,6 +222,12 @@ def stalled(self) -> bool: """ +class GyroDriveBase(DriveBase): + """A robotic vehicle with two powered wheels and an optional support + wheel or caster. It measures the heading using the hub's built-in gyroscope, + which can make turning and driving straight more accurate.""" + + # HACK: hide from jedi if TYPE_CHECKING: del Motor