86 lines
3.4 KiB
Python
86 lines
3.4 KiB
Python
from Thing import Thing
|
|
from LinearAlgebra.Spherical import Spherical
|
|
from LinearAlgebra.Direction import Direction
|
|
from LinearAlgebra.Angle import Angle
|
|
|
|
class DifferentialDrive(Thing):
|
|
"""! A thing which can move itself using a differential drive system
|
|
"""
|
|
def __init__(self, participant = None):
|
|
"""! Create a new differential drive
|
|
"""
|
|
super().__init__(participant)
|
|
|
|
## The radius of a wheel in meters
|
|
self.wheel_radius = 1.0
|
|
## The distance between the wheels in meters
|
|
self.wheel_separation = 1.0
|
|
|
|
self.wheel_left: Thing = None
|
|
self.wheel_right: Thing = None
|
|
|
|
def SetDriveDimensions(self, wheel_diameter, wheel_separation):
|
|
if wheel_diameter < 0:
|
|
wheel_diameter = -wheel_diameter
|
|
self.wheel_radius = wheel_diameter / 2
|
|
|
|
if wheel_separation < 0:
|
|
wheel_separation = -wheel_separation
|
|
self.wheel_separation = wheel_separation
|
|
|
|
self.SetMotors(self.wheel_left, self.wheel_right)
|
|
|
|
def SetMotors(self, wheel_left, wheel_right):
|
|
distance: float = self.wheel_separation / 2
|
|
|
|
self.wheel_left = wheel_left
|
|
if self.wheel_left is not None:
|
|
self.wheel_left.SetPosition(Spherical(distance, Direction.Left))
|
|
|
|
self.wheel_right = wheel_right
|
|
if self.wheel_right is not None:
|
|
self.wheel_right.SetPosition(Spherical(distance, Direction.Right))
|
|
|
|
def SetWheelVelocity(self, speed_left, speed_right):
|
|
"""! Directly specify the speeds of the motors
|
|
@params speed_left The speed of the left wheel in degrees per second. Positive moves the robot in the forward direction.
|
|
@params speed_right The speed of the right wheel in degrees per second. Positive moves the robot in the forward direction.
|
|
"""
|
|
if self.wheel_left is not None:
|
|
self.wheel_left.SetAngularVelocity(Spherical(speed_left, Direction.Left))
|
|
|
|
if self.wheel_right is not None:
|
|
self.wheel_right.SetAngularVelocity(Spherical(speed_right, Direction.Right))
|
|
|
|
speed = self.wheel_radius * (speed_left + speed_right) / 2 * Angle.Deg2Rad
|
|
self.SetLinearVelocity(Spherical(speed, Direction.forward))
|
|
steer = self.wheel_radius * (speed_left - speed_right) / self.wheel_separation
|
|
self.SetAngularVelocity(Spherical(steer, Direction.up))
|
|
|
|
def Update(self, currentTimeMs, recursive = True):
|
|
"""!
|
|
@copydoc Thing::Update
|
|
"""
|
|
# This assumes forward velocity only...
|
|
linear_velocity: Spherical = self.linear_velocity.distance
|
|
|
|
angular_velocity: Spherical = self.angular_velocity
|
|
angular_speed: float = angular_velocity.distance * Angle.Deg2Rad # in radians/sec
|
|
# Determine the rotation direction
|
|
if angular_velocity.direction.horizontal < 0:
|
|
angular_speed = -angular_speed
|
|
|
|
if self.wheel_left is None or self.wheel_right is None:
|
|
return
|
|
|
|
speed_left: float = 0 if self.wheel_left is None else \
|
|
(linear_velocity + angular_speed * self.wheel_left.position.distance) / self.wheel_radius * Angle.Rad2Deg
|
|
speed_right: float = 0 if self.wheel_right is None else \
|
|
(linear_velocity - angular_speed * self.wheel_right.position.distance) / self.wheel_radius * Angle.Rad2Deg
|
|
|
|
self.SetWheelVelocity(speed_left, speed_right)
|
|
|
|
super().Update(currentTimeMs)
|
|
|
|
|