Encoded motor rotate function

XRPLib/encoded_motor.py

@@ -3,6 +3,8 @@
 from machine import Timer
 from .controller import Controller
 from .pid import PID
+from .timeout import Timeout
+import time
 
 class EncodedMotor:
 
@@ -151,4 +153,63 @@ def _update(self):
             error = self.target_speed - self.speed
             effort = self.speedController.update(error)
             self._motor.set_effort(effort)
-        self.prev_position = current_position
+        self.prev_position = current_position
+
+    def rotate(self, degrees: float, max_effort: float = 0.5, timeout: float = None, main_controller: Controller = None) -> bool:
+        """
+        Rotate the motor by some number of degrees, and exit function when distance has been traveled.
+        Max_effort is bounded from -1 (reverse at full speed) to 1 (forward at full speed)
+
+        :param degrees: The distance for the motor to rotate (In Degrees)
+        :type degrees: float
+        :param max_effort: The max effort for which the robot to travel (Bounded from -1 to 1). Default is half effort forward
+        :type max_effort: float
+        :param timeout: The amount of time before the robot stops trying to move forward and continues to the next step (In Seconds)
+        :type timeout: float
+        :param main_controller: The main controller, for handling the motor's rotation
+        :type main_controller: Controller
+        :return: if the distance was reached before the timeout
+        :rtype: bool
+        """
+        # ensure effort is always positive while distance could be either positive or negative
+        if max_effort < 0:
+            max_effort *= -1
+            degrees *= -1
+
+        time_out = Timeout(timeout)
+        starting = self.get_position_counts()
+
+        degrees *= self._encoder.resolution/360
+
+        if main_controller is None:
+            main_controller = PID(
+                kp = 0.1,
+                ki = 0.065,
+                kd = 0.0275,
+                min_output = 0.3,
+                max_output = max_effort,
+                max_integral = 25,
+                tolerance = 3,
+                tolerance_count = 3,
+            )
+
+
+        while True:
+
+            # calculate the distance traveled
+            delta = self.get_position_counts() - starting
+
+            # PID for distance
+            error = degrees - delta
+            effort = main_controller.update(error)
+
+            if main_controller.is_done() or time_out.is_done():
+                break
+
+            self.set_effort(effort)
+
+            time.sleep(0.01)
+
+        self.set_effort(0)
+
+        return not time_out.is_done()

XRPLib/motor_group.py

@@ -1,4 +1,9 @@
 from .encoded_motor import EncodedMotor
+from .controller import Controller
+from .pid import PID
+from .timeout import Timeout
+import time
+
 class MotorGroup(EncodedMotor):
     def __init__(self, *motors: EncodedMotor):
         """
@@ -91,4 +96,63 @@ def set_speed_controller(self, new_controller):
         :type new_controller: Controller
         """
         for motor in self.motors:
-            motor.set_speed_controller(new_controller)
+            motor.set_speed_controller(new_controller)
+
+    def rotate(self, degrees: float, max_effort: float = 0.5, timeout: float = None, main_controller: Controller = None) -> bool:
+        """
+        Rotate all motors in this group by some number of degrees, and exit function when distance has been traveled.
+        Max_effort is bounded from -1 (reverse at full speed) to 1 (forward at full speed)
+
+        :param degrees: The distance for the motor to rotate (In Degrees)
+        :type degrees: float
+        :param max_effort: The max effort for which the robot to travel (Bounded from -1 to 1). Default is half effort forward
+        :type max_effort: float
+        :param timeout: The amount of time before the robot stops trying to move forward and continues to the next step (In Seconds)
+        :type timeout: float
+        :param main_controller: The main controller, for handling the motor's rotation
+        :type main_controller: Controller
+        :return: if the distance was reached before the timeout
+        :rtype: bool
+        """
+        # ensure effort is always positive while distance could be either positive or negative
+        if max_effort < 0:
+            max_effort *= -1
+            degrees *= -1
+
+        time_out = Timeout(timeout)
+        starting = self.get_position_counts()
+
+        degrees *= self._encoder.resolution/360
+
+        if main_controller is None:
+            main_controller = PID(
+                kp = 58.5,
+                ki = 38.025,
+                kd = 16.0875,
+                min_output = 0.3,
+                max_output = max_effort,
+                max_integral = 0.04,
+                tolerance = 0.01,
+                tolerance_count = 3,
+            )
+
+
+        while True:
+
+            # calculate the distance traveled
+            delta = self.get_position_counts() - starting
+
+            # PID for distance
+            error = degrees - delta
+            effort = main_controller.update(error)
+
+            if main_controller.is_done() or time_out.is_done():
+                break
+
+            self.set_effort(effort)
+
+            time.sleep(0.01)
+
+        self.set_effort(0)
+
+        return not time_out.is_done()