diff --git a/Things/DifferentialDrive.py b/Things/DifferentialDrive.py index c10e350..6ed5ff1 100644 --- a/Things/DifferentialDrive.py +++ b/Things/DifferentialDrive.py @@ -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 - - 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 + 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 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 - self.SetWheelVelocity(speed_left, speed_right) + self.SetWheelVelocity(speed_left, speed_right) super().Update(currentTimeMs)