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