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)
