[WIP] 4wd controller

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()

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

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):

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,))

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

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)