diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index dede228..fc372af 100644 --- a/XRPLib/encoded_motor.py +++ b/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 \ No newline at end of file + 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() \ No newline at end of file diff --git a/XRPLib/motor_group.py b/XRPLib/motor_group.py index ecfe4f0..38777d3 100644 --- a/XRPLib/motor_group.py +++ b/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) \ No newline at end of file + 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() \ No newline at end of file