RoboidControl/RoboidControl-python
RoboidControl/RoboidControl-python
1
0
Fork 0
Code Issues Pull Requests Actions Packages Releases Activity

Improved working diff. drive

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-17 17:53:37 +01:00
parent 061f0d3eb7
commit 9b421c0044
1 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
Things/DifferentialDrive.py
View File

@ -61,24 +61,24 @@ class DifferentialDrive(Thing):
"""!
@copydoc Thing::Update
"""
# This assumes forward velocity only...
linear_velocity: Spherical = self.linear_velocity.distance
if not (self.linear_velocity_updated or self.angular_velocity_updated):
# This assumes no sideward velocity because diff drive does not support it
linear_speed = self.linear_velocity.distance
# When the direction is backwards...
if self.linear_velocity.direction.horizontal < -90 or self.linear_velocity.direction.horizontal > 90:
linear_speed = -linear_speed
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
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
if self.wheel_left is not None and self.wheel_right is not None:
speed_left: float = 0 if self.wheel_left is None else (linear_speed + angular_speed * self.wheel_left.position.distance) / self.wheel_radius * Angle.Rad2Deg
speed_right: float = 0 if self.wheel_right is None else (linear_speed - angular_speed * self.wheel_right.position.distance) / self.wheel_radius * Angle.Rad2Deg
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)
self.SetWheelVelocity(speed_left, speed_right)
super().Update(currentTimeMs)