From 37dbbabfe3f68eee2452e4574aff3554b6eb5719 Mon Sep 17 00:00:00 2001 From: Kevin Siegall Date: Sat, 28 Oct 2023 14:47:20 -0400 Subject: [PATCH 1/3] Added rotate function --- XRPLib/encoded_motor.py | 62 ++++++++++++++++++++++++++++++++++++++++- XRPLib/motor_group.py | 23 ++++++++++++++- 2 files changed, 83 insertions(+), 2 deletions(-) diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index dede228..f9abd0a 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,62 @@ 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()*360 + + + if main_controller is None: + main_controller = PID( + kp = 0.1, + ki = 0.04, + kd = 0.04, + min_output = 0.3, + max_output = max_effort, + max_integral = 10, + tolerance = 0.25, + tolerance_count = 3, + ) + + + while True: + + # calculate the distance traveled + delta = 360*self.get_position() - 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..eb99732 100644 --- a/XRPLib/motor_group.py +++ b/XRPLib/motor_group.py @@ -1,4 +1,6 @@ from .encoded_motor import EncodedMotor +from .controller import Controller + class MotorGroup(EncodedMotor): def __init__(self, *motors: EncodedMotor): """ @@ -91,4 +93,23 @@ 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 + """ + for motor in self.motors: + motor.rotate(degrees, max_effort, timeout, main_controller) \ No newline at end of file From 20888aa2d7d3c9d863c165b7b1077ce974de6284 Mon Sep 17 00:00:00 2001 From: Kevin Siegall Date: Sat, 28 Oct 2023 15:35:04 -0400 Subject: [PATCH 2/3] Rotate tuned --- XRPLib/encoded_motor.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index f9abd0a..fc372af 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -177,18 +177,19 @@ def rotate(self, degrees: float, max_effort: float = 0.5, timeout: float = None, degrees *= -1 time_out = Timeout(timeout) - starting = self.get_position()*360 + starting = self.get_position_counts() + degrees *= self._encoder.resolution/360 if main_controller is None: main_controller = PID( kp = 0.1, - ki = 0.04, - kd = 0.04, + ki = 0.065, + kd = 0.0275, min_output = 0.3, max_output = max_effort, - max_integral = 10, - tolerance = 0.25, + max_integral = 25, + tolerance = 3, tolerance_count = 3, ) @@ -196,7 +197,7 @@ def rotate(self, degrees: float, max_effort: float = 0.5, timeout: float = None, while True: # calculate the distance traveled - delta = 360*self.get_position() - starting + delta = self.get_position_counts() - starting # PID for distance error = degrees - delta From 3d353b5f172d621968badbb5139a0a679407e657 Mon Sep 17 00:00:00 2001 From: Kevin Siegall Date: Tue, 28 Nov 2023 14:34:52 -0500 Subject: [PATCH 3/3] Motor Group better implementation --- XRPLib/motor_group.py | 47 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 45 insertions(+), 2 deletions(-) diff --git a/XRPLib/motor_group.py b/XRPLib/motor_group.py index eb99732..38777d3 100644 --- a/XRPLib/motor_group.py +++ b/XRPLib/motor_group.py @@ -1,5 +1,8 @@ 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): @@ -111,5 +114,45 @@ def rotate(self, degrees: float, max_effort: float = 0.5, timeout: float = None, :return: if the distance was reached before the timeout :rtype: bool """ - for motor in self.motors: - motor.rotate(degrees, max_effort, timeout, main_controller) \ No newline at end of file + # 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