63 lines
2.4 KiB
Python
63 lines
2.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):
|
|
def __init__(self, participant = None):
|
|
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 = None
|
|
self.wheel_right = 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):
|
|
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))
|
|
|
|
def Update(self, currentTimeMs, recursive = True):
|
|
# This assumes forward velocity only...
|
|
linear_velocity: Spherical = self.GetLinearVelocity().distance
|
|
|
|
angular_velocity: Spherical = self.GetAngularVelocity()
|
|
angular_speed: float = angular_velocity.distance * Angle.Deg2Rad # in radians/sec
|
|
# Determine the rotation direction
|
|
if angular_velocity.direction.horizontal.InDegrees() < 0:
|
|
angular_speed = -angular_speed
|
|
|
|
speed_left: float = (linear_velocity + angular_speed * self.wheel_left.GetPosition().distance) / self.wheel_radius * Angle.Rad2Deg
|
|
speed_right: float = (linear_velocity - angular_speed * self.wheel_right.GetPosition().distance) / self.wheel_radius * Angle.Rad2Deg
|
|
|
|
self.SetWheelVelocity(speed_left, speed_right)
|
|
|
|
|