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 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 orbitBeforeKick(player):
"""
State for circling the ball when we're facing our goal.
"""
brain = player.brain
my = brain.my
if player.firstFrame():
player.orbitStartH = my.h
brain.CoA.setRobotGait(brain.motion)
brain.tracker.trackBall()
shotPoint = KickingHelpers.getShotCloseAimPoint(player)
bearingToGoal = my.getRelativeBearing(shotPoint)
spinDir = -MyMath.sign(bearingToGoal)
player.brain.nav.orbitAngle(spinDir * 90)
if not player.brain.tracker.activeLocOn and \
transitions.shouldScanFindBall(player):
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('scanFindBall')
elif brain.ball.dist > constants.STOP_ORBIT_BALL_DIST:
return player.goLater('chase')
if player.brain.nav.isStopped() and not player.firstFrame():
return player.goLater('positionForKick')
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.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 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 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.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 orbitBeforeKick(player):
"""
State for circling the ball when we're facing our goal.
"""
brain = player.brain
my = brain.my
if player.firstFrame():
player.orbitStartH = my.h
brain.CoA.setRobotGait(brain.motion)
brain.tracker.trackBall()
shotPoint = KickingHelpers.getShotCloseAimPoint(player)
bearingToGoal = MyMath.getRelativeBearing(my.x, my.y, my.h,
shotPoint[0],
shotPoint[1] )
spinDir = -MyMath.sign(bearingToGoal)
player.brain.nav.orbitAngle(spinDir * 90)
if not player.brain.tracker.activeLocOn and \
transitions.shouldScanFindBall(player):
player.brain.CoA.setRobotGait(player.brain.motion)
return player.goLater('scanFindBall')
elif brain.ball.dist > constants.STOP_ORBIT_BALL_DIST:
return player.goLater('chase')
if player.brain.nav.isStopped() and not player.firstFrame():
return player.goLater('positionForKick')
return player.stay()
def relocalize(player):
if player.firstFrame():
player.setWalk(constants.RELOC_X_SPEED, 0, 0)
if player.brain.my.locScore != NogginConstants.locScore.BAD_LOC:
player.shouldRelocalizeCounter += 1
if player.shouldRelocalizeCounter > 30:
player.shouldRelocalizeCounter = 0
return player.goLater(player.lastDiffState)
else:
player.shouldRelocalizeCounter = 0
if not player.brain.motion.isHeadActive():
player.brain.tracker.locPans()
if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
direction = MyMath.sign(player.getWalk()[2])
if direction == 0:
direction = 1
player.setWalk(0, 0, constants.RELOC_SPIN_SPEED * direction)
return player.stay()
def penaltyRelocalize(player):
if player.firstFrame():
player.setWalk(1, 0, 0)
if player.brain.ball.on:
player.brain.tracker.trackBall()
return player.goLater('gamePlaying')
if player.brain.my.locScore == NogginConstants.GOOD_LOC or \
player.brain.my.locScore == NogginConstants.OK_LOC:
player.shouldRelocalizeCounter += 1
if player.shouldRelocalizeCounter > 30:
player.shouldRelocalizeCounter = 0
return player.goLater('gamePlaying')
else:
player.shouldRelocalizeCounter = 0
if not player.brain.motion.isHeadActive():
player.brain.tracker.locPans()
if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
direction = MyMath.sign(player.getWalk()[2])
if direction == 0:
direction = 1
player.setWalk(0 , 0, constants.RELOC_SPIN_SPEED * direction)
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 setSpeed(nav, x, y, theta):
"""
Wrapper method to easily change the walk vector of the robot
"""
walk = motion.WalkCommand(x=x,y=y,theta=theta)
nav.brain.motion.setNextWalkCommand(walk)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
def step(nav, x, y, theta, numSteps):
"""
Wrapper method to easily change the walk vector of the robot
"""
steps = motion.StepCommand(x=x,y=y,theta=theta,numSteps=numSteps)
nav.brain.motion.sendStepCommand(steps)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
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 getSpinOnlyParam(my, dest):
# Determine the speed to turn
# see if getRotScale can go faster
headingDiff = my.getRelativeBearing(dest)
if (fabs(headingDiff) < 25.0):
sTheta = 0.0
else:
sTheta = MyMath.sign(headingDiff) * constants.MAX_SPIN_MAGNITUDE * \
getRotScale(headingDiff)
sX, sY = 0, 0
return (sX, sY, sTheta)
def ballInMyBox(player):
if player.firstFrame():
player.brain.tracker.activeLoc()
player.brain.CoA.setRobotGait(player.brain.motion)
ball = player.brain.ball
if fabs(ball.bearing) > constants.BALL_APPROACH_BEARING_THRESH:
player.setWalk(0, 0,
constants.BALL_SPIN_SPEED * MyMath.sign(ball.bearing))
elif fabs(ball.bearing) < constants.BALL_APPROACH_BEARING_OFF_THRESH:
player.stopWalking()
if not player.ballInMyGoalBox():
return player.goLater('chase')
return player.stay()
def ballInMyBox(player):
if player.firstFrame():
player.brain.tracker.activeLoc()
player.brain.CoA.setRobotGait(player.brain.motion)
ball = player.brain.ball
if fabs(ball.bearing) > constants.BALL_APPROACH_BEARING_THRESH:
player.setSpeed(0, 0, constants.BALL_SPIN_SPEED *
MyMath.sign(ball.bearing) )
elif fabs(ball.bearing) < constants.BALL_APPROACH_BEARING_OFF_THRESH :
player.stopWalking()
if not player.ballInMyGoalBox():
return player.goLater('chase')
return player.stay()
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 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 setDribbleSpeed(nav, x, y, theta):
"""
Wrapper to set walk vector while using dribble gait
"""
if x < BACKWARDS_GAIT_THRESH:
nav.brain.CoA.setRobotBackwardsGait(nav.brain.motion)
else:
nav.brain.CoA.setDribbleGait(nav.brain.motion)
x_cms, y_cms, theta_degs = convertWalkVector(nav.brain, x, y, theta)
createAndSendWalkVector(nav, x_cms, y_cms, theta_degs)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
def step(nav, x, y, theta, numSteps):
"""
Wrapper method to easily change the walk vector of the robot
"""
if x < BACKWARDS_GAIT_THRESH:
nav.brain.CoA.setRobotBackwardsGait(nav.brain.motion)
else:
nav.brain.CoA.setRobotSlowGait(nav.brain.motion)
x_cms, y_cms, theta_degs = convertWalkVector(nav.brain, x, y, theta)
createAndSendStepsVector(nav, x_cms, y_cms, theta_degs)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
def setSlowSpeed(nav, x, y, theta):
"""
Wrapper to set walk vector while using slow gait
TODO: dynamic gait so this is unnecessary
"""
if x < BACKWARDS_GAIT_THRESH:
nav.brain.CoA.setRobotBackwardsGait(nav.brain.motion)
else:
nav.brain.CoA.setRobotSlowGait(nav.brain.motion)
x_cms, y_cms, theta_degs = convertWalkVector(nav.brain, x, y, theta)
createAndSendWalkVector(nav, x_cms, y_cms, theta_degs)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
def setSpeed(nav, x, y, theta):
"""
Wrapper method to easily change the walk vector of the robot
"""
# use backwards gait if appropriate
if x < BACKWARDS_GAIT_THRESH:
nav.brain.CoA.setRobotBackwardsGait(nav.brain.motion)
else:
nav.brain.CoA.setRobotGait(nav.brain.motion)
x_cms, y_cms, theta_degs = convertWalkVector(nav.brain, x, y, theta)
createAndSendWalkVector(nav, x_cms, y_cms, theta_degs)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
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 ballInMyBox(nav):
if nav.firstFrame():
nav.brain.tracker.activeLoc()
ball = nav.brain.ball
if fabs(ball.bearing) > constants.BALL_APPROACH_BEARING_THRESH:
nav.setWalk(0, 0,
constants.BALL_SPIN_SPEED * MyMath.sign(ball.bearing) )
elif fabs(ball.bearing) < constants.BALL_APPROACH_BEARING_OFF_THRESH :
nav.stopWalking()
if not nav.ball.inMyGoalBox():
return nav.goLater('chase')
return nav.stay()
def relocalize(player):
if player.firstFrame():
pass #player.stopWalking()
if player.brain.my.locScore == NogginConstants.GOOD_LOC or \
player.brain.my.locScore == NogginConstants.OK_LOC:
player.shouldRelocalizeCounter += 1
if player.shouldRelocalizeCounter > 15:
player.shouldRelocalizeCounter = 0
return player.goLater(player.lastDiffState)
else:
player.shouldRelocalizeCounter = 0
if not player.brain.motion.isHeadActive():
player.brain.tracker.locPans()
direction = MyMath.sign(player.getWalk()[2])
if direction == 0:
direction = 1
if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
player.setWalk(0, 0, constants.RELOC_SPIN_SPEED * direction)
return player.stay()
def setSpeed(nav, x, y, theta):
"""
Wrapper method to easily change the walk vector of the robot
"""
#print (theta**2 + x**2 + y**2)
# use backwards gait if appropriate
if x < BACKWARDS_GAIT_THRESH:
nav.brain.CoA.setRobotBackwardsGait(nav.brain.motion)
elif fabs(theta) > SPIN_GAIT_THRESH and \
fabs(x) < 0.1 and fabs(y) < 0.15 and \
fabs(theta) > fabs(x) and \
(theta**2 + x**2 + y**2) > 0.1:
nav.brain.CoA.setRobotSlowGait(nav.brain.motion)
else:
nav.brain.CoA.setRobotGait(nav.brain.motion)
x_cms, y_cms, theta_degs = convertWalkVector(nav.brain, x, y, theta)
createAndSendWalkVector(nav, x_cms, y_cms, theta_degs)
nav.walkX, nav.walkY, nav.walkTheta = x, y, theta
nav.curSpinDir = MyMath.sign(theta)
def penaltyRelocalize(player):
"""
Note: This is the old code that I'm using as a back-up in case we can't
see any goal posts. It may be possible to make this smarter. -Wils
"""
gcState = player.brain.gameController.currentState
if player.firstFrame():
player.setWalk(1, 0, 0)
if player.brain.ball.framesOn >= OBJ_SEEN_THRESH:
player.brain.tracker.trackBall()
return player.goLater(gcState)
if player.brain.my.locScore == NogginConstants.GOOD_LOC or \
player.brain.my.locScore == NogginConstants.OK_LOC:
player.shouldRelocalizeCounter += 1
if player.shouldRelocalizeCounter > 30:
player.shouldRelocalizeCounter = 0
return player.goLater(gcState)
else:
player.shouldRelocalizeCounter = 0
if not player.brain.motion.isHeadActive():
player.brain.tracker.locPans()
if player.counter > constants.RELOC_SPIN_FRAME_THRESH:
direction = MyMath.sign(player.getWalk()[2])
if direction == 0:
direction = 1
player.setWalk(0 , 0, constants.RELOC_SPIN_SPEED * direction)
return player.stay()
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()
