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
예제 #3
0
    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