Python MyMath.sub180Angle方法代码示例

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def getOmniWalkParam(my, dest):
    # we use distance and bearing to get relX, relY which we already have
    # for the ball. be nice not to recalculate it.
    relX, relY = 0, 0

    if hasattr(dest, "relX") and \
            hasattr(dest, "relY") and \
            hasattr(dest, "relH"):
        relX = dest.relX
        relY = dest.relY
        relH = dest.relH

    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relY = MyMath.getRelativeY(distToDest, bearingDeg)
        relH = MyMath.sub180Angle(dest.h - my.h)

    # calculate forward speed
    forwardGain = constants.OMNI_GOTO_X_GAIN * relX
    sX = constants.OMNI_GOTO_FORWARD_SPEED * forwardGain
    sX = MyMath.clip(sX,
                     constants.OMNI_MIN_X_SPEED,
                     constants.OMNI_MAX_X_SPEED)
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0

    # calculate sideways speed
    strafeGain = constants.OMNI_GOTO_Y_GAIN * relY
    sY = constants.OMNI_GOTO_STRAFE_SPEED  * strafeGain
    sY = MyMath.clip(sY,
                     constants.OMNI_MIN_Y_SPEED,
                     constants.OMNI_MAX_Y_SPEED,)
    if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
        sY = 0

    # calculate spin speed
    spinGain = constants.GOTO_SPIN_GAIN
    hDiff = MyMath.sub180Angle(dest.h - my.h)

    if (fabs(hDiff) < 2.0):
        sTheta = 0.0
    else:
        sTheta = MyMath.sign(hDiff) * getRotScale(hDiff) * \
                 constants.OMNI_MAX_SPIN_SPEED * spinGain

        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MIN_SPIN_SPEED,
                             constants.OMNI_MAX_SPIN_SPEED)

    return (sX, sY, sTheta)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def atHeading(my, targetHeading):
    """
    Returns true if we are at a heading close enough to what we want
    """
    hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading))
    return hDiff < constants.CLOSE_ENOUGH_H and \
           my.uncertH < constants.LOC_IS_ACTIVE_H

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def getWalkStraightParam(my, dest):
    relX, relH = 0, 0
    if hasattr(dest, "relX") and \
            hasattr(dest, "relH"):
        relX = dest.relX
        relH = dest.relH
    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relH = MyMath.sub180Angle(dest.h - my.h)

    # calculate spin speed
    if (fabs(relH) < 25.0):
        sTheta = 0.0
    else: #spin first
        spinGain = 20. / constants.APPROACH_THETA_WITH_GAIN_DIST # 20degs/sec in theta
        sTheta = relH * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)
        return (0, 0, sTheta)

   # calculate forward speed if h is good.
    forwardGain = 20. / constants.APPROACH_X_WITH_GAIN_DIST # 20cm/sec in x direction
    sX = relX * forwardGain
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0
    else:
        sX = MyMath.clip(sX,
                         constants.GOTO_BACKWARD_SPEED,
                         constants.GOTO_FORWARD_SPEED)

    return (sX, 0, 0)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def getWalkBackParam(my, dest):
    relX, relH = 0, 0
    if hasattr(dest, "relX") and \
            hasattr(dest, "relH"):
        relX = dest.relX
        relH = dest.relH
    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relH = MyMath.sub180Angle(dest.h - my.h)

    if not fabs(relH) > 150 :
        spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST
        sTheta = (180-relH) * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)
        return ( 0, 0, sTheta)

    forwardGain = constants.APPROACH_X_WITH_GAIN_DIST
    sX = relX * forwardGain
    sX = MyMath.clip(sX,
                     constants.GOTO_BACKWARD_SPEED,
                     constants.GOTO_FORWARD_SPEED)

    return (sX, 0, 0)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def getOmniWalkParam(my, dest):
    # we use distance and bearing to get relX, relY which we already have
    # for the ball. be nice not to recalculate it.
    relX, relY, relH = 0, 0, 0
    if hasattr(dest, "relX") and \
           hasattr(dest, "relY"):
        relX = dest.relX
        relY = dest.relY
    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relY = MyMath.getRelativeY(distToDest, bearingDeg)

    if hasattr(dest, "relH"):
        relH = dest.relH
    elif hasattr(dest, "bearing"):
        relH = dest.bearing
    else:
        relH = MyMath.sub180Angle(dest.h - my.h)

    # calculate forward speed
    forwardGain = constants.APPROACH_X_WITH_GAIN_DIST
    sX = relX * forwardGain
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0
    else:
        sX = MyMath.clip(sX,
                         constants.OMNI_REV_MAX_SPEED,
                         constants.OMNI_FWD_MAX_SPEED)

    # calculate sideways speed
    strafeGain = constants.APPROACH_Y_WITH_GAIN_DIST
    sY = relY * strafeGain
    if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
        sY = 0
    else:
        sY = MyMath.clip(sY,
                         constants.OMNI_RIGHT_MAX_SPEED,
                         constants.OMNI_LEFT_MAX_SPEED,)

    # calculate spin speed
    if (fabs(relH) < 10.0):
        sTheta = 0.0
    else:
        spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST
        sTheta = relH * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)

    denom = sqrt(sX*sX + sY*sY + sTheta*sTheta) / constants.OMNI_GAIN
    if denom != 0:
        sX     /= denom
        sY     /= denom
        sTheta /= denom

    return (sX, sY, sTheta)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def getOmniWalkParam(my, dest):
    # we use distance and bearing to get relX, relY which we already have
    # for the ball. be nice not to recalculate it.
    relX, relY, relH = 0, 0, 0
    if hasattr(dest, "relX") and \
            hasattr(dest, "relY") and \
            hasattr(dest, "relH"):
        relX = dest.relX
        relY = dest.relY
        relH = dest.relH
    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relY = MyMath.getRelativeY(distToDest, bearingDeg)
        relH = MyMath.sub180Angle(dest.h - my.h)

    # calculate forward speed
    forwardGain = 20. / constants.APPROACH_X_WITH_GAIN_DIST # 20cm/sec in x direction
    sX = relX * forwardGain
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0
    else:
        sX = MyMath.clip(sX,
                         constants.OMNI_REV_MAX_SPEED,
                         constants.OMNI_FWD_MAX_SPEED)

    # calculate sideways speed
    strafeGain = 15. / constants.APPROACH_Y_WITH_GAIN_DIST # 15cm/sec in y direction
    sY = relY * strafeGain
    if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
        sY = 0
    else:
        sY = MyMath.clip(sY,
                         constants.OMNI_RIGHT_MAX_SPEED,
                         constants.OMNI_LEFT_MAX_SPEED,)

    # calculate spin speed
    if (fabs(relH) < 25.0):
        sTheta = 0.0
    else:
        spinGain = 20. / constants.APPROACH_THETA_WITH_GAIN_DIST # 20degs/sec in theta
        sTheta = relH * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)

    # refine x and y speeds
    if (fabs(relH) > 50):
        sX = 0
        sY = 0

    elif (fabs(relH) > 35):
        sY = 0

    return (sX, sY, sTheta)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def atHeading(nav):
    """
    Returns true if we are at a heading close enough to what we want
    """
    my = nav.brain.my
    dest = nav.getDestination()

    if nav.destType is constants.BALL:
        return abs(nav.brain.ball.bearing) < constants.CLOSE_ENOUGH_H

    hDiff = fabs(MyMath.sub180Angle(my.h - dest.h))
    return hDiff < constants.CLOSE_ENOUGH_H and \
           my.uncertH < constants.LOC_IS_ACTIVE_H

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def getWalkSpinParam(my, dest):
    relX, relH = 0, 0
    if hasattr(dest, "relX") and \
            hasattr(dest, "relH"):
        relX = dest.relX
        relH = dest.relH
    else:
        bearingDeg = my.getRelativeBearing(dest)
        distToDest = my.distTo(dest)
        relX = MyMath.getRelativeX(distToDest, bearingDeg)
        relH = MyMath.sub180Angle(dest.h - my.h)

   # calculate forward speed
    forwardGain = 1./constants.APPROACH_X_WITH_GAIN_DIST
    sX = relX * forwardGain
    if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
        sX = 0
    else:
        sX = MyMath.clip(sX,
                         constants.OMNI_REV_MAX_SPEED,
                         constants.OMNI_FWD_MAX_SPEED)

    # calculate spin speed
    if (fabs(relH) < 5.0):
        sTheta = 0.0
    else:
        spinGain = 1./constants.APPROACH_THETA_WITH_GAIN_DIST
        sTheta = relH * spinGain
        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MAX_RIGHT_SPIN_SPEED,
                             constants.OMNI_MAX_LEFT_SPIN_SPEED)
    # Correct sX
    if fabs(relH)  > 50.:
        sX = 0

    return (sX, 0, sTheta)

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def pfk_all(nav):
    """
    ball bearing is inside safe margin for spinning
    try to get almost all heading adjustment done here
    move x, y, and theta
    """

    if nav.firstFrame():
        nav.stopTheta = 0
        nav.stopY_Theta = 0
        print "entered from: ", nav.lastDiffState

    (x_offset, y_offset, heading) = nav.kick.getPosition()

    ball = nav.brain.ball

    # calculate spin speed
    # hDiff = MyMath.sub180Angle(heading - nav.brain.my.h)
    # rotate to ball here, we will strafe to aim kick later
    hDiff = MyMath.sub180Angle(ball.bearing)

    if (fabs(hDiff) < PFK_CLOSE_ENOUGH_THETA):
        sTheta = 0.0
    else:
        sTheta = MyMath.sign(hDiff) * constants.GOTO_SPIN_SPEED * \
                 walker.getCloseRotScale(hDiff)

        sTheta = MyMath.clip(sTheta,
                             constants.OMNI_MIN_SPIN_SPEED,
                             constants.OMNI_MAX_SPIN_SPEED)

    if fabs(hDiff) < PFK_CLOSE_ENOUGH_THETA:
        nav.stopTheta += 1
        if nav.stopTheta > BUFFER_FRAMES_THRESHOLD:
            return nav.goNow('pfk_xy')
    else:
        nav.stopTheta = 0

    # if the ball is outside safe bearing and we're spinning away from it
    # or we're spinning towards it but we're likely to spin into it...
    # or we're close enough to the correct bearing
    if fabs(ball.bearing) > STOP_SPIN_BEARING and \
       ((MyMath.sign(ball.bearing) != MyMath.sign(hDiff))
            or ball.relX < SAFE_BALL_REL_X \
            or sTheta == 0.0):
        return nav.goNow('pfk_xy')

    target_y = ball.relY - y_offset

    # arbitrary
    if fabs(target_y) < PFK_CLOSE_ENOUGH_XY:
        sY = 0
    else:
        sY = MyMath.clip(target_y * PFK_Y_GAIN,
                         PFK_MIN_Y_SPEED,
                         PFK_MAX_Y_SPEED)
        sY = max(PFK_MIN_Y_MAGNITUDE,sY) * MyMath.sign(sY)

    if sY == 0.0 and sTheta == 0.0:
        nav.stopY_Theta += 1
        if nav.stopY_Theta > BUFFER_FRAMES_THRESHOLD:
            return nav.goNow('pfk_final')
    else:
        nav.stopY_Theta = 0

    x_diff = ball.relX - SAFE_BALL_REL_X
    # arbitrary
    if fabs(x_diff) < PFK_CLOSE_ENOUGH_XY:
        sX = 0.0
    else:
        sX = MyMath.clip(x_diff * PFK_X_GAIN,
                         PFK_MIN_X_SPEED,
                         PFK_MAX_X_SPEED)
        sX = max(PFK_MIN_X_MAGNITUDE,sX) * MyMath.sign(sX)

    print "hDiff:%g target_y:%g x_diff:%g" % (hDiff, target_y, x_diff)
    print "sTheta:%g sY:%g sX:%g" % (sTheta, sY, sX)
    helper.setSlowSpeed(nav,sX,sY,sTheta)

    return nav.stay()

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def notAtHeading(my, targetHeading):
    hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading))

    return hDiff > constants.ALMOST_CLOSE_ENOUGH_H and \
           my.uncertH < constants.LOC_IS_ACTIVE_H

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def atHeadingGoTo(my, targetHeading):
    hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading))
    return hDiff < constants.AT_HEADING_GOTO_DEG

# 需要导入模块: from man.noggin.util import MyMath [as 别名]
# 或者: from man.noggin.util.MyMath import sub180Angle [as 别名]
def shouldChaseOrbit(myH, destH):
    hDiff = MyMath.sub180Angle(myH - destH)
    return( fabs(hDiff) > KICK_STRAIGHT_BEARING_THRESH)