def change_radius(speeds, radius):
if radius is None:
return
if abs(radius) > 0.0:
if abs(2.0 * radius + ROBO_WIDTH) > 0.0:
if radius < 0.0:
# turn left
speed_left = speeds.speed_right * (2.0 * radius - ROBO_WIDTH) / (2.0 * radius + ROBO_WIDTH)
if abs(speeds.speed_left) > 0.0:
factor_for_left = abs(speed_left / speeds.speed_left)
speeds.speed_front_left = logic.sign(speeds.speed_front_left) * factor_for_left * \
abs(speeds.speed_front_left)
speeds.speed_rear_left = logic.sign(speeds.speed_rear_left) * factor_for_left * \
abs(speeds.speed_rear_left)
else:
# turn right
speed_right = speeds.speed_left * (2.0 * radius - ROBO_WIDTH) / (2.0 * radius + ROBO_WIDTH)
if abs(speeds.speed_right) > 0.0:
factor_for_right = abs(speed_right / speeds.speed_right)
speeds.speed_front_right = logic.sign(speeds.speed_front_right) * factor_for_right * \
abs(speeds.speed_front_right)
speeds.speed_rear_right = logic.sign(speeds.speed_rear_right) * factor_for_right * \
abs(speeds.speed_rear_right)
else:
# rotate in place
if speeds.radius < 0:
# rotate in left
speeds.speed_front_left = -speeds.speed_front_right
speeds.speed_rear_left = -speeds.speed_rear_right
else:
# rotate in right
speeds.speed_front_right = -speeds.speed_front_left
speeds.speed_rear_right = -speeds.speed_rear_left
speeds.compute_other_speed()
def __compute_speed(self, drive_angle, drive_distance):
alpha = 0.17453292519943295
beta = 0.5235987755982988
if abs(drive_angle) < alpha: # 10st
# drive normal
left, right = MAX_SPEED, MAX_SPEED
elif abs(drive_angle) > beta: # 60st
# rotate in place
left = -MAX_SPEED
right = MAX_SPEED
if drive_angle < 0:
left, right = right, left
else:
# drive on turn
left = MAX_SPEED - DriveToPoint.compute_change(drive_angle, math.pi / beta)
right = MAX_SPEED + DriveToPoint.compute_change(drive_angle, math.pi / beta)
_left, _right = self.__speeds_filter(abs(left), abs(right))
left, right = logic.sign(left) * _left, logic.sign(right) * _right
return left, right
def __compute_speed(self, drive_angle, drive_distance):
alpha = 0.17453292519943295
beta = 0.5235987755982988
if abs(drive_angle) < alpha: # 10st
# drive normal
left, right = MAX_SPEED, MAX_SPEED
elif abs(drive_angle) > beta: # 60st
# rotate in place
left = -MAX_SPEED
right = MAX_SPEED
if drive_angle < 0:
left, right = right, left
else:
# drive on turn
left = MAX_SPEED - DriveToPoint.compute_change(
drive_angle, math.pi / beta)
right = MAX_SPEED + DriveToPoint.compute_change(
drive_angle, math.pi / beta)
_left, _right = self.__speeds_filter(abs(left), abs(right))
left, right = logic.sign(left) * _left, logic.sign(right) * _right
return left, right
