From d5fc0bfa54016343ae086deb11af7124446e47ae Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Fri, 7 May 2021 18:57:17 +0800 Subject: [PATCH 01/13] Change robot move to use speeds directly instead of doing an RPM conversion --- pitop/robotics/drive_controller.py | 20 +++++++++----------- tests/test_drive_controller.py | 16 ++++++++-------- 2 files changed, 17 insertions(+), 19 deletions(-) diff --git a/pitop/robotics/drive_controller.py b/pitop/robotics/drive_controller.py index 87770a43c..8a66e1c7c 100644 --- a/pitop/robotics/drive_controller.py +++ b/pitop/robotics/drive_controller.py @@ -68,27 +68,25 @@ def check_initialization(self, *args, **kwargs): return fcn(self, *args, **kwargs) return check_initialization - def _calculate_motor_rpms(self, linear_speed, angular_speed, turn_radius): + def _calculate_motor_speeds(self, linear_speed, angular_speed, turn_radius): # if angular_speed is positive, then rotation is anti-clockwise in this coordinate frame speed_right = linear_speed + (turn_radius + self._wheel_separation / 2) * angular_speed speed_left = linear_speed + (turn_radius - self._wheel_separation / 2) * angular_speed - rpm_right = self._speed_to_rpm(speed_right) - rpm_left = self._speed_to_rpm(speed_left) - if abs(rpm_right) > self._max_motor_rpm or abs(rpm_left) > self._max_motor_rpm: - factor = self._max_motor_rpm / max(abs(rpm_left), abs(rpm_right)) - rpm_right = rpm_right * factor - rpm_left = rpm_left * factor + if abs(speed_right) > self._max_motor_speed or abs(speed_left) > self._max_motor_speed: + factor = self._max_motor_speed / max(abs(speed_left), abs(speed_right)) + speed_right = speed_right * factor + speed_left = speed_left * factor - return rpm_left, rpm_right + return speed_left, speed_right @is_initialized def robot_move(self, linear_speed, angular_speed, turn_radius=0.0): # TODO: turn_radius will introduce a hidden linear speed component to the robot, so params are syntactically # misleading - rpm_left, rpm_right = self._calculate_motor_rpms(linear_speed, angular_speed, turn_radius) - self.left_motor.set_target_rpm(target_rpm=rpm_left) - self.right_motor.set_target_rpm(target_rpm=rpm_right) + speed_left, speed_right = self._calculate_motor_speeds(linear_speed, angular_speed, turn_radius) + self.left_motor.set_target_speed(target_speed=speed_left) + self.right_motor.set_target_speed(target_speed=speed_right) @is_initialized def forward(self, speed_factor, hold=False): diff --git a/tests/test_drive_controller.py b/tests/test_drive_controller.py index 4674b1e3f..1d2363e80 100644 --- a/tests/test_drive_controller.py +++ b/tests/test_drive_controller.py @@ -115,17 +115,17 @@ def test_rotate_fails_on_invalid_time(self): d.rotate(0, time_to_take) def test_motor_rpm_calculations_based_on_speeds(self): - """motor RPM calculations based on linear/angular speeds.""" + """motor speed calculations based on linear/angular speeds.""" d = DriveController() test_values = [ [0, 0, 0, 0, 0], - [96.39, 114.00, 1, 1, 0], - [104.84, 114.00, 1, 1, 1], - [70.90, 83.90, 0.3, 0.3, 0], - [103.84, 114.0, 0.3, 0.3, 0.8], + [0.374, 0.442, 1, 1, 0], + [0.406, 0.442, 1, 1, 1], + [0.275, 0.325, 0.3, 0.3, 0], + [0.403, 0.442, 0.3, 0.3, 0.8], ] for exp_rpm_left, exp_rpm_right, linear_speed, angular_speed, turn_radius in test_values: - rpm_left, rpm_right = d._calculate_motor_rpms(linear_speed, angular_speed, turn_radius) - self.assertEquals(round(rpm_left, 2), exp_rpm_left) - self.assertEquals(round(rpm_right, 2), exp_rpm_right) + speed_left, speed_right = d._calculate_motor_speeds(linear_speed, angular_speed, turn_radius) + self.assertEquals(round(speed_left, 3), exp_rpm_left) + self.assertEquals(round(speed_right, 3), exp_rpm_right) From eeaed6d1d94770bed7fb660216c8899032d4b1ba Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Fri, 7 May 2021 19:06:45 +0800 Subject: [PATCH 02/13] Change rotate function to use motor speeds --- pitop/robotics/drive_controller.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pitop/robotics/drive_controller.py b/pitop/robotics/drive_controller.py index 8a66e1c7c..3b3ef9d83 100644 --- a/pitop/robotics/drive_controller.py +++ b/pitop/robotics/drive_controller.py @@ -160,12 +160,12 @@ def rotate(self, angle, time_to_take): angle_radians = radians(angle) angular_speed = angle_radians / time_to_take - rpm_left, rpm_right = self._calculate_motor_rpms(0, angular_speed, turn_radius=0) - rotations = abs(angle) * pi * self._wheel_separation / (360 * self._wheel_circumference) - self.left_motor.set_target_rpm(target_rpm=rpm_left, - total_rotations=rotations * rpm_left / abs(rpm_left)) - self.right_motor.set_target_rpm(target_rpm=rpm_right, - total_rotations=rotations * rpm_right / abs(rpm_right)) + speed_left, speed_right = self._calculate_motor_speeds(0, angular_speed, turn_radius=0) + distance = abs(angle_radians) * self._wheel_separation / 2 + self.left_motor.set_target_speed(target_speed=speed_left, + distance=distance * speed_left / abs(speed_left)) + self.right_motor.set_target_speed(target_speed=speed_right, + distance=distance * speed_right / abs(speed_right)) sleep(time_to_take) def stop(self): From cd842e477f04ba9aa8c2218c1486c26ce393d744 Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Fri, 7 May 2021 19:10:48 +0800 Subject: [PATCH 03/13] Remove all mention of RPMs from drive controller --- pitop/robotics/drive_controller.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/pitop/robotics/drive_controller.py b/pitop/robotics/drive_controller.py index 3b3ef9d83..873376cc7 100644 --- a/pitop/robotics/drive_controller.py +++ b/pitop/robotics/drive_controller.py @@ -41,9 +41,7 @@ def __init__(self, left_motor_port="M3", right_motor_port="M0", name="drive"): self.right_motor = EncoderMotor(port_name=right_motor_port, forward_direction=ForwardDirection.COUNTER_CLOCKWISE) - self._max_motor_rpm = floor(min(self.left_motor.max_rpm, self.right_motor.max_rpm)) - - self._max_motor_speed = self._rpm_to_speed(self._max_motor_rpm) + self._max_motor_speed = floor(min(self.left_motor.max_speed, self.right_motor.max_speed)) self._max_robot_angular_speed = self._max_motor_speed / (self._wheel_separation / 2) self.__target_lock_pid_controller = PID(Kp=0.045, @@ -184,14 +182,6 @@ def stop_rotation(self): """ self.robot_move(self._linear_speed_x_hold, 0) - def _speed_to_rpm(self, speed): - rpm = round(60.0 * speed / self._wheel_circumference, 1) - return rpm - - def _rpm_to_speed(self, rpm): - speed = round(rpm * self._wheel_circumference / 60.0, 3) - return speed - @property def wheel_separation(self): return self._wheel_separation From 3172fd9c8ceac0fe404e818ed89113819d2dc81e Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Mon, 10 May 2021 16:11:54 +0800 Subject: [PATCH 04/13] Fix bug with max motor speed being zero --- pitop/robotics/drive_controller.py | 3 ++- tests/test_drive_controller.py | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/pitop/robotics/drive_controller.py b/pitop/robotics/drive_controller.py index 873376cc7..8f5453f0c 100644 --- a/pitop/robotics/drive_controller.py +++ b/pitop/robotics/drive_controller.py @@ -41,7 +41,8 @@ def __init__(self, left_motor_port="M3", right_motor_port="M0", name="drive"): self.right_motor = EncoderMotor(port_name=right_motor_port, forward_direction=ForwardDirection.COUNTER_CLOCKWISE) - self._max_motor_speed = floor(min(self.left_motor.max_speed, self.right_motor.max_speed)) + # Round down to ensure no speed value ever goes above maximum due to rounding issues (resulting in error) + self._max_motor_speed = floor(min(self.left_motor.max_speed, self.right_motor.max_speed) * 1000) / 1000 self._max_robot_angular_speed = self._max_motor_speed / (self._wheel_separation / 2) self.__target_lock_pid_controller = PID(Kp=0.045, diff --git a/tests/test_drive_controller.py b/tests/test_drive_controller.py index 1d2363e80..d9ec19985 100644 --- a/tests/test_drive_controller.py +++ b/tests/test_drive_controller.py @@ -119,11 +119,11 @@ def test_motor_rpm_calculations_based_on_speeds(self): d = DriveController() test_values = [ - [0, 0, 0, 0, 0], - [0.374, 0.442, 1, 1, 0], - [0.406, 0.442, 1, 1, 1], + [0, 0, 0, 0, 0] + [0.378, 0.447, 1, 1, 0], + [0.411, 0.447, 1, 1, 1], [0.275, 0.325, 0.3, 0.3, 0], - [0.403, 0.442, 0.3, 0.3, 0.8], + [0.407, 0.447, 0.3, 0.3, 0.8], ] for exp_rpm_left, exp_rpm_right, linear_speed, angular_speed, turn_radius in test_values: speed_left, speed_right = d._calculate_motor_speeds(linear_speed, angular_speed, turn_radius) From 82cf216e8538b4273e067c57246f45b7c55ae11a Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Mon, 10 May 2021 16:21:54 +0800 Subject: [PATCH 05/13] Fix list --- tests/test_drive_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_drive_controller.py b/tests/test_drive_controller.py index d9ec19985..691ccbf7b 100644 --- a/tests/test_drive_controller.py +++ b/tests/test_drive_controller.py @@ -119,7 +119,7 @@ def test_motor_rpm_calculations_based_on_speeds(self): d = DriveController() test_values = [ - [0, 0, 0, 0, 0] + [0, 0, 0, 0, 0], [0.378, 0.447, 1, 1, 0], [0.411, 0.447, 1, 1, 1], [0.275, 0.325, 0.3, 0.3, 0], From adbf66425f94e422f99198096f7fa9919db52550 Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Mon, 10 May 2021 20:11:02 +0800 Subject: [PATCH 06/13] create 4wd drive controller --- pitop/__init__.py | 1 + pitop/labs/web/blueprints/rover/helpers.py | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/pitop/__init__.py b/pitop/__init__.py index 3a8356906..4d286a421 100644 --- a/pitop/__init__.py +++ b/pitop/__init__.py @@ -25,6 +25,7 @@ # Robotics from .robotics.drive_controller import DriveController +from .robotics.drive_controller_4wd import DriveController4WD from .robotics.pan_tilt_controller import PanTiltController from .robotics.pincer_controller import PincerController from .robotics.configurations import ( diff --git a/pitop/labs/web/blueprints/rover/helpers.py b/pitop/labs/web/blueprints/rover/helpers.py index 619c8a5e0..094d7c208 100644 --- a/pitop/labs/web/blueprints/rover/helpers.py +++ b/pitop/labs/web/blueprints/rover/helpers.py @@ -40,3 +40,21 @@ def calculate_velocity_twist(data): 'linear': math.cos(direction * math.pi / 180) * linear_speed, 'angular': math.sin(direction * math.pi / 180) * angular_speed, } + + +MAX_LINEAR_SPEED_4WD = 0.596 +MAX_ANGULAR_SPEED_4WD = 3.24 + + +def calculate_linear_velocities_4wd(data): + angle = data.get('angle', {}) + degree = angle.get('degree', 0) + distance = data.get('distance', 0) + direction = calculate_direction(degree) + linear_speed_x = (distance * MAX_LINEAR_SPEED / 100.0) * math.cos(direction * math.pi / 180) + linear_speed_y = (distance * MAX_LINEAR_SPEED / 100.0) * math.sin(direction * math.pi / 180) + + return { + 'linear_x': linear_speed_x, + 'linear_y': linear_speed_y, + } From 1cf94f57725188a62d0916910396184af950ff4d Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Tue, 11 May 2021 14:59:59 +0800 Subject: [PATCH 07/13] Add 4WD mecanum wheel drive controller and web controller --- examples/labs/4wd_rover_controller/main.py | 19 ++ pitop/labs/web/blueprints/rover/__init__.py | 1 + .../web/blueprints/rover/drive_handler_4wd.py | 69 ++++++ pitop/labs/web/blueprints/rover/helpers.py | 18 -- pitop/robotics/drive_controller_4wd.py | 199 ++++++++++++++++++ 5 files changed, 288 insertions(+), 18 deletions(-) create mode 100644 examples/labs/4wd_rover_controller/main.py create mode 100644 pitop/labs/web/blueprints/rover/drive_handler_4wd.py create mode 100644 pitop/robotics/drive_controller_4wd.py diff --git a/examples/labs/4wd_rover_controller/main.py b/examples/labs/4wd_rover_controller/main.py new file mode 100644 index 000000000..5a337f71c --- /dev/null +++ b/examples/labs/4wd_rover_controller/main.py @@ -0,0 +1,19 @@ +from pitop import Pitop, DriveController4WD, Camera +from pitop.labs import RoverWebController +from pitop.labs.web.blueprints.rover import DriveHandler4WD + +rover = Pitop() +rover.add_component(DriveController4WD()) +rover.add_component(Camera(resolution=(320, 240))) + +drive_handler_4wd = DriveHandler4WD(drive=rover.drive) + +rover_controller = RoverWebController( + get_frame=rover.camera.get_frame, + message_handlers={ + 'right_joystick': lambda data: drive_handler_4wd.update_linear(data), + 'left_joystick': lambda data: drive_handler_4wd.update_angular(data) + } +) + +rover_controller.serve_forever() diff --git a/pitop/labs/web/blueprints/rover/__init__.py b/pitop/labs/web/blueprints/rover/__init__.py index 7437092d1..3a3983933 100644 --- a/pitop/labs/web/blueprints/rover/__init__.py +++ b/pitop/labs/web/blueprints/rover/__init__.py @@ -2,6 +2,7 @@ from pitop.labs.web.blueprints.controller import ControllerBlueprint from .helpers import calculate_velocity_twist, calculate_pan_tilt_angle +from .drive_handler_4wd import DriveHandler4WD def drive_handler(drive, data): diff --git a/pitop/labs/web/blueprints/rover/drive_handler_4wd.py b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py new file mode 100644 index 000000000..8b9179aaa --- /dev/null +++ b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py @@ -0,0 +1,69 @@ +from sched import scheduler +from threading import Thread +import time +from .helpers import calculate_direction +import math + +MAX_LINEAR_SPEED = 0.596 +MAX_ANGULAR_SPEED = 3.24 + + +def calculate_joystick_vector(data): + angle = data.get('angle', {}) + degree = angle.get('degree', 0) + distance = data.get('distance', 0) + direction = math.radians(calculate_direction(degree)) + + return distance, direction + + +def calculate_linear_velocities(data): + distance, direction = calculate_joystick_vector(data) + linear_speed_x = (distance * MAX_LINEAR_SPEED / 100.0) * math.cos(direction) + linear_speed_y = (distance * MAX_LINEAR_SPEED / 100.0) * math.sin(direction) + + return { + 'linear_speed_x': linear_speed_x, + 'linear_speed_y': linear_speed_y, + } + + +def calculate_angular_velocity(data): + distance, direction = calculate_joystick_vector(data) + angular_speed_z = (distance * MAX_ANGULAR_SPEED / 100.0) * math.sin(direction) + return { + 'angular_speed_z': angular_speed_z + } + + +class DriveHandler4WD: + def __init__(self, drive): + self._drive = drive + self._v_x = 0.0 + self._v_y = 0.0 + self._w_z = 0.0 + self._publish_dt = 1 / 10.0 + self._publish_thread = Thread(target=self.__publish_sheduler, daemon=True) + self._publish_thread.start() + + def update_linear(self, data): + linear_velocities = calculate_linear_velocities(data) + self._v_x = linear_velocities.get('linear_speed_x', 0) + self._v_y = linear_velocities.get('linear_speed_y', 0) + + def update_angular(self, data): + angular_velocity = calculate_angular_velocity(data) + self._w_z = angular_velocity.get('angular_speed_z', 0) + + def __publish_sheduler(self): + s = scheduler(time.time, time.sleep) + current_time = time.time() + s.enterabs(current_time + self._publish_dt, 1, self.__publish_velocity_commands, (s,)) + s.run() + + def __publish_velocity_commands(self, s): + current_time = time.time() + + self._drive.robot_move(linear_x=self._v_x, linear_y=self._v_y, angular_z=self._w_z) + + s.enterabs(current_time + self._publish_dt, 1, self.__publish_velocity_commands, (s,)) diff --git a/pitop/labs/web/blueprints/rover/helpers.py b/pitop/labs/web/blueprints/rover/helpers.py index 094d7c208..619c8a5e0 100644 --- a/pitop/labs/web/blueprints/rover/helpers.py +++ b/pitop/labs/web/blueprints/rover/helpers.py @@ -40,21 +40,3 @@ def calculate_velocity_twist(data): 'linear': math.cos(direction * math.pi / 180) * linear_speed, 'angular': math.sin(direction * math.pi / 180) * angular_speed, } - - -MAX_LINEAR_SPEED_4WD = 0.596 -MAX_ANGULAR_SPEED_4WD = 3.24 - - -def calculate_linear_velocities_4wd(data): - angle = data.get('angle', {}) - degree = angle.get('degree', 0) - distance = data.get('distance', 0) - direction = calculate_direction(degree) - linear_speed_x = (distance * MAX_LINEAR_SPEED / 100.0) * math.cos(direction * math.pi / 180) - linear_speed_y = (distance * MAX_LINEAR_SPEED / 100.0) * math.sin(direction * math.pi / 180) - - return { - 'linear_x': linear_speed_x, - 'linear_y': linear_speed_y, - } diff --git a/pitop/robotics/drive_controller_4wd.py b/pitop/robotics/drive_controller_4wd.py new file mode 100644 index 000000000..870f6668d --- /dev/null +++ b/pitop/robotics/drive_controller_4wd.py @@ -0,0 +1,199 @@ +from math import ( + floor, + pi, + radians, +) +from time import sleep + +from simple_pid import PID + +from pitop.core.exceptions import UninitializedComponent +from pitop.core.mixins import ( + Stateful, + Recreatable, +) +from pitop.pma import ( + EncoderMotor, + ForwardDirection, + BrakingType, +) +import numpy as np + + + +class DriveController4WD(Stateful, Recreatable): + """Represents a vehicle with two wheels connected by an axis, and an + optional support wheel or caster.""" + _initialized = False + + def __init__(self, + front_left_motor_port="M2", + front_right_motor_port="M1", + rear_left_motor_port="M3", + rear_right_motor_port="M0", + name="drive" + ): + self.name = name + self.front_left_motor_port = front_left_motor_port + self.front_right_motor_port = front_right_motor_port + self.rear_left_motor_port = rear_left_motor_port + self.rear_right_motor_port = rear_right_motor_port + + # Robot dimensions + self._wheel_diameter = 0.1 + self.half_wheel_separation_x = 0.18 / 2 + self.half_wheel_separation_y = 0.168 / 2 + + # Matrix to convert robot twist commands (v_x, v_y, w_z) to individual mecanum wheel speeds + self._command_to_wheel_speed_transform = np.array( + [ + [1, -1, -(self.half_wheel_separation_y + self.half_wheel_separation_x)], # front left + [1, 1, (self.half_wheel_separation_y + self.half_wheel_separation_x)], # front right + [1, 1, -(self.half_wheel_separation_y + self.half_wheel_separation_x)], # rear left + [1, -1, (self.half_wheel_separation_y + self.half_wheel_separation_x)] # rear right + ] + ) + + # Motor initialization + self.front_left_motor = EncoderMotor(port_name=front_left_motor_port, + forward_direction=ForwardDirection.CLOCKWISE, + wheel_diameter=self._wheel_diameter, + braking_type=BrakingType.COAST) + self.front_right_motor = EncoderMotor(port_name=front_right_motor_port, + forward_direction=ForwardDirection.COUNTER_CLOCKWISE, + wheel_diameter=self._wheel_diameter, + braking_type=BrakingType.COAST) + self.rear_left_motor = EncoderMotor(port_name=rear_left_motor_port, + forward_direction=ForwardDirection.CLOCKWISE, + wheel_diameter=self._wheel_diameter, + braking_type=BrakingType.COAST) + self.rear_right_motor = EncoderMotor(port_name=rear_right_motor_port, + forward_direction=ForwardDirection.COUNTER_CLOCKWISE, + wheel_diameter=self._wheel_diameter, + braking_type=BrakingType.COAST) + + # Maximum speeds + self.max_wheel_speed = floor(min(self.front_left_motor.max_speed, + self.front_right_motor.max_speed, + self.rear_left_motor.max_speed, + self.rear_right_motor.max_speed + ) * 1000 + ) / 1000 + self.max_robot_angular_speed = floor( + self.max_wheel_speed / (self.half_wheel_separation_x + self.half_wheel_separation_y) * 100 + ) / 100 + + # Speed hold variables + self._linear_speed_x_hold = 0 + self._linear_speed_y_hold = 0 + + Stateful.__init__(self, children=['front_left_motor', + 'front_right_motor', + 'rear_left_motor', + 'rear_right_motor' + ] + ) + Recreatable.__init__(self, config_dict={"front_left_motor_port": front_left_motor_port, + "front_right_motor_port": front_right_motor_port, + "rear_left_motor_port": rear_left_motor_port, + "rear_right_motor_port": rear_right_motor_port, + "name": self.name}) + + def _calculate_motor_speeds(self, twist_array): + motor_speeds = self._command_to_wheel_speed_transform.dot(twist_array) + + scaler = abs(motor_speeds[np.argmax(np.absolute(motor_speeds)), 0] / self.max_wheel_speed) + + if scaler > 1.0: + # if any desired motor speed is above the maximum speed, scale all motor speeds + motor_speeds /= scaler + + return motor_speeds.T[0] + + def robot_move(self, linear_x: float = 0.0, linear_y: float = 0.0, angular_z: float = 0.0): + velocity_array = np.array([[linear_x], [linear_y], [angular_z]]) + speed_front_left, speed_front_right, speed_rear_left, speed_rear_right = self._calculate_motor_speeds( + velocity_array) + self.front_left_motor.set_target_speed(target_speed=speed_front_left) + self.front_right_motor.set_target_speed(target_speed=speed_front_right) + self.rear_left_motor.set_target_speed(target_speed=speed_rear_left) + self.rear_right_motor.set_target_speed(target_speed=speed_rear_right) + + def forward(self, speed_factor, hold=False): + """Move the robot forward. + + :param float speed_factor: + Factor relative to the maximum motor speed, used to set the velocity, in the range -1.0 to 1.0. + Using negative values will cause the robot to move backwards. + :param bool hold: + Setting this parameter to true will cause subsequent movements to use the speed set as the base speed. + """ + linear_speed_x = self.max_wheel_speed * speed_factor + if hold: + self._linear_speed_x_hold = linear_speed_x + else: + self._linear_speed_x_hold = 0.0 + self.robot_move(linear_x=linear_speed_x, linear_y=self._linear_speed_y_hold, angular_z=0.0) + + def backward(self, speed_factor, hold=False): + """Move the robot backward. + + :param float speed_factor: + Factor relative to the maximum motor speed, used to set the velocity, in the range -1.0 to 1.0. + Using negative values will cause the robot to move forwards. + :param bool hold: + Setting this parameter to true will cause subsequent movements to use the speed set as the base speed. + """ + self.forward(-speed_factor, hold) + + def left(self, speed_factor): + """Make the robot move to the left, using a circular trajectory. + + :param float speed_factor: + Factor relative to the maximum motor speed, used to set the velocity, in the range -1.0 to 1.0. + Using negative values will cause the robot to turn right. + :param float turn_radius: + Radius used by the robot to perform the movement. Using `turn_radius=0` will cause the robot to rotate in place. + """ + + self.robot_move(linear_x=self._linear_speed_x_hold, + linear_y=self._linear_speed_y_hold, + angular_z=self.max_robot_angular_speed * speed_factor + ) + + def right(self, speed_factor): + """Make the robot move to the right, using a circular trajectory. + + :param float speed_factor: + Factor relative to the maximum motor speed, used to set the velocity, in the range -1.0 to 1.0. + Using negative values will cause the robot to turn left. + """ + + self.left(-speed_factor) + + def left_strafe(self, speed_factor, hold=False): + linear_speed_y = self.max_wheel_speed * speed_factor + if hold: + self._linear_speed_y_hold = linear_speed_y + else: + self._linear_speed_y_hold = 0.0 + self.robot_move(linear_x=self._linear_speed_x_hold, linear_y=linear_speed_y, angular_z=0.0) + + def right_strafe(self, speed_factor, hold=False): + self.left_strafe(-speed_factor, hold=hold) + + def stop(self): + """Stop any movement being performed by the motors.""" + self._linear_speed_x_hold = 0 + self._linear_speed_y_hold = 0 + self.robot_move(0, 0, 0) + + def stop_rotation(self): + """Stop any angular movement performed by the robot. + + In the case where linear and rotational movements are being + performed at the same time, calling this method will cause the robot + to continue the linear movement, so it will continue to move + forward. + """ + self.robot_move(linear_x=self._linear_speed_x_hold, linear_y=self._linear_speed_y_hold, angular_z=0) From c9505f5509281945803678610130a06d0236436b Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Tue, 11 May 2021 15:05:44 +0800 Subject: [PATCH 08/13] Remove unused imports and modify docstrings --- pitop/robotics/drive_controller_4wd.py | 26 +++++++------------------- 1 file changed, 7 insertions(+), 19 deletions(-) diff --git a/pitop/robotics/drive_controller_4wd.py b/pitop/robotics/drive_controller_4wd.py index 870f6668d..ea0f269fe 100644 --- a/pitop/robotics/drive_controller_4wd.py +++ b/pitop/robotics/drive_controller_4wd.py @@ -1,13 +1,4 @@ -from math import ( - floor, - pi, - radians, -) -from time import sleep - -from simple_pid import PID - -from pitop.core.exceptions import UninitializedComponent +from math import floor from pitop.core.mixins import ( Stateful, Recreatable, @@ -20,10 +11,8 @@ import numpy as np - class DriveController4WD(Stateful, Recreatable): - """Represents a vehicle with two wheels connected by an axis, and an - optional support wheel or caster.""" + """Represents a vehicle with four mecanum wheels.""" _initialized = False def __init__(self, @@ -146,14 +135,13 @@ def backward(self, speed_factor, hold=False): """ self.forward(-speed_factor, hold) + # TODO: decide whether to change the API (2WD vs 4WD) and call this "turn_left" so "left" is reserved for strafe def left(self, speed_factor): - """Make the robot move to the left, using a circular trajectory. + """Make the robot turn to the left. :param float speed_factor: - Factor relative to the maximum motor speed, used to set the velocity, in the range -1.0 to 1.0. + Factor relative to the maximum motor speed, used to set the angular velocity, in the range -1.0 to 1.0. Using negative values will cause the robot to turn right. - :param float turn_radius: - Radius used by the robot to perform the movement. Using `turn_radius=0` will cause the robot to rotate in place. """ self.robot_move(linear_x=self._linear_speed_x_hold, @@ -162,10 +150,10 @@ def left(self, speed_factor): ) def right(self, speed_factor): - """Make the robot move to the right, using a circular trajectory. + """Make the robot turn to the right. :param float speed_factor: - Factor relative to the maximum motor speed, used to set the velocity, in the range -1.0 to 1.0. + Factor relative to the maximum motor speed, used to set the angular velocity, in the range -1.0 to 1.0. Using negative values will cause the robot to turn left. """ From 8e16a6c9dfabf9025a2db14ceffcbc1537e5d34c Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Tue, 11 May 2021 18:18:57 +0800 Subject: [PATCH 09/13] Add pan-tilt with pan follow mode and joystick deadzone --- examples/labs/4wd_rover_controller/main.py | 11 +- .../web/blueprints/rover/drive_handler_4wd.py | 121 ++++++++++++------ 2 files changed, 88 insertions(+), 44 deletions(-) diff --git a/examples/labs/4wd_rover_controller/main.py b/examples/labs/4wd_rover_controller/main.py index 5a337f71c..c54d75aea 100644 --- a/examples/labs/4wd_rover_controller/main.py +++ b/examples/labs/4wd_rover_controller/main.py @@ -1,18 +1,19 @@ -from pitop import Pitop, DriveController4WD, Camera +from pitop import Pitop, DriveController4WD, Camera, PanTiltController from pitop.labs import RoverWebController -from pitop.labs.web.blueprints.rover import DriveHandler4WD +from pitop.labs.web.blueprints.rover.drive_handler_4wd import DriveMode, DriveHandler4WD rover = Pitop() rover.add_component(DriveController4WD()) rover.add_component(Camera(resolution=(320, 240))) +rover.add_component(PanTiltController()) -drive_handler_4wd = DriveHandler4WD(drive=rover.drive) +drive_handler_4wd = DriveHandler4WD(drive=rover.drive, pan_tilt=rover.pan_tilt, mode=DriveMode.PAN_FOLLOW) rover_controller = RoverWebController( get_frame=rover.camera.get_frame, message_handlers={ - 'right_joystick': lambda data: drive_handler_4wd.update_linear(data), - 'left_joystick': lambda data: drive_handler_4wd.update_angular(data) + 'right_joystick': lambda data: drive_handler_4wd.right_joystick_update(data), + 'left_joystick': lambda data: drive_handler_4wd.left_joystick_update(data) } ) diff --git a/pitop/labs/web/blueprints/rover/drive_handler_4wd.py b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py index 8b9179aaa..1fae60295 100644 --- a/pitop/labs/web/blueprints/rover/drive_handler_4wd.py +++ b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py @@ -3,67 +3,110 @@ import time from .helpers import calculate_direction import math +from enum import IntEnum MAX_LINEAR_SPEED = 0.596 MAX_ANGULAR_SPEED = 3.24 +MAX_SERVO_ANGLE = 90.0 -def calculate_joystick_vector(data): +def get_joystick_distances(data): angle = data.get('angle', {}) degree = angle.get('degree', 0) distance = data.get('distance', 0) direction = math.radians(calculate_direction(degree)) - return distance, direction + horizontal_distance = distance * math.sin(direction) / 100.0 + vertical_distance = distance * math.cos(direction) / 100.0 + return horizontal_distance, vertical_distance -def calculate_linear_velocities(data): - distance, direction = calculate_joystick_vector(data) - linear_speed_x = (distance * MAX_LINEAR_SPEED / 100.0) * math.cos(direction) - linear_speed_y = (distance * MAX_LINEAR_SPEED / 100.0) * math.sin(direction) - return { - 'linear_speed_x': linear_speed_x, - 'linear_speed_y': linear_speed_y, - } +class DriveMode(IntEnum): + FREE = 0 # pan servo is not controlled by web controller + PAN_FOLLOW = 1 # pan servo is controlled by left joystick and chassis moves to follow it -def calculate_angular_velocity(data): - distance, direction = calculate_joystick_vector(data) - angular_speed_z = (distance * MAX_ANGULAR_SPEED / 100.0) * math.sin(direction) - return { - 'angular_speed_z': angular_speed_z - } +class DriveHandler4WD: + # TODO: investigate adding a timeout so robot stops if no new commands are sent after specified duration + _MAX_PAN_ANGLE = 20 + _MAX_TILT_SERVO_SPEED = 50.0 + _JOYSTICK_DEADZONE = 0.05 -class DriveHandler4WD: - def __init__(self, drive): + def __init__(self, drive, pan_tilt, mode: DriveMode = DriveMode.FREE): self._drive = drive - self._v_x = 0.0 - self._v_y = 0.0 - self._w_z = 0.0 - self._publish_dt = 1 / 10.0 - self._publish_thread = Thread(target=self.__publish_sheduler, daemon=True) - self._publish_thread.start() - - def update_linear(self, data): - linear_velocities = calculate_linear_velocities(data) - self._v_x = linear_velocities.get('linear_speed_x', 0) - self._v_y = linear_velocities.get('linear_speed_y', 0) - - def update_angular(self, data): - angular_velocity = calculate_angular_velocity(data) - self._w_z = angular_velocity.get('angular_speed_z', 0) - - def __publish_sheduler(self): + self._pan_tilt = pan_tilt + self._mode = mode + self._linear_speed_x = 0.0 + self._linear_speed_y = 0.0 + self._angular_speed = 0.0 + self._pan_servo_angle = 0.0 + self._tilt_servo_speed = 0.0 + self._command_dt = 1 / 8.0 + self._command_thread = Thread(target=self.__command_scheduler, daemon=True) + self._command_thread.start() + + def right_joystick_update(self, data): + horiz_distance, vert_distance = get_joystick_distances(data) + shifted_horiz_distance, shifted_vert_distance = self.__get_shifted_joystick_distances( + horiz_distance, vert_distance) + + if abs(vert_distance) > self._JOYSTICK_DEADZONE: + self._linear_speed_x = shifted_vert_distance * MAX_LINEAR_SPEED + else: + self._linear_speed_x = 0.0 + + if abs(horiz_distance) > self._JOYSTICK_DEADZONE: + self._linear_speed_y = shifted_horiz_distance * MAX_LINEAR_SPEED + else: + self._linear_speed_y = 0.0 + + def left_joystick_update(self, data): + horiz_distance, vert_distance = get_joystick_distances(data) + shifted_horiz_distance, shifted_vert_distance = self.__get_shifted_joystick_distances( + horiz_distance, vert_distance) + + if abs(vert_distance) > self._JOYSTICK_DEADZONE: + self._tilt_servo_speed = -shifted_vert_distance * self._MAX_TILT_SERVO_SPEED + else: + self._tilt_servo_speed = 0.0 + + if abs(horiz_distance) > self._JOYSTICK_DEADZONE: + self._angular_speed = shifted_horiz_distance * MAX_ANGULAR_SPEED + if self._mode == DriveMode.PAN_FOLLOW: + self._pan_tilt.pan_servo.target_speed = 25.0 # set lower speed for turning angle + pan_angle = shifted_horiz_distance * MAX_SERVO_ANGLE + self._pan_servo_angle = max(-self._MAX_PAN_ANGLE, min(self._MAX_PAN_ANGLE, pan_angle)) + else: + self._angular_speed = 0.0 + if self._mode == DriveMode.PAN_FOLLOW: + self._pan_tilt.pan_servo.target_speed = 100.0 # set highest speed for reset back to zero + self._pan_servo_angle = 0.0 + + def __get_shifted_joystick_distances(self, horizontal_distance, vertical_distance): + shifted_horiz_distance = horizontal_distance \ + - (self._JOYSTICK_DEADZONE * horizontal_distance / abs(horizontal_distance)) + shifted_vert_distance = vertical_distance \ + - (self._JOYSTICK_DEADZONE * vertical_distance / abs(vertical_distance)) + + return shifted_horiz_distance, shifted_vert_distance + + def __command_scheduler(self): s = scheduler(time.time, time.sleep) current_time = time.time() - s.enterabs(current_time + self._publish_dt, 1, self.__publish_velocity_commands, (s,)) + s.enterabs(current_time + self._command_dt, 1, self.__command_loop, (s,)) s.run() - def __publish_velocity_commands(self, s): + def __command_loop(self, s): current_time = time.time() - self._drive.robot_move(linear_x=self._v_x, linear_y=self._v_y, angular_z=self._w_z) + self._drive.robot_move(linear_x=self._linear_speed_x, + linear_y=self._linear_speed_y, + angular_z=self._angular_speed + ) + self._pan_tilt.tilt_servo.sweep(speed=self._tilt_servo_speed) + if self._mode == DriveMode.PAN_FOLLOW: + self._pan_tilt.pan_servo.target_angle = self._pan_servo_angle - s.enterabs(current_time + self._publish_dt, 1, self.__publish_velocity_commands, (s,)) + s.enterabs(current_time + self._command_dt, 1, self.__command_loop, (s,)) From 63a09ab60b195e8bdefa5521e6e5d0f9409d4788 Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Tue, 11 May 2021 20:05:47 +0800 Subject: [PATCH 10/13] Add calibrate --- examples/labs/4wd_rover_controller/main.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/labs/4wd_rover_controller/main.py b/examples/labs/4wd_rover_controller/main.py index c54d75aea..0c8095d72 100644 --- a/examples/labs/4wd_rover_controller/main.py +++ b/examples/labs/4wd_rover_controller/main.py @@ -6,6 +6,7 @@ rover.add_component(DriveController4WD()) rover.add_component(Camera(resolution=(320, 240))) rover.add_component(PanTiltController()) +rover.pan_tilt.calibrate() drive_handler_4wd = DriveHandler4WD(drive=rover.drive, pan_tilt=rover.pan_tilt, mode=DriveMode.PAN_FOLLOW) From 89433630db47fa66339f0ff6d8c99f815bdb8a73 Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Tue, 11 May 2021 20:06:02 +0800 Subject: [PATCH 11/13] Fix divide by zero error --- pitop/labs/web/blueprints/rover/drive_handler_4wd.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pitop/labs/web/blueprints/rover/drive_handler_4wd.py b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py index 1fae60295..2c82e7919 100644 --- a/pitop/labs/web/blueprints/rover/drive_handler_4wd.py +++ b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py @@ -86,9 +86,9 @@ def left_joystick_update(self, data): def __get_shifted_joystick_distances(self, horizontal_distance, vertical_distance): shifted_horiz_distance = horizontal_distance \ - - (self._JOYSTICK_DEADZONE * horizontal_distance / abs(horizontal_distance)) + - (self._JOYSTICK_DEADZONE * horizontal_distance / (abs(horizontal_distance) + 1e-6)) shifted_vert_distance = vertical_distance \ - - (self._JOYSTICK_DEADZONE * vertical_distance / abs(vertical_distance)) + - (self._JOYSTICK_DEADZONE * vertical_distance / (abs(vertical_distance) + 1e-6)) return shifted_horiz_distance, shifted_vert_distance From 78783c34c97ed9c0fa1442c131d2e28445e41160 Mon Sep 17 00:00:00 2001 From: Ryan Dunwoody Date: Tue, 11 May 2021 20:59:45 +0800 Subject: [PATCH 12/13] Clean up naming --- .../web/blueprints/rover/drive_handler_4wd.py | 40 ++++++++----------- 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/pitop/labs/web/blueprints/rover/drive_handler_4wd.py b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py index 2c82e7919..8dd7a55e6 100644 --- a/pitop/labs/web/blueprints/rover/drive_handler_4wd.py +++ b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py @@ -10,7 +10,7 @@ MAX_SERVO_ANGLE = 90.0 -def get_joystick_distances(data): +def get_joystick_coordinates(data): angle = data.get('angle', {}) degree = angle.get('degree', 0) distance = data.get('distance', 0) @@ -48,35 +48,35 @@ def __init__(self, drive, pan_tilt, mode: DriveMode = DriveMode.FREE): self._command_thread.start() def right_joystick_update(self, data): - horiz_distance, vert_distance = get_joystick_distances(data) - shifted_horiz_distance, shifted_vert_distance = self.__get_shifted_joystick_distances( - horiz_distance, vert_distance) + x, y = get_joystick_coordinates(data) - if abs(vert_distance) > self._JOYSTICK_DEADZONE: - self._linear_speed_x = shifted_vert_distance * MAX_LINEAR_SPEED + if abs(y) > self._JOYSTICK_DEADZONE: + shifted_y = y - self._JOYSTICK_DEADZONE * y / abs(y) + self._linear_speed_x = shifted_y * MAX_LINEAR_SPEED else: self._linear_speed_x = 0.0 - if abs(horiz_distance) > self._JOYSTICK_DEADZONE: - self._linear_speed_y = shifted_horiz_distance * MAX_LINEAR_SPEED + if abs(x) > self._JOYSTICK_DEADZONE: + shifted_x = x - self._JOYSTICK_DEADZONE * x / abs(x) + self._linear_speed_y = shifted_x * MAX_LINEAR_SPEED else: self._linear_speed_y = 0.0 def left_joystick_update(self, data): - horiz_distance, vert_distance = get_joystick_distances(data) - shifted_horiz_distance, shifted_vert_distance = self.__get_shifted_joystick_distances( - horiz_distance, vert_distance) + x, y = get_joystick_coordinates(data) - if abs(vert_distance) > self._JOYSTICK_DEADZONE: - self._tilt_servo_speed = -shifted_vert_distance * self._MAX_TILT_SERVO_SPEED + if abs(y) > self._JOYSTICK_DEADZONE: + shifted_y = y - self._JOYSTICK_DEADZONE * y / abs(y) + self._tilt_servo_speed = -shifted_y * self._MAX_TILT_SERVO_SPEED else: self._tilt_servo_speed = 0.0 - if abs(horiz_distance) > self._JOYSTICK_DEADZONE: - self._angular_speed = shifted_horiz_distance * MAX_ANGULAR_SPEED + if abs(x) > self._JOYSTICK_DEADZONE: + shifted_x = x - self._JOYSTICK_DEADZONE * x / abs(x) + self._angular_speed = shifted_x * MAX_ANGULAR_SPEED if self._mode == DriveMode.PAN_FOLLOW: self._pan_tilt.pan_servo.target_speed = 25.0 # set lower speed for turning angle - pan_angle = shifted_horiz_distance * MAX_SERVO_ANGLE + pan_angle = shifted_x * MAX_SERVO_ANGLE self._pan_servo_angle = max(-self._MAX_PAN_ANGLE, min(self._MAX_PAN_ANGLE, pan_angle)) else: self._angular_speed = 0.0 @@ -84,14 +84,6 @@ def left_joystick_update(self, data): self._pan_tilt.pan_servo.target_speed = 100.0 # set highest speed for reset back to zero self._pan_servo_angle = 0.0 - def __get_shifted_joystick_distances(self, horizontal_distance, vertical_distance): - shifted_horiz_distance = horizontal_distance \ - - (self._JOYSTICK_DEADZONE * horizontal_distance / (abs(horizontal_distance) + 1e-6)) - shifted_vert_distance = vertical_distance \ - - (self._JOYSTICK_DEADZONE * vertical_distance / (abs(vertical_distance) + 1e-6)) - - return shifted_horiz_distance, shifted_vert_distance - def __command_scheduler(self): s = scheduler(time.time, time.sleep) current_time = time.time() From 98887a6b011e5b3b834f5a0753a0a931718cfd70 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 10 Aug 2021 11:04:51 +0000 Subject: [PATCH 13/13] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- pitop/robotics/drive_controller_4wd.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pitop/robotics/drive_controller_4wd.py b/pitop/robotics/drive_controller_4wd.py index ea0f269fe..7f345bf0e 100644 --- a/pitop/robotics/drive_controller_4wd.py +++ b/pitop/robotics/drive_controller_4wd.py @@ -180,8 +180,8 @@ def stop_rotation(self): """Stop any angular movement performed by the robot. In the case where linear and rotational movements are being - performed at the same time, calling this method will cause the robot - to continue the linear movement, so it will continue to move - forward. + performed at the same time, calling this method will cause the + robot to continue the linear movement, so it will continue to + move forward. """ self.robot_move(linear_x=self._linear_speed_x_hold, linear_y=self._linear_speed_y_hold, angular_z=0)