stages.py 文件源码

def polar_goals(x, y):
    """
    Use a polar coordinate system to calculate goals from given X and Y
    values.

    :param x: x cartesian coordinate
    :param y: y cartesian coordinate
    :return: polar coordinate derived base, fibia, and tibia servo goal values
    """
    if y < 0:
        raise ValueError("Cannot accept negative Y values.")

    # base servo value which represents 0 degrees. Strongly dependent upon
    # physical construction, servo installation and location of arm
    polar_axis = 210
    polar_180_deg = 810  # base servo value which represents 180 degrees
    polar_diff = polar_180_deg - polar_axis

    x_shift = Decimal(MAX_IMAGE_WIDTH / 2).quantize(
        exp=Decimal('1.0'), rounding=ROUND_HALF_UP)
    log.info("[polar_goals] x_shift:{0}".format(x_shift))
    # shift origin of x, y to center of base
    shifted_x = x - x_shift
    log.info("[polar_goals] new_x:{0} new_y:{1}".format(shifted_x, y))

    # horizontal kicker to overcome what we think is image parallax
    if 40 < abs(shifted_x) < 10:
        shifted_x *= 1.25
    elif 90 < abs(shifted_x) < 40:
        shifted_x *= 1.40

    # convert shifted x and y to rho and theta
    rho, theta = cart2polar(int(shifted_x), int(y))
    log.info("[polar_goals] r:{0} theta:{1}".format(rho, theta))

    # convert theta into base rotation goal
    bg = int((polar_diff / 180) * theta + polar_axis)
    # ensure base goal does not go beyond the 180 or 0 servo rotation boundaries
    if bg > polar_180_deg:
        bg = polar_180_deg
    if bg < polar_axis:
        bg = polar_axis

    bg_cart, fg, tg = cartesian_goals(x, y)
    # Return polar coordinates for base and cartesian for arm goals. We found
    # this combination to work best through experimentation over hours of
    # operation.
    return bg + 20, fg, tg