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 getWalkStraightParam(my, dest):
distToDest = my.distTo(dest)
if distToDest < ChaseBallConstants.APPROACH_WITH_GAIN_DIST:
gain = constants.GOTO_FORWARD_GAIN * distToDest
else:
gain = 1.0
sX = MyMath.clip(constants.GOTO_FORWARD_SPEED*gain,
constants.WALK_TO_MIN_X_SPEED,
constants.WALK_TO_MAX_X_SPEED)
bearingDeg= my.getRelativeBearing(dest)
if (fabs(bearingDeg) < 2.0):
sTheta = 0.0
else:
sTheta = MyMath.clip(MyMath.sign(bearingDeg) *
constants.GOTO_STRAIGHT_SPIN_SPEED *
getRotScale(bearingDeg),
-constants.GOTO_STRAIGHT_SPIN_SPEED,
constants.GOTO_STRAIGHT_SPIN_SPEED )
sY = 0
return (sX, sY, sTheta)
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 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 approachBallWalk(player):
"""
Method that is used by both approach ball and dribble
We use things as to when we should leave and how we should walk
"""
if not player.brain.play.isRole(GOALIE):
if transitions.shouldNotGoInBox(player):
return player.goLater('ballInMyBox')
elif transitions.shouldChaseAroundBox(player):
return player.goLater('chaseAroundBox')
elif transitions.shouldApproachBallWithLoc(player):
return player.goNow('approachBallWithLoc')
elif transitions.shouldTurnToBall_ApproachBall(player):
return player.goLater('turnToBall')
elif not player.brain.tracker.activeLocOn and \
transitions.shouldScanFindBall(player):
return player.goLater('scanFindBall')
elif player.brain.tracker.activeLocOn and \
transitions.shouldScanFindBallActiveLoc(player):
return player.goLater('scanFindBall')
elif transitions.shouldAvoidObstacleDuringApproachBall(player):
return player.goLater('avoidObstacle')
# Determine our speed for approaching the ball
ball = player.brain.ball
if player.brain.play.isRole(GOALIE) and goalTran.dangerousBall(player):
return player.goNow('approachDangerousBall')
if ball.dist < constants.APPROACH_WITH_GAIN_DIST:
sX = MyMath.clip(ball.dist * constants.APPROACH_X_GAIN,
constants.MIN_APPROACH_X_SPEED,
constants.MAX_APPROACH_X_SPEED)
else:
sX = constants.MAX_APPROACH_X_SPEED
# Determine the speed to turn to the ball
sTheta = MyMath.clip(ball.bearing * constants.APPROACH_SPIN_GAIN,
-constants.APPROACH_SPIN_SPEED,
constants.APPROACH_SPIN_SPEED)
# Avoid spinning so slowly that we step in place
if fabs(sTheta) < constants.MIN_APPROACH_SPIN_MAGNITUDE:
sTheta = 0.0
# Set our walk towards the ball
if ball.on:
player.setWalk(sX, 0, sTheta)
return player.stay()
def approachBallWalk(player):
"""
Method that is used by both approach ball and dribble
We use things as to when we should leave and how we should walk
"""
if player.brain.playbook.role != pbc.GOALIE:
if transitions.shouldNotGoInBox(player):
return player.goLater('ballInMyBox')
elif transitions.shouldChaseAroundBox(player):
return player.goLater('chaseAroundBox')
elif transitions.shouldApproachBallWithLoc(player):
return player.goNow('approachBallWithLoc')
elif transitions.shouldTurnToBall_ApproachBall(player):
return player.goLater('turnToBall')
elif not player.brain.tracker.activeLocOn and \
transitions.shouldScanFindBall(player):
return player.goLater('scanFindBall')
elif player.brain.tracker.activeLocOn and \
transitions.shouldScanFindBallActiveLoc(player):
return player.goLater('scanFindBall')
elif transitions.shouldAvoidObstacleDuringApproachBall(player):
return player.goLater('avoidObstacle')
# Determine our speed for approaching the ball
ball = player.brain.ball
if player.brain.playbook.role == pbc.GOALIE and goalTran.dangerousBall(player):
return player.goNow('approachDangerousBall')
if ball.dist < constants.APPROACH_WITH_GAIN_DIST:
sX = MyMath.clip(ball.dist*constants.APPROACH_X_GAIN,
constants.MIN_APPROACH_X_SPEED,
constants.MAX_APPROACH_X_SPEED)
else :
sX = constants.MAX_APPROACH_X_SPEED
# Determine the speed to turn to the ball
sTheta = MyMath.clip(ball.bearing*constants.APPROACH_SPIN_GAIN,
-constants.APPROACH_SPIN_SPEED,
constants.APPROACH_SPIN_SPEED)
# Avoid spinning so slowly that we step in place
if fabs(sTheta) < constants.MIN_APPROACH_SPIN_MAGNITUDE:
sTheta = 0.0
# Set our walk towards the ball
if ball.on:
player.setSpeed(sX,0,sTheta)
return player.stay()
def positionForKick(player):
"""
State to align on the ball once we are near it
"""
if player.firstFrame():
player.brain.CoA.setRobotSlowGait(player.brain.motion)
ball = player.brain.ball
player.inKickingState = True
# Leave this state if necessary
if transitions.shouldKick(player):
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goNow('waitBeforeKick')
elif transitions.shouldScanFindBall(player):
player.inKickingState = False
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('scanFindBall')
elif transitions.shouldTurnToBallFromPositionForKick(player):
player.inKickingState = False
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('turnToBall')
elif transitions.shouldApproachFromPositionForKick(player):
player.inKickingState = False
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('approachBall')
# Determine approach speed
targetY = ball.relY
sY = MyMath.clip(targetY * constants.PFK_Y_GAIN,
constants.PFK_MIN_Y_SPEED,
constants.PFK_MAX_Y_SPEED)
sY = max(constants.PFK_MIN_Y_MAGNITUDE,sY) * MyMath.sign(sY)
if transitions.shouldApproachForKick(player):
targetX = (ball.relX -
(constants.BALL_KICK_LEFT_X_CLOSE +
constants.BALL_KICK_LEFT_X_FAR) / 2.0)
sX = MyMath.clip(ball.relX * constants.PFK_X_GAIN,
constants.PFK_MIN_X_SPEED,
constants.PFK_MAX_X_SPEED)
else:
sX = 0.0
if ball.on:
player.setSpeed(sX,sY,0)
return player.stay()
def spinToBall(player):
"""
State to spin to turn to the ball
"""
if player.firstFrame():
player.stopWalking()
player.brain.tracker.trackBall()
ball = player.brain.ball
turnRate = MyMath.clip(ball.locBearing*ChaseConstants.BALL_SPIN_GAIN,
-ChaseConstants.BALL_SPIN_SPEED,
ChaseConstants.BALL_SPIN_SPEED)
if transitions.atSpinBallDir(player):
return player.goLater('atSpinBallPosition')
elif transitions.shouldSpinFindBallPosition(player):
return player.goLater('spinFindBallPosition')
elif player.currentSpinDir != MyMath.sign(turnRate):
player.stopWalking()
player.currentSpinDir = MyMath.sign(turnRate)
elif player.stoppedWalk and ball.on and player.brain.nav.isStopped():
player.setWalk(x=0,y=0,theta=turnRate)
return player.stay()
def turnToBall(player):
"""
Rotate to align with the ball. When we get close, we will approach it
"""
ball = player.brain.ball
if player.firstFrame():
player.hasAlignedOnce = False
player.brain.tracker.trackBall()
player.brain.CoA.setRobotGait(player.brain.motion)
# Determine the speed to turn to the ball
turnRate = MyMath.clip(ball.bearing*constants.BALL_SPIN_GAIN,
-constants.BALL_SPIN_SPEED,
constants.BALL_SPIN_SPEED)
# Avoid spinning so slowly that we step in place
if fabs(turnRate) < constants.MIN_BALL_SPIN_MAGNITUDE:
turnRate = MyMath.sign(turnRate)*constants.MIN_BALL_SPIN_MAGNITUDE
if ball.on:
player.setSpeed(x=0,y=0,theta=turnRate)
if transitions.shouldKick(player):
return player.goNow('waitBeforeKick')
elif transitions.shouldPositionForKick(player):
return player.goNow('positionForKick')
elif transitions.shouldApproachBall(player):
return player.goLater('approachBall')
elif transitions.shouldScanFindBall(player):
return player.goLater('scanFindBall')
return player.stay()
def spinToBall(player):
"""
State to spin to turn to the ball
"""
if player.firstFrame():
player.stopWalking()
player.brain.tracker.trackBall()
ball = player.brain.ball
turnRate = MyMath.clip(ball.locBearing * ChaseConstants.BALL_SPIN_GAIN,
-ChaseConstants.BALL_SPIN_SPEED,
ChaseConstants.BALL_SPIN_SPEED)
if transitions.atSpinBallDir(player):
return player.goLater('atSpinBallPosition')
elif transitions.shouldSpinFindBallPosition(player):
return player.goLater('spinFindBallPosition')
elif player.currentSpinDir != MyMath.sign(turnRate):
player.stopWalking()
player.currentSpinDir = MyMath.sign(turnRate)
elif player.stoppedWalk and ball.on and player.brain.nav.isStopped():
player.setWalk(x=0, y=0, theta=turnRate)
return player.stay()
def pfk_final(nav):
"""
we're done spinning and aligned for y.
approach to final relX with x only.
(may need to accept minor y changes in future)
"""
ball = nav.brain.ball
(x_offset, y_offset, heading) = nav.kick.getPosition()
x_diff = ball.relX - x_offset
# 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)
helper.setSlowSpeed(nav,sX, 0.0, 0.0)
# kicking time!
if sX == 0.0:
return nav.goNow('stop')
return nav.stay()
def turnToBall(player):
"""
Rotate to align with the ball. When we get close, we will approach it
"""
ball = player.brain.ball
if player.firstFrame():
player.hasAlignedOnce = False
player.brain.tracker.trackBall()
player.brain.CoA.setRobotGait(player.brain.motion)
# Determine the speed to turn to the ball
turnRate = MyMath.clip(ball.bearing * constants.BALL_SPIN_GAIN,
-constants.BALL_SPIN_SPEED,
constants.BALL_SPIN_SPEED)
# Avoid spinning so slowly that we step in place
if fabs(turnRate) < constants.MIN_BALL_SPIN_MAGNITUDE:
turnRate = MyMath.sign(turnRate) * constants.MIN_BALL_SPIN_MAGNITUDE
if ball.on:
player.setWalk(x=0, y=0, theta=turnRate)
if transitions.shouldKick(player):
return player.goNow('waitBeforeKick')
elif transitions.shouldPositionForKick(player):
return player.goNow('positionForKick')
elif transitions.shouldApproachBall(player):
return player.goLater('approachBall')
elif transitions.shouldScanFindBall(player):
return player.goLater('scanFindBall')
return player.stay()
def positionForKick(player):
"""
State to align on the ball once we are near it
"""
if player.firstFrame():
player.brain.CoA.setRobotSlowGait(player.brain.motion)
ball = player.brain.ball
player.inKickingState = True
# Leave this state if necessary
if transitions.shouldKick(player):
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goNow('waitBeforeKick')
elif transitions.shouldScanFindBall(player):
player.inKickingState = False
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('scanFindBall')
elif transitions.shouldTurnToBallFromPositionForKick(player):
player.inKickingState = False
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('turnToBall')
elif transitions.shouldApproachFromPositionForKick(player):
player.inKickingState = False
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('approachBall')
# Determine approach speed
targetY = ball.relY
sY = MyMath.clip(targetY * constants.PFK_Y_GAIN, constants.PFK_MIN_Y_SPEED,
constants.PFK_MAX_Y_SPEED)
sY = max(constants.PFK_MIN_Y_MAGNITUDE, sY) * MyMath.sign(sY)
if transitions.shouldApproachForKick(player):
# targetX = (ball.relX -
# (constants.BALL_KICK_LEFT_X_CLOSE +
# constants.BALL_KICK_LEFT_X_FAR) / 2.0)
sX = MyMath.clip(ball.relX * constants.PFK_X_GAIN,
constants.PFK_MIN_X_SPEED, constants.PFK_MAX_X_SPEED)
else:
sX = 0.0
if ball.on:
player.setWalk(sX, sY, 0)
return player.stay()
def pfk_xy(nav):
"""
ball bearing is outside safe limit, we're in danger of losing the ball.
position x,y only
"""
ball = nav.brain.ball
if ball.relX < SAFE_BALL_REL_X and \
ball.dist < SAFE_TO_STRAFE_DIST:
return nav.goNow('pfk_x')
"""
if fabs(ball.bearing) < START_SPIN_BEARING:
print "bearing to ball: %g" % ball.bearing
return nav.goNow('pfk_all')
"""
(x_offset, y_offset, heading) = nav.kick.getPosition()
target_y = ball.relY - y_offset
# arbitrary
if fabs(target_y) < PFK_CLOSE_ENOUGH_XY:
return nav.goNow('pfk_final')
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)
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)
# in position, let's kick the ball!
if (sX == 0.0 and sY == 0.0):
return nav.goNow('stop')
helper.setSlowSpeed(nav,sX,sY,0.0)
return nav.stay()
def getOmniWalkParam(my, dest):
bearing = radians(my.getRelativeBearing(dest))
distToDest = my.dist(dest)
# calculate forward speed
forwardGain = constants.OMNI_GOTO_X_GAIN * distToDest* \
cos(bearing)
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 * distToDest* \
sin(bearing)
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
if atDestinationCloser(my, dest):
sX = sY = 0.0
# calculate spin speed
spinGain = constants.GOTO_SPIN_GAIN
spinDir = my.spinDirToHeading(dest.h)
sTheta = spinDir * fabs(my.h - dest.h) * spinGain
sTheta = MyMath.clip(sTheta,
constants.OMNI_MIN_SPIN_SPEED,
constants.OMNI_MAX_SPIN_SPEED)
if fabs(sTheta) < constants.OMNI_MIN_SPIN_MAGNITUDE:
sTheta = 0.0
if atHeading(my, dest.h):
sTheta = 0.0
return (sX, sY, sTheta)
def rotAlignOnBall(player):
'''Rotate to align with the ball. When we get close, we will approach it '''
turnRate = MyMath.clip(player.brain.ball.bearing*0.5,-10.,10.)
player.printf( "Ball bearing i "+ str(player.brain.ball.bearing)+
" turning at "+str(turnRate))
player.setSpeed(x=0,y=0,theta=turnRate)
if player.brain.ball.on and player.brain.ball.bearing < BALL_BEARING_THRESH :
return player.goNow('approachBall')
if player.brain.ball.framesOff > FRAMES_OFF_THRESH:
return player.goNow('scanFindBall')
return player.stay()
def getSpinOnlyParam(my, dest):
# Determine the speed to turn
bearing = my.getRelativeBearing(dest)
if (fabs(bearing) < 5.0):
sTheta = 0.0
else:
spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST
sTheta = bearing * spinGain
sTheta = MyMath.clip(sTheta,
constants.GOTO_RIGHT_SPIN_SPEED,
constants.GOTO_LEFT_SPIN_SPEED)
sX, sY = 0, 0
return (sX, sY, sTheta)
def getWalkSpinParam(my, dest):
"""
Takes an x, y destinatino and walks towards it. Does not worry
about final heading, only about getting to the x,y.
"""
if hasattr(dest, "bearing"):
bearing = dest.bearing
else:
bearing = my.getRelativeBearing(dest)
if hasattr(dest, "dist"):
dist = dest.dist
else:
dist = my.distTo(dest)
# calculate forward speed
sX = dist * constants.APPROACH_X_WITH_GAIN_DIST
if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE or \
fabs(bearing) > 40.:
sX = 0
else:
sX = MyMath.clip(sX,
constants.OMNI_REV_MAX_SPEED,
constants.OMNI_FWD_MAX_SPEED)
# calculate spin speed
if (fabs(bearing) < 5.0):
sTheta = 0.0
else:
sTheta = bearing * constants.APPROACH_THETA_WITH_GAIN_DIST
sTheta = MyMath.clip(sTheta,
constants.OMNI_MAX_RIGHT_SPIN_SPEED,
constants.OMNI_MAX_LEFT_SPIN_SPEED)
return (sX, 0, sTheta)
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_y(nav, ball, targetY):
targetDist = ball.relY - targetY
if (fabs(targetDist) <= PFK_CLOSE_ENOUGH_Y):
nav.stopY = True
return 0
if (targetDist > 0):
# Move left to match ball with target
# Change the distance to a speed and clip within vector limits
sY = MyMath.clip(targetDist/PFK_MAX_Y_SPEED_DIST,
PFK_MIN_Y_MAGNITUDE,
PFK_LEFT_SPEED)
return sY
else:
# Move right to match ball with target
# Change the distance to a speed and clip within vector limits
sY = MyMath.clip(targetDist/PFK_MAX_Y_SPEED_DIST,
PFK_RIGHT_SPEED,
-PFK_MIN_Y_MAGNITUDE)
return sY
def pfk_x(nav, ball, targetX):
targetDist = ball.relX - targetX
if (fabs(targetDist) <= PFK_CLOSE_ENOUGH_X):
nav.stopX = True
return 0
if (targetDist > 0):
# Move foward to match ball with target
# Change the distance to a speed and clip within vector limits
sX = MyMath.clip(targetDist/PFK_MAX_X_SPEED_DIST,
PFK_MIN_X_MAGNITUDE,
PFK_FWD_SPEED)
return sX
else:
# Move Backwards Slowly
return -.5
def getWalkSpinParam(my, dest):
relX = 0
bearingDeg = my.getRelativeBearing(dest)
distToDest = my.distTo(dest)
if hasattr(dest, "relX"):
relX = dest.relX
else:
relX = MyMath.getRelativeX(distToDest, bearingDeg)
# calculate ideal max forward speed
sX = constants.GOTO_FORWARD_SPEED * MyMath.sign(relX)
if (fabs(bearingDeg) < 2.0):
sTheta = 0.0
else:
# calculate ideal max spin speed
sTheta = (MyMath.sign(bearingDeg) * getRotScale(bearingDeg) *
constants.OMNI_MAX_SPIN_SPEED)
absSTheta = fabs(sTheta)
if fabs(bearingDeg) > 20:
sX = MyMath.clip(sX,
constants.OMNI_MIN_X_SPEED,
constants.OMNI_MAX_X_SPEED)
sTheta = MyMath.sign(sTheta)* constants.OMNI_MAX_SPIN_SPEED
elif fabs(bearingDeg) > 35:
sX = 0
sTheta = constants.MAX_SPIN_SPEED * MyMath.sign(sTheta)
gain = 1.0
if distToDest < ChaseBallConstants.APPROACH_WITH_GAIN_DIST:
gain = constants.GOTO_CLOSE_GAIN
return (sX * gain, 0, sTheta * gain)
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()
