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)
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
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)
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)
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)
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)
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
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)
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()
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
def atHeadingGoTo(my, targetHeading): hDiff = fabs(MyMath.sub180Angle(my.h - targetHeading)) return hDiff < constants.AT_HEADING_GOTO_DEG
def shouldChaseOrbit(myH, destH): hDiff = MyMath.sub180Angle(myH - destH) return( fabs(hDiff) > KICK_STRAIGHT_BEARING_THRESH)