def DecideNextAction():
global doKickOff
resetEachFrame()
rSetPlay.resetPerform()
Global.firstPlayFrame = Global.frame + 1
#import Constant
#print rSetPlay.getGapTo(Constant.MAX_GOALBOX_EDGE_X, Constant.TOP_GOALBOX_EDGE_Y)
#print rSetPlay.getObstaclesToGoal()
# Receiving team look at the ball
# FIXME: look to front for obstacles too
if VisionLink.getKickOffTeam() != VisionLink.getTeamColor():
if pReady.isCentre(Global.kickOffPos):
hTrack.stationaryLocalise()
else:
hFWHead.DecideNextAction()
# Kicker looks at ball (no localise). Others scan to see obstacles
elif pReady.isCentre(Global.kickOffPos):
sFindBall.perform(True)
elif pReady.isForward(Global.kickOffPos):
hTrack.stationaryLocalise(4, 30, -30)
else:
hTrack.stationaryLocalise()
Indicator.showBatteryStatus()
doKickOff = True
def receiveKickOffPincer():
global playCounter
if playCounter > 85:
return False
if pReady.isCentre(Global.kickOffPos):
if playCounter < 65:
Global.myRole = Global.myLastRole = Constant.DEFENDER
if sBlock.checkThenBlock(True, 3.0, bothSides=True):
return
sFindBall.perform(True)
hTrack.saGoToTargetFacingHeading(Constant.FIELD_WIDTH / 2,
Constant.FIELD_LENGTH * 0.29,
90,
maxSpeed=20)
#rDefender.DecideNextAction()
#Action.walk(0.01, 0, 0) # don't go anywhere
return True
return False
# Wide players
hFWHead.DecideNextAction()
sFindBall.walkToBall(Global.ballD, Global.ballH)
return True
def performWithoutGrab(dkd):
global gKickCounter
global gIsKickTriggering
if (gUseHeadLeft and isHeadLeftOk())\
or (not gUseHeadLeft and isHeadRightOk()):
gIsKickTriggering = True
if gIsKickTriggering:
if gKickCounter < 15:
if gUseHeadLeft:
Action.kick(Action.HeadLeftWT)
else:
Action.kick(Action.HeadRightWT)
else:
resetPerform()
return Constant.STATE_SUCCESS
gKickCounter += 1
return Constant.STATE_EXECUTING
if gTargetAngle == None:
setGetBehind(dkd)
if gIsGetBehindNeeded:
sGetBehindBall.perform(gTargetAngle)
else:
sFindBall.perform()
gIsKickTriggering = False
gKickCounter = 0
return Constant.STATE_EXECUTING
def performWithGrab():
global gKickCounter
if gKickCounter < GRAB_PAUSE_TIME:
Action.walk(Action.MAX_FORWARD,
0,
0,
walkType=Action.NormalWalkWT,
minorWalkType=Action.GrabTurnOnlyMWT)
elif gKickCounter == GRAB_PAUSE_TIME:
if gUseUpennLeft:
Action.kick(Action.UpennLeftWT)
else:
Action.kick(Action.UpennRightWT)
elif Action.shouldIContinueKick():
sFindBall.perform(True)
Action.continueKick()
return Constant.STATE_EXECUTING
elif gKickCounter < UPENN_DURATION:
sFindBall.perform(True)
Action.continueKick()
else:
resetPerform()
return Constant.STATE_SUCCESS
gKickCounter += 1
return Constant.STATE_EXECUTING
def DecideNextAction():
Indicator.setDefault()
if Global.lostBall < Constant.LOST_BALL_GPS\
and perform() == Constant.STATE_EXECUTING:
Indicator.showRedEye()
else:
sFindBall.perform()
def doTrackBall(headOnly=False):
if Global.lightingChallenge:
global lastTrackBall, startTrackBall
if Global.frame - lastTrackBall > 1:
startTrackBall = Global.frame
lastTrackBall = Global.frame
sFindBall.perform(headOnly)
def DecideNextAction():
isNeeded, leftAngle, rightAngle = isStealthDogNeededForBall()
Indicator.setDefault()
if isNeeded:
print "sStealthDog : leftAngle = ", leftAngle, " rightAngle = ", rightAngle
performToBall(leftAngle,rightAngle)
else:
if Global.ballD < 25:
resetPerform()
sFindBall.perform()
def DecideNextAction():
import sFindBall
import hFWHead
Indicator.setDefault()
hFWHead.resetEachFrame()
if Global.lostBall < 30:
hFWHead.DecideNextAction()
if perform():
Indicator.showFacePattern([5,3,3,3,5])
else:
sFindBall.perform()
def DecideNextAction():
global frameSinceLastGrabKick
if sGrab.isGrabbed:
if perform() != Constant.STATE_EXECUTING:
sGrab.resetPerform()
frameSinceLastGrabKick = Global.frame
resetPerform()
elif Global.frame - frameSinceLastGrabKick > 60:
sGrab.perform()
else:
sFindBall.perform()
def DecideNextAction():
Indicator.setDefault()
if Global.lostBall < Constant.LOST_BALL_GPS:
r = performBall()
sFindBall.perform(True)
if r == Constant.STATE_SUCCESS:
Indicator.showFacePattern([5, 3, 3, 3, 5])
resetPerform()
Action.stopLegs()
else:
resetPerform()
sFindBall.perform()
def doGotoCenter():
global lastGoToCenter, startGoToCenter
if Global.frame - lastGoToCenter > 1:
startGoToCenter = Global.frame
lastGoToCenter = Global.frame
movePoint = (centerPos[0], centerPos[1])
distanceToPoint = hMath.getDistanceBetween(movePoint[0], movePoint[1],
pos[0], pos[1])
if (distanceToPoint < POS_ACCURACY):
sFindBall.perform()
else:
hTrack.stationaryLocalise()
hTrack.saGoToTarget(movePoint[0], movePoint[1])
def DecideNextAction():
global gCounter
import Indicator
Indicator.setDefault()
if sGrab.isGrabbed:
if perform(90, True) == Constant.STATE_SUCCESS:
gCounter = 0
elif gCounter < 50:
gCounter += 1
sFindBall.perform()
else:
sGrab.perform()
def perform(paw=AUTO_PAW_KICK):
global pawKickType
if Global.lostBall > Constant.LOST_BALL_GPS:
return Constant.STATE_FAILED
# Head ball tracking movement
sFindBall.perform(True)
PAW_KICK_Y_OFFSET = Constant.BallRadius
PAW_KICK_X_OFFSET = 7.0
ballX = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD
ballY = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD
# The 2003 offset is 6 and it is not too bad.
if paw == AUTO_PAW_KICK:
if pawKickType == LEFT_PAW_KICK and ballX < -PAW_KICK_X_OFFSET / 2:
pawKickType = RIGHT_PAW_KICK
elif pawKickType == RIGHT_PAW_KICK and ballX > PAW_KICK_X_OFFSET / 2:
pawKickType = LEFT_PAW_KICK
else:
pawKickType = paw
if pawKickType == RIGHT_PAW_KICK:
targetX = ballX + PAW_KICK_X_OFFSET
else:
targetX = ballX - PAW_KICK_X_OFFSET
targetY = ballY - PAW_KICK_Y_OFFSET
targetD = math.sqrt(hMath.SQUARE(targetX) + hMath.SQUARE(targetY))
targetH = hMath.RAD2DEG(math.asin(targetX / targetD))
# Once the ball is lined up and well within range for the forward, the
# robot should commit to charge at the ball at full speed.
if abs(targetX) <= 1.0 and targetD < Constant.BallRadius * 1.5:
turnccw = hMath.CLIP(targetH, 10)
Action.walk(Action.MAX_FORWARD,
0,
turnccw,
minorWalkType=Action.SkeFastForwardMWT)
else:
sFindBall.walkToBall(targetD,
targetH,
getBehind=sFindBall.GET_BEHIND_LOTS)
return Constant.STATE_EXECUTING
def doKick():
global gSelectedKick
global gIsKickTriggering
global gLastKickedFrame
# After you released the ball, don't grab for a second.
if Global.frame - gLastKickedFrame < 75:
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_LOTS)
return
kickType = gSelectedKick[0]
# grab actions
if sSelKick.doINeedToGrab(kickType)\
or sGrab.isGrabbed:
r = doKickWithGrab()
# non grab actions
else:
r = doKickWithoutGrab()
# When return value from skill is not executing,
# Do some resetting business
if r != Constant.STATE_EXECUTING:
# Resetting grab and all other kick modules
sGrab.resetPerform()
doKickReset()
if kickType != sSelKick.KT_AVOID_OWN_GOAL\
and r == Constant.STATE_SUCCESS:
gLastKickedFrame = Global.frame
gIsKickTriggering = False
gSelectedKick = None
if kickType == sSelKick.KT_GRAB_DRIBBLE:
pass
elif kickType == sSelKick.KT_DRIBBLE\
or kickType == sSelKick.KT_PAW_KICK:
hTrack.panLow = True
else:
hTrack.panLow = False
doTrackBall()
showKickTypeIndicator(kickType)
def ballTrack():
global headMode, localiseTimer
global ballSeenCounter, ballSearchTimer
if goalieHeadDebug:
print "ballTrack():",
sFindBall.perform(True)
# Occasionally localise
if localiseTimer <= 500:
localiseTimer += 1
if shouldLocalise():
#headMode = goalBoxLocalise
headMode = localise
localiseTimer = 0
return False
def TrackOrFindBall():
global ShouldLocateBall, ShouldLocateWirelessBall
debug = False
lostBall = Global.lostBall
ShouldLocateBall = False
ShouldLocateWirelessBall = False
myx, myy = Global.selfLoc.getPos()
wirelessBallDist = HelpMath.getDistanceBetween(myx, myy,
Global.sharedBallX,
Global.sharedBallY)
if debug:
print "sharedBallVar: %.2f , %.2f, %.2f" % (
Global.sharedBallVar, Global.sharedBallX, Global.sharedBallY)
if lostBall < 5:
FWHead.compulsoryAction = FWHead.mustSeeBall
if Debug.headDebug:
print "Track visual ball < 5"
elif (lostBall <= Constant.VBALL_LOST
and abs(Global.gpsLocalBall.getHeading()) < 90):
FWHead.compulsoryAction = FWHead.mustSeeGpsBall
if Debug.headDebug:
print "GPS ball in attacker!"
elif (lostBall <= Constant.VBALL_LOST):
FWHead.compulsoryAction = FWHead.mustSeeBall
if Debug.headDebug:
print "track visual ball >= 5"
elif (Global.sharedBallVar < Constant.LARGE_VAL and wirelessBallDist > 80):
ShouldLocateWirelessBall = True
if debug:
print "wirelessBallDist: %.2f" % (wirelessBallDist)
print "going to wireless ball at %.2f, %.2f" % (Global.sharedBallX,
Global.sharedBallY)
FWHead.compulsoryAction = FWHead.fixHead
LocateBall.headFindBall(-50, -5)
HelpTrack.saGoToTarget(Global.sharedBallX, Global.sharedBallY)
if Debug.headDebug:
print "Wireless ball in attacker!"
else: # the case when lostBall > Constant.VBALL_LOST
ShouldLocateBall = True
if sFindBall.perform() == sConstant.STATE_SUCCESS:
sFindBall.resetPerform()
setLocaliseTimer(0, 2)
if Debug.headDebug:
print "Locate ball in attacker!"
def DecideNextAction():
global grabCounter
grabCounter -= 1
#testKick()
#return
Action.openMouth()
sGrab.moveHeadForward()
Action.walk(0, 0, 0)
obsFront = VisionLink.getNoObstacleInBox(-25, 100, 25, 20,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
obsLeft = VisionLink.getNoObstacleInBox(-80, 80, -40, -80,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
obsRight = VisionLink.getNoObstacleInBox(40, 80, 80, -80,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
print "Obstacle Front : ", obsFront, " Left : ", obsLeft, " Right: ", obsRight
return
#print "Camera Frame : ", Global.cameraFrame
Indicator.setDefault()
if not sGrab.isGrabbed or performToTargetGoal(
) != Constant.STATE_EXECUTING:
if grabCounter == 60:
hTrack.panLow = True
resetPerform()
sGrab.resetPerform()
if grabCounter > 0:
sFindBall.perform()
else:
sGrab.perform()
grabCounter -= 1
else:
grabCounter = 60
def doActivelyLocalise():
global gLastActivelyLocalisedFrame
global gActiveLocaliseTimer
gLastActivelyLocalisedFrame = Global.frame
r = sActiveLocalise.perform()
if r == Constant.STATE_FAILED:
gActiveLocaliseTimer = 0
sFindBall.perform()
return
turnCCW = 0
if Global.vBall.isVisible() and abs(Global.ballH) < 20:
turnCCW = Global.ballH
Action.walk(Action.MAX_FORWARD,
0,
turnCCW,
minorWalkType=Action.SkeFastForwardMWT)
Global.lostBall = 0
sFindBall.setForce(sFindBall.FORCE_LAST_VISUAL)
def perform():
global gLastFrameCalled
global gDirection
global gNoGetBehindCounter
if gLastFrameCalled != Global.frame - 1:
resetPerform()
gLastFrameCalled = Global.frame
gNoGetBehindCounter = max(gNoGetBehindCounter - 1, 0)
selfLoc = Global.selfLoc
selfX, selfY = selfLoc.getPos()
selfH = selfLoc.getHeading()
ballX, ballY = Global.ballX, Global.ballY
ballD = Global.ballD
ballH = Global.ballH
selfH2BallH = selfH + ballH
ballH2OGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y)
selfH2OGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2OGoalH)
selfH2SafeH = hMath.normalizeAngle_180(selfH2OGoalH + 180)
if abs(selfH2OGoalH) > 110:
resetPerform()
return Constant.STATE_SUCCESS
# If we are far away from the ball, then just use our implicit get behind.
#if Global.ballD > 60:
# sFindBall.perform(doGetBehind = sFindBall.GET_BEHIND_LOTS)
# # Reset the direction.
# gDirection = None
# return Constant.STATE_EXECUTING
# First time to enter the function, so choose the direction.
if gDirection == None:
# go left
if selfH2OGoalH < 0:
gDirection = Constant.dANTICLOCKWISE
# go right
else:
gDirection = Constant.dCLOCKWISE
if gNoGetBehindCounter > 0:
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY)
else:
sFindBall.perform(True)
r = sGetBehindBall.performBall(dist=25,
direction=gDirection,
accuracy=10)
if r == Constant.STATE_SUCCESS:
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY)
gNoGetBehindCounter = NO_GET_BEHIND_DURATION
return Constant.STATE_EXECUTING
def kickOffUPenn():
global playCounter, whenGrabbed
if pReady.chosenFormation == 0:
kickLeft = False
else:
kickLeft = True
if pReady.isCentre(Global.kickOffPos):
if playCounter > 150:
return False
if whenGrabbed == 0:
print "grabbing"
if sGrab.perform() == Constant.STATE_FAILED:
return False
if sGrab.isGrabbed:
whenGrabbed = playCounter
return True
print "Grabbed!"
if sUpennKick.perform(kickLeft) == Constant.STATE_SUCCESS:
sGrab.resetPerform()
return False
sFindBall.perform(True)
return True
elif pReady.isForward(Global.kickOffPos):
if playCounter > 40:
return False
hTrack.saGoToTarget(Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y)
hFWHead.DecideNextAction()
else:
return False
return True
def performWithoutGrab(dkd):
global gKickCounter
global gIsKickTriggering
id(dkd)
if (gUseUpennLeft and isUpennLeftOk())\
or (not gUseUpennLeft and isUpennRightOk()):
if gUseUpennLeft:
Action.kick(Action.UpennLeftWT)
else:
Action.kick(Action.UpennRightWT)
sFindBall.perform(True)
gIsKickTriggering = True
elif gIsKickTriggering:
if Action.shouldIContinueKick():
sFindBall.perform(True)
Action.continueKick()
elif gKickCounter < UPENN_DURATION:
sFindBall.perform(True)
Action.continueKick()
gKickCounter += 1
else:
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY)
resetPerform()
return Constant.STATE_SUCCESS
else:
sFindBall.perform()
gIsKickTriggering = False
gKickCounter = 0
return Constant.STATE_EXECUTING
def dodgyDogToBall():
global gNewHeading
if Global.lostBall > Constant.LOST_BALL_GPS:
if Debug.dodgyDebug:
print "dodgyDogToBall: cannot see ball, searching"
gNewHeading = True
return sFindBall.perform()
if Global.vBall.isVisible():
ball = Global.vBall.getPos()
#headAction = hFWHead.mustSeeBall
else:
ball = Global.gpsGlobalBall.getPos()
#headAction = hFWHead.mustSeeGpsBall
rtn = dodgyDogTo(ball[0], ball[1])
#look at ball
#hFWHead.compulsoryAction = headAction
#hFWHead.DecideNextAction()
return rtn
def performTimeElapsedAction(selType):
global gLastActionFrame
global gForceTimeElapse
global gKickType
global gLastKickFrame
global gLastBreakFrame
global gLastLocaliseFrame
global gKickCounter
global gBreakCounter
global gLocaliseCounter
gLastActionFrame = Global.frame
gForceTimeElapse = True
#print "performTimeElapsed(", selType, ")"
#if selType == SEL_OFFENSIVE:
# import traceback
# traceback.print_stack()
if Action.shouldIContinueKick():
#print "Waiting for kick to activate ...", gKickCounter
Action.continueKick()
moveHeadForwardTight()
elif gKickCounter > 0:
#print "Kicking ...", gKickCounter
if gKickType == AVOID_OWN_GOAL:
if gKickCounter > 10:
perform(0, 0, Action.MAX_TURN)
elif gKickCounter > 8:
perform(0, 0, 0)
else:
perform(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeGrabDribbleMWT)
elif gKickType == GAP_KICK:
selfH = Global.selfLoc.getHeading()
if abs(gKickHead - selfH) > 10:
turnCCW = gKickHead - selfH
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.SoftTapWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == SIDE_STEP_LEFT_SHOOT\
or gKickType == SIDE_STEP_RIGHT_SHOOT\
or gKickType == FWD_STEP_SHOOT:
#print "gKickType = SIDESTEP_X_SHOOT"
# Do side step shoot...
if gKickCounter > FWD_STEP_SHOOT_DURATION:
left = Action.MAX_LEFT
if gKickType == SIDE_STEP_RIGHT_SHOOT:
left = -Action.MAX_LEFT
turnCCW = 0
# FIXME: Sidestep to Gap
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 30)
perform(5, left, turnCCW)
# Do forward step shoot...
elif gKickCounter > NU_FWD_DURATION:
fwd = Action.MAX_FORWARD
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 15)
perform(fwd,
0,
turnCCW,
minorWalkType=Action.SkeFastForwardMWT)
# Do kick!
else:
gKickType = gNextKickType
print "after sidestep setting kick type to", gNextKickType
#perform(0,0,0)
Action.kick(gKickType)
moveHeadForwardTight()
elif gKickType == RELEASE:
#print "gKickType = RELEASE", gKickCounter
Action.walk(0, 0, 0, minorWalkType=Action.SkeGrabDribbleHoldMWT)
if gKickCounter > RELEASE_DURATION - 15:
#print "stopping..."
Action.openMouth()
sGrab.moveHeadForward()
elif gKickCounter > 15:
#print "scanning"
hTrack.stationaryLocalise(speed=6)
Action.closeMouth()
else:
#print "regrabbing"
sGrab.moveHeadForward()
Action.openMouth()
# Reset grab timers
sGrab.gLastGrabTime = VisionLink.getCurrentTime()
sGrab.gGrabCount = 0
gForceTimeElapse = False
else:
#print "gKickCounter =", gKickCounter, "- continuing kick"
# Reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk
# straight.
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 160)
VisionLink.resetBall(x, y, 0, 0)
Action.continueKick()
Action.openMouth()
hTrack.panLow = True
gKickCounter -= 1
gLastKickFrame = Global.frame
if gKickType != RELEASE:
sFindBall.setForce(sFindBall.FORCE_FOLLOW)
elif gBreakCounter > 0:
#print "gBreakCounter > 0"
#if Global.penaltyShot or Global.lightingChallenge:
# # Stop and release
# gBreakCounter = 0
# gKickType = RELEASE
# gKickCounter = RELEASE_DURATION
# If we cannot see the ball, then reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
if not Global.vBall.isVisible():
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 50)
VisionLink.resetBall(x, y, 0, 0)
if gBreakCounter > 10:
Action.setHeadParams(0, 0, 0, Action.HTAbs_h)
Action.walk(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeFastForwardMWT)
elif gBreakCounter > 6:
Global.lostBall = Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(True)
rate = Action.SKE_FF_PG * 8.0 * 2.0 / 1000.0
fwd = Action.MAX_SKE_FF_FWD_STP * 0.7 / rate
Action.walk(fwd, 0, 0, minorWalkType=Action.SkeFastForwardMWT)
Action.openMouth()
else:
Global.lostBall = 8 - gBreakCounter + Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY)
Action.openMouth()
hTrack.panLow = True
gBreakCounter -= 1
gLastBreakFrame = Global.frame
elif Global.frame - gLastKickFrame > 1\
and Global.frame - gLastBreakFrame > 1:
#print "gKickCounter = 0, Selecting kick", selType
selectKick(selType)
else:
resetPerform()
sGrab.resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def approachBall(sGrabDoGetBehind=None):
global gLastApproachFrame
global gLastReallyCloseFrame
global gLastTurnFrame
resetPerform()
if sGrabDoGetBehind == None:
sGrabDoGetBehind = sFindBall.GET_BEHIND_DEFAULT
# Setting default action values with sFindBall.
# Action values are overwritten by the approach ball code below.
sFindBall.perform(doGetBehind=sGrabDoGetBehind)
if Global.ballD < TIME_CRITICAL_DIST and Global.lostBall <= 3:
VisionLink.setTimeCritical()
else:
VisionLink.clearTimeCritical()
if Global.lostBall < Constant.LOST_BALL_GPS and Global.ballD < CLOSE_DIST:
gLastApproachFrame = Global.frame
if Global.frame - gLastApproachFrame < 5:
VisionLink.setGrabbing()
# Try to step just the right amount, but limit the minimum step
# length. Reduce max to shorten step time and improve reaction time
#vel = VisionLink.getGPSBallVInfo(Constant.CTLocal)
#velX, velY = vel[0], vel[1]
rate = Action.SKE_FF_PG * 2.0 * 8.0 / 1000.0
fwd = Global.ballD - BALL_AGAINST_CHEST_DIST
fwd = hMath.EXTEND(fwd, Action.MAX_SKE_FF_FWD_STP * 0.7)
# don't walk backward
if fwd < 0:
fwd = 0
turnCCW = Global.ballH
left = Global.ballD * math.sin(hMath.DEG2RAD(Global.ballH))
#if left > 0:
# left = hMath.CLIP(left,Action.MAX_SKE_FF_LEFT_STP * 0.8)
#else:
# left = hMath.CLIP(left,Action.MAX_SKE_FF_RIGHT_STP * 0.8)
if Global.ballD < REALLY_CLOSE_DIST:
gLastReallyCloseFrame = Global.frame
mwt = Action.SkeNeckTurnWalkMWT
if Global.frame - gLastReallyCloseFrame < 5:
turnCCW = hMath.CLIP(turnCCW, 30)
if abs(left) < 2:
#print "branch 1"
fwd = fwd / rate
left = left / rate
#turnCCW = hMath.CLIP(turnCCW * 0.4,30) / rate
Action.walk(fwd, left, turnCCW, "sss", minorWalkType=mwt)
elif abs(left) < 8:
#print "branch 2"
left = left * 1.2 / rate
#turnCCW = hMath.CLIP(turnCCW * 0.4,30) / rate
Action.walk(0, left, 0, "sss", minorWalkType=mwt)
else:
#print "branch 3"
#turnCCW = hMath.CLIP(turnCCW * 0.5,30) / rate
turnCCW = turnCCW * 0.7
Action.walk(0, 0, turnCCW, minorWalkType=mwt)
gLastTurnFrame = Global.frame
else:
if abs(turnCCW) < 30:
#print "branch 5"
#left = left * 0.6
turnCCW = turnCCW * 1.1
Action.walk(fwd, 0, turnCCW, "ddd", minorWalkType=mwt)
else:
if Global.ballD < 30:
#print "branch 6"
turnCCW = turnCCW * 0.8
Action.walk(0, 0, turnCCW, minorWalkType=mwt)
else:
Action.walk(fwd, 0, turnCCW, minorWalkType=mwt)
gLastTurnFrame = Global.frame
# Use the hacked version of HTAbs_xyz
if Action.finalValues[Action.HeadType] == Action.HTAbs_xyz:
Action.finalValues[Action.HeadType] = Action.HTAbs_xyz_hack
# Force high gain!!
Global.forceHighGain = True
# setting hTrack.panLow variable to true.
hTrack.panLow = True
def DecideNextAction():
global pauseCounter
global fired1, fired2
"""
sFindBall.perform(True)
if Action.finalValues[Action.HeadType] == Action.HTAbs_xyz:
Action.finalValues[Action.HeadType] = Action.HTAbs_xyz_hack
Action.walk(0,0,0,minorWalkType=Action.SkeFastForwardMWT)
print "heading : ", Global.ballH
print "weightedVisBallHead : ", Global.weightedVisBallHead
ballX = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD
ballY = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD
print "ballX ", ballX
print "ballY ", ballY
Action.walk(15,0,0,minorWalkType=Action.SkeFastForwardMWT)
return
pauseCounter += 1
if pauseCounter < 300:
return
elif pauseCounter < 350:
global grabbingCount
if pauseCounter == 300:
grabbingCount = 1
perform()
else:
pauseCounter = 0
resetPerform()
return
"""
if pauseCounter > 0:
if pauseCounter > 60:
if perform() == Constant.STATE_FAILED:
resetPerform()
pauseCounter = 61
else:
if pauseCounter == 60:
# print ""
# print "stats!!!!!"
# print "fired1 : ", fired1, ", failed1 : ", failed1, ", success rate : ", ((fired1 - failed1 + 0.0) / (fired1 + 0.000001))
# print "fired2 : ", fired2, ", failed2 : ", failed2, ", success rate : ", ((fired2 - failed2 + 0.0) / (fired2 + 0.000001))
# print "last fired type : ", firedType
# print ""
resetPerform()
sFindBall.perform()
pauseCounter -= 1
return
r = perform()
if r == Constant.STATE_SUCCESS:
pauseCounter = 120
elif r == Constant.STATE_FAILED:
if firedType == 1:
fired1 -= 1
elif firedType == 2:
fired2 -= 1
def perform(params=None):
global gGetOutOfTheBallCounter
global gGetOutOfTheAttackerCounter
global gGetOutOfTheCircleCounter
global gMateLoc
global gTargetPointReqAccuracy
id(params) # ignore
resetEachFrame()
# Back of if stuck
if hStuck.amIStuckForward():
hFWHead.DecideNextAction()
return
hFWHead.DecideNextAction()
# If the attacker has grabbed, get out of the way
# if hTeam.hasTeammateGrabbedBall():
# if getOutOfEveryonesWay():
# return
# ---------------------------------------------------------------
# Check if I need to get out of the way for the attacker first.
targetH = hMath.normalizeAngle_0_360(Global.selfLoc.getHeading() +
Global.ballH)
if gGetOutOfTheBallCounter > 0:
r = sGetOutOfTheWay.perform(Global.ballX,Global.ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y,\
targetH,True)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheBallCounter -= 1
Indicator.showFacePattern([0, 0, 2, 0, 0])
return
gGetOutOfTheBallCounter = 0
if gGetOutOfTheAttackerCounter > 0:
r = sGetOutOfTheWay.perform(Global.ballX,Global.ballY,\
gMateLoc.getX(),gMateLoc.getY(),\
targetH,True)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheAttackerCounter -= 1
Indicator.showFacePattern([0, 0, 2, 0, 0])
return
gGetOutOfTheAttackerCounter = 0
if gGetOutOfTheCircleCounter > 0:
r = sGetOutOfTheWay.getOutOfTheCircle(Global.ballX, Global.ballY,
targetH, True)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheCircleCounter -= 1
Indicator.showFacePattern([0, 2, 0, 2, 0])
return
gGetOutOfTheCircleCounter = 0
for i in Global.otherValidForwards:
mate = Global.teamPlayers[i]
if mate.isAttacker()\
and mate.getTimeToReachBall() < 3000:
r = sGetOutOfTheWay.perform(Global.ballX,Global.ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y,\
targetH)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheBallCounter = GET_OUT_DURATION
Indicator.showFacePattern([0, 0, 2, 0, 0])
return
mateLoc = Global.teammatesLoc[i]
gMateLoc = mateLoc.getCopy()
r = sGetOutOfTheWay.perform(Global.ballX,Global.ballY,\
mateLoc.getX(),mateLoc.getY(),\
targetH)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheAttackerCounter = GET_OUT_DURATION
Indicator.showFacePattern([0, 0, 2, 0, 0])
return
r = sGetOutOfTheWay.getOutOfTheCircle(Global.ballX, Global.ballY,
targetH)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheCircleCounter = GET_OUT_DURATION
Indicator.showFacePattern([0, 2, 0, 2, 0])
return
# ---------------------------------------------------------------
# trigger BOP if i was doing it.
if sBirdOfPrey.isBirdOfPreyTriggering():
# FIXME: Winger currently not in use so birding not updated
if sBirdOfPrey.perform(Global.ballX,
Global.ballY) == Constant.STATE_EXECUTING:
return
# trigger BOP when I am certain distance behind from the ball
DEFENCE_OFFSET = 10.0
DEFENCE_ANGLE = 150.0
if sBirdOfPrey.areWeAboveTheLine(DEFENCE_OFFSET,DEFENCE_ANGLE,True, \
Global.ballX, Global.ballY):
sBirdOfPrey.perform(Global.ballX, Global.ballY)
return
# If I have lost the ball for a while, then find the ball.
if (Global.ballSource == Constant.GPS_BALL and Global.lostBall > 210)\
or (Global.ballSource == Constant.WIRELESS_BALL and Global.lostBall > 240):
Indicator.showFacePattern([5, 0, 0, 0, 5])
sFindBall.perform()
return
doGetToTargetPoint()
def DecideNextAction():
global focusTotalTim
global focusDuration
global timerSinceLastLocalise
global isLocalising
global minHeadVariance
global minPosVariance
#---------------------------------------------------------------------------
# Special cases.
#---------------------------------------------------------------------------
# Sometime players want to handle the head itself
if compulsoryAction == doNothing:
return
# Sometimes players want to see the ball no matter what.
if compulsoryAction == mustSeeBall:
hTrack.trackVisualBall()
return
# Force to track wireless ball.
if compulsoryAction == mustSeeWirelessBall:
hTrack.trackWirelessBall()
return
# Force to track gps ball.
if compulsoryAction == mustSeeGpsBall:
hTrack.trackGpsBall()
return
# Force a beacon localisation - caller will set its own timers.
# JOSH: WARNING: this does not appear to work.
# ALEX: it doesn't work because calling SmartSetBeacon every frame is wrong
#if compulsoryAction == mustLocalise:
# isLocalising = True
# focusDuration = 0
# sActiveLocalise.SmartSetBeacon()
# sActiveLocalise.DecideNextAction()
# return
# Force to stationary localise - head swiping
if compulsoryAction == mustStatLocalise:
hTrack.stationaryLocalise()
return
# Force to track a point given in local coords
if compulsoryAction == mustSeeLocalPoint:
global localPointX, localPointY
if localPointX == 0 and localPointY == 0:
print "hFWHead: mustSeeLocalPoint: local point is (0,0), possibly uninitialised?"
hTrack.trackLocalPoint(localPointX, localPointY)
return
#---------------------------------------------------------------------------
# Default behaviour of the head.
#---------------------------------------------------------------------------
#
# Notice that winger, supporter etc are using the default behaviour.
headColor(Indicator.RGB_NONE)
# print "posVar: ", Global.selfLoc.getPosVar(), "headVar: ", Global.selfLoc.getHeadingVar()
ballv = VisionLink.getGPSBallVInfo(Constant.CTLocal)
#---------------------------------------------------------------------------
# Already localising.
if isLocalising:
if focusDuration < focusTotalTime:
sActiveLocalise.DecideNextAction()
focusDuration += 1
headColor(Indicator.RGB_ORANGE)
# Counter-act the effect of increasing lostBall counter while
# localising, if not have this, locateBall will be (inappropriate)
# quickly triggereds.
Global.lostBall = hMath.DECREMENT(Global.lostBall)
else:
# Reset variables if finished focusing on beacon.
isLocalising = False
timerSinceLastLocalise = 0
# Track or find the ball
sFindBall.perform(True)
#---------------------------------------------------------------------------
# Trigger the active localise only if
# a) Dog not sure where it is, or where it is heading, OR
# b) Haven't done it for a while (var is small doesn't mean the position
# is definitely correct)
# c) AND haven't just localised
# d) AND ball velocity is low
#
elif (Global.selfLoc.getPosVar() > hMath.get95var(minPosVariance)\
or Global.selfLoc.getHeadingVar() > hMath.get95var(minHeadVariance)\
or timerSinceLastLocalise > MAX_BEACON_LOOK_INTERVAL)\
and not (Global.vBall.isVisible()\
and Global.ballD <= 40)\
and not (timerSinceLastLocalise < MIN_BEACON_LOOK_INTERVAL)\
and ballv[2] < 2:
focusDuration = 0 # Increase from this moment on.
isLocalising = True
sActiveLocalise.SmartSetBeacon()
sActiveLocalise.DecideNextAction()
# Look at the ball
else:
headColor(Indicator.RGB_GREEN)
timerSinceLastLocalise += 1
# Track or find the ball
sFindBall.perform(True)
def DecideNextAction():
if perform() != Constant.STATE_EXECUTING:
sFindBall.perform()
def performTimeElapsedAction(selType):
global gForceTimeElapse
global gKickType
global gLastKickFrame
global gLastBreakFrame
global gLastLocaliseFrame
global gKickCounter
global gBreakCounter
global gLocaliseCounter
gForceTimeElapse = True
if Action.shouldIContinueKick():
#print "Waiting for Kick to activate ...", gKickCounter
Action.continueKick()
moveHeadForwardTight()
elif gKickCounter > 0:
#print "Kicking ...", gKickCounter
if gKickType == AVOID_OWN_GOAL:
if gKickCounter > 10:
perform(0, 0, Action.MAX_TURN)
elif gKickCounter > 8:
perform(0, 0, 0)
else:
perform(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeGrabDribbleMWT)
elif gKickType == GAP_KICK:
selfH = Global.selfLoc.getHeading()
if abs(gKickHead - selfH) > 10:
turnCCW = gKickHead - selfH
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.SoftTapWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == SIDE_STEP_LEFT_SHOOT\
or gKickType == SIDE_STEP_RIGHT_SHOOT\
or gKickType == FWD_STEP_SHOOT:
# Do side step shoot...
if gKickCounter > FWD_STEP_SHOOT_DURATION:
left = Action.MAX_LEFT
if gKickType == SIDE_STEP_RIGHT_SHOOT:
left = -Action.MAX_LEFT
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 30)
perform(5, left, turnCCW)
# Do forward step shoot...
elif gKickCounter > NU_FWD_DURATION:
fwd = Action.MAX_FORWARD
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 15)
perform(fwd,
0,
turnCCW,
minorWalkType=Action.SkeFastForwardMWT)
# Do kick!
else:
gKickType = Action.NUFwdKickWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == JUST_SHOOT:
selfH = Global.selfLoc.getHeading()
if gKickHead - selfH > 5:
turnCCW = (gKickHead - selfH) / 2
# if (turnCCW > 0):
# left = Action.MAX_LEFT
# else:
# left = -Action.MAX_LEFT
#
# perform(5,left,hMath.CLIP(turnCCW,30))
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.HandKickLeftWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
else:
# Reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 160)
VisionLink.resetBall(x, y, 0, 0)
Action.continueKick()
Action.openMouth()
hTrack.panLow = False
gKickCounter -= 1
gLastKickFrame = Global.frame
elif gBreakCounter > 0:
# If we cannot see the ball, then reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
if not Global.vBall.isVisible():
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 50)
VisionLink.resetBall(x, y, 0, 0)
if gBreakCounter > 10:
Action.setHeadParams(0, 0, 0, Action.HTAbs_h)
Action.walk(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeFastForwardMWT)
elif gBreakCounter > 5:
Global.lostBall = Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(True)
rate = Action.SKE_FF_PG * 8.0 * 2.0 / 1000.0
fwd = Action.MAX_SKE_FF_FWD_STP * 0.8 / rate
Action.walk(fwd, 0, 0, minorWalkType=Action.SkeFastForwardMWT)
Action.openMouth()
else:
Global.lostBall = 8 - gBreakCounter + Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform()
Action.openMouth()
hTrack.panLow = True
gBreakCounter -= 1
gLastBreakFrame = Global.frame
elif Global.frame - gLastKickFrame > 1\
and Global.frame - gLastBreakFrame > 1:
#print "Kick!!!!"
selectKick(selType)
else:
resetPerform()
sGrab.resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def perform(params=None):
VisionLink.startProfile("rAttacker.py")
global gLastFrameCalled
global gLastDecisionFunction
global isGoal
id(params) # ignore
shouldIBeDodgy = gUseDodgyDog
if gLastFrameCalled != Global.frame - 1:
resetPerform()
gLastFrameCalled = Global.frame
### Variable lighting challenge ###
if Global.lightingChallenge and playMode:
if Global.vBall.isVisible():
isGoal = False
for pressSensor in range(1, 4, 1): # Used to react to back sensors
if VisionLink.getPressSensorCount(pressSensor) > 8:
print "It's a goal!!"
isGoal = True
if isGoal:
VisionLink.resetBall(Constant.FIELD_WIDTH / 2.0,
Constant.FIELD_LENGTH / 2.0, 0.0, 0.0)
pos = Global.selfLoc.getPos()
if (hMath.getDistanceBetween(pos[0], pos[1],
Constant.FIELD_WIDTH / 2.0,
Constant.FIELD_LENGTH / 2.0) < 50):
sFindBall.perform()
else:
hTrack.stationaryLocalise()
hTrack.saGoToTarget(Constant.FIELD_WIDTH / 2.0,
Constant.FIELD_LENGTH / 2.0)
return
else:
pass # perform normal attacker
elif Global.lightingChallenge:
ObsMapping()
return
### End variable lighting challenge ###
### Penalty Shooter ####
if Global.penaltyShot:
if Global.frame - Global.firstPlayFrame < 90:
hTrack.stationaryLocalise()
return
elif Global.frame - Global.firstPlayFrame < 115:
sFindBall.perform(True)
return
#print "rAttacker frame",
# Last man attacker can block
# if not hTeam.amIFurthestBack(ignoreRoles = []):
# print "not furthest back"
# elif sGrab.grabbingCount > 0:
# print "grabbing count", sGrab.grabbingCount
# elif sGrab.isGrabbed:
# print "grabbed"
# elif Global.frame - sGrabDribble.gLastActionFrame < 60:
# print "grabdribbleaction", sGrabDribble.gLastActionFrame
## if hTeam.amIFurthestBack(ignoreRoles = []) \
## and not sGrab.grabbingCount > 0 \
## and not sGrab.isGrabbed \
## and hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > 7000 \
## and not Global.penaltyShot \
## and Global.frame - sGrabDribble.gLastActionFrame > 60 \
## and Global.selfLoc.getHeading() < 180 \
## and Global.selfLoc.getY() < Constant.FIELD_LENGTH * 0.7:
## #print __name__, "candidate block"
## #_, _, speed, dirDegrees, _, _ = VisionLink.getGPSBallVInfo(Constant.CTLocal)
## #print "speed", speed
## # This speed really needs to be 4.5 to remove all noise but then it
## # nearly never blocks. I'm under pressure to keep it possible in the
## # face of all the other checks above, hence 4
## if sBlock.checkThenBlock(onlyVisualBall = True, minBallSpeed = 3,
## minDist = 40, maxDist = 80,
## bothSides = False, dontBlockCentre = True):
## print __name__, hMath.getTimeElapsed(sGrab.gLastGrabTime,
## VisionLink.getCurrentTime())
## print __name__, Global.frame, "blocking, gLastActionFrame =", sGrabDribble.gLastActionFrame
## print
## return
# Back off if stuck
if hStuck.amIStuckForward()\
and Global.ballD > 200\
and Global.gpsLocalBall.getDistance() > 200\
and not sGrab.isGrabbed\
and sGrab.grabbingCount == 0\
and Global.frame - sGrab.gLastApproachFrame >= 5\
and (not Global.penaltyShot)\
and (not Global.lightingChallenge):
sFindBall.perform(True)
return
# Don't go into own goal box
if hWhere.ballInOwnGoalBox(0)\
and (not sGrab.isGrabbed)\
and sGrab.grabbingCount == 0\
and (not Global.penaltyShot)\
and (not Global.lightingChallenge):
#print "Ball in goal box"
#print "avoiding"
doAvoidGoalBox()
hFWHead.DecideNextAction()
return # no dodgy
if not shouldIEndActivelyLocalise()\
or shouldIActivelyLocalise():
#print "localising"
doActivelyLocalise()
elif shouldIKick():
#shouldIBeDodgy = False #Really?
#print "kicking"
doKick()
elif Global.lightingChallenge and shouldIGoToCenter():
doGotoCenter()
else:
#print "tracking"
doTrackBall()
# Use dodgy dog if required
fwdCmd = Action.finalValues[Action.Forward]
shouldIBeDodgy = shouldIBeDodgy \
and (not sGrab.isGrabbed) \
and sGrab.grabbingCount == 0 \
and Global.frame - sGrab.gLastApproachFrame >= 5 \
and Global.ballD >= sGrab.CLOSE_DIST \
and Global.frame - sFindBall.gLastSpinFrame >= 15 \
and Global.frame - sGetBehindBall.gLastCalledFrame >= 5 \
and fwdCmd > Action.MAX_SKE_FWD_SPD/2 \
and fwdCmd > abs(Action.finalValues[Action.Left]) \
and sDodgyDog.shouldIBeDodgyToBall()
if shouldIBeDodgy:
#print "and dodging"
sDodgyDog.dodgyDogToBall()
VisionLink.stopProfile("rAttacker.py")
