diff --git a/examples/labs/4wd_rover_controller/main.py b/examples/labs/4wd_rover_controller/main.py new file mode 100644 index 000000000..0c8095d72 --- /dev/null +++ b/examples/labs/4wd_rover_controller/main.py @@ -0,0 +1,21 @@ +from pitop import Pitop, DriveController4WD, Camera, PanTiltController +from pitop.labs import RoverWebController +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()) +rover.pan_tilt.calibrate() + +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.right_joystick_update(data), + 'left_joystick': lambda data: drive_handler_4wd.left_joystick_update(data) + } +) + +rover_controller.serve_forever() diff --git a/pitop/__init__.py b/pitop/__init__.py index d3ad146ed..9e8c13eba 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.tilt_roll_head_controller import TiltRollHeadController from .robotics.pincer_controller import PincerController 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..8dd7a55e6 --- /dev/null +++ b/pitop/labs/web/blueprints/rover/drive_handler_4wd.py @@ -0,0 +1,104 @@ +from sched import scheduler +from threading import Thread +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 get_joystick_coordinates(data): + angle = data.get('angle', {}) + degree = angle.get('degree', 0) + distance = data.get('distance', 0) + direction = math.radians(calculate_direction(degree)) + + horizontal_distance = distance * math.sin(direction) / 100.0 + vertical_distance = distance * math.cos(direction) / 100.0 + + return horizontal_distance, vertical_distance + + +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 + + +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 + + def __init__(self, drive, pan_tilt, mode: DriveMode = DriveMode.FREE): + self._drive = drive + 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): + x, y = get_joystick_coordinates(data) + + 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(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): + x, y = get_joystick_coordinates(data) + + 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(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_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 + 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 __command_scheduler(self): + s = scheduler(time.time, time.sleep) + current_time = time.time() + s.enterabs(current_time + self._command_dt, 1, self.__command_loop, (s,)) + s.run() + + def __command_loop(self, s): + current_time = time.time() + + 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._command_dt, 1, self.__command_loop, (s,)) diff --git a/pitop/robotics/drive_controller.py b/pitop/robotics/drive_controller.py index 446abe8e4..077d8ba8b 100644 --- a/pitop/robotics/drive_controller.py +++ b/pitop/robotics/drive_controller.py @@ -54,6 +54,13 @@ def __init__(self, left_motor_port="M3", right_motor_port="M0", name="drive"): "right_motor_port": right_motor_port, "name": self.name}) + def is_initialized(fcn): + def check_initialization(self, *args, **kwargs): + if not self._initialized: + raise UninitializedComponent("DriveController not initialized") + return fcn(self, *args, **kwargs) + return check_initialization + 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 diff --git a/pitop/robotics/drive_controller_4wd.py b/pitop/robotics/drive_controller_4wd.py new file mode 100644 index 000000000..7f345bf0e --- /dev/null +++ b/pitop/robotics/drive_controller_4wd.py @@ -0,0 +1,187 @@ +from math import floor +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 four mecanum wheels.""" + _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) + + # 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 turn to the left. + + :param float speed_factor: + 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. + """ + + 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 turn to the right. + + :param float speed_factor: + 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. + """ + + 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)