def DecideNextAction():
global isGoal
#print "rAttacker: Global.selfLoc at", ("%x" % id(Global.selfLoc)),\
# "=", Global.selfLoc
Indicator.setDefault()
#VisionLink.setGPSPaused(0)
if 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):
scFindBall.perform()
else:
hTrack.stationaryLocalise()
hTrack.saGoToTarget(Constant.FIELD_WIDTH / 2.0,
Constant.FIELD_LENGTH / 2.0)
else:
perform()
else:
ObsMapping()
def doGotoCenter():
global lastGoToCenter, startGoToCenter
if Global.frame - lastGoToCenter > 1:
startGoToCenter = Global.frame
lastGoToCenter = Global.frame
hTrack.stationaryLocalise()
hTrack.saGoToTarget(Constant.FIELD_WIDTH / 2.0,
Constant.FIELD_LENGTH / 2.0 + 100)
def start():
global headMode, localiseTimer
hTrack.stationaryLocalise()
localiseTimer += 1
if localiseTimer > ORIENT_TIME:
headMode = ballTrack
localiseTimer = 0
return False
def ObsMapping():
global playMode
for pressSensor in range(1, 4, 1): # Used to react to back sensors
if VisionLink.getPressSensorCount(pressSensor) > 8:
print "Stop mapping and play the game."
VisionLink.setIfObstacleUpdate(False)
playMode = True
VisionLink.setIfObstacleUpdate(False)
return
VisionLink.setIfObstacleUpdate(True)
hTrack.stationaryLocalise()
def inPositionAndFacingTargetLocalise():
# Now what localise method to use? We have some selections here.
# 1. Stationary Localise 2. Smart Localise 3. Beacon Localise
#if VisionLink.getKickOffTeam() == VisionLink.getTeamColor() and \
# Global.kickOffState == Constant.DECIDEPOSITION_B:
# # Kicker looks for ball as well as beacons
# #hTrack.scanLikeGermanTeam()
# hTrack.scan(maxPan = 60, minPan = -60, highCrane = 35,
# lowSpeed = 8, highSpeed = 8)
#else:
hTrack.stationaryLocalise()
def spinWalkGenFunc():
for _ in xrange(150):
Indicator.showHeadColor(Indicator.RGB_GREEN)
hTrack.stationaryLocalise(5)
VisionLink.setIfGPSVisionUpdate(True)
yield True
while True:
VisionLink.setIfGPSVisionUpdate(False) # turn off vision update
Indicator.showHeadColor(Indicator.RGB_YELLOW)
target = (Constant.FIELD_WIDTH/2, Constant.FIELD_LENGTH/2)
walk(target, Global.selfLoc.getPos(), Global.selfLoc.getHeading())
yield True
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():
current = VisionLink.getBatteryLevel()
if current < 40: # if battery is low
Action.stopLegs()
hTrack.stationaryLocalise()
return
if command == "std" or command == "adj" or not currentMode:
adjWalk()
elif command == "cal" or command == "lrn":
calWalk()
else:
return # for all other commands, do nothing
# indicate
indicateReceivedCommand()
Indicator.showBatteryStatus()
if shouldOpenMouth or currentMode == grabTurn:
Action.openMouth()
else:
Action.closeMouth()
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 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 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")
def localise():
hTrack.stationaryLocalise(4, 50, -50)
def DecideNextAction():
global indCycle, grabObject, counter, shouldGrab, moveTo
global haveObstacle, isCorrectstandingPosition, distanceOK, framesLastSentYUV
global localiseTime, lastHeading, lostObstacle
global allBox, leftBox, rightBox, leftLongBox, rightLongBox
Indicator.superDebug()
point4 = [SIDE_MAX_DETECTION, 50]
point_leftbox_topleftlong = [-SIDE_MAX_DETECTION, 150]
shouldHeading = None
calculateObstacleBox()
if False:
print
print "isCorrectstandingPosition:", isCorrectstandingPosition
print "shouldGrab:", shouldGrab
print "haveObstacle:", haveObstacle
print "lostObstacle:", lostObstacle
print "Global.BallD:", Global.ballD
isCorrectstandingPosition = True
if (Global.vBall.isVisible() and Global.ballD > 30):
isCorrectstandingPosition = False
if shouldGrab or not isCorrectstandingPosition:
print "shouldGrab or find ball and stop"
if grabObject == None:
grabObject = GrabGenerator()
try:
grabObject.next()
except StopIteration:
reset()
else:
print "stand still"
Action.standStill()
result =\
VisionLink.getPointToNearestObstacleInBox(\
point_leftbox_topleftlong[0],\
point_leftbox_topleftlong[1],\
point4[0],point4[1],
5\
)
# if (leftlong > MIN_POINTS_CONSIDERED_AS_OBSTACLE and leftlong > rightlong):
# haveObstacle = haveObstacle + 1
# print "haveObstacle", haveObstacle
# print "checking left"
# if result == None:
# print "error it should be checking left but return None"
# shouldHeading = None
# localise()
# else:
# pointX, pointY = result
# shouldHeading = hMath.getHeadingToMe(0,0,0,pointX, pointY)
# elif (rightlong > MIN_POINTS_CONSIDERED_AS_OBSTACLE and rightlong > leftlong):
# haveObstacle = haveObstacle + 1
# print "haveObstacle", haveObstacle
# print "checking right"
# if result == None:
# print "error it should be checking right but return None"
# shouldHeading = None
# localise()
# else:
# pointX, pointY = result
# shouldHeading = hMath.getHeadingToMe(0,0,0,pointX, pointY)
# else :
# localise()
# if haveObstacle > 0:
# print "haveObstacle", haveObstacle
# haveObstacle = 0
if result == None:
if (lostObstacle > 3 and lastHeading != None):
shouldHeading = lastHeading
print "just lost"
else:
localise()
print "no nearest obstacle"
haveObstacle = 0
lostObstacle = lostObstacle + 1
lastHeading = None
else:
print "result is not none", result
pointX, pointY = result
shouldHeading = hMath.getHeadingToMe(0, 0, 0, pointX, pointY)
print "pointing", -pointX, ":", pointY
print "heading:", shouldHeading
lastHeading = shouldHeading
if (shouldHeading != None):
#localise()
calculate = int((shouldHeading - 90.0) / 90 * 80)
if (framesLastSentYUV > 30):
VisionLink.sendYUVPlane()
framesLastSentYUV = 0
hTrack.stationaryLocalise(2, calculate + 3, calculate - 3)
#Action.setHeadParams(-pointX, 0, pointY, Action.HTAbs_xyz)
print "calculate:", calculate
if result != None and (leftBox > MIN_POINTS_CONSIDERED_AS_OBSTACLE
and leftBox > rightBox):
print "move left:", haveObstacle
haveObstacle = haveObstacle + 1
lostObstacle = 0
if (framesLastSentYUV > 30):
VisionLink.sendYUVPlane()
framesLastSentYUV = 0
if (haveObstacle > 3):
shouldGrab = True
moveTo = LEFT
elif result != None and (rightBox > MIN_POINTS_CONSIDERED_AS_OBSTACLE
and rightBox > leftBox):
print "move right:", haveObstacle
haveObstacle = haveObstacle + 1
lostObstacle = 0
if (framesLastSentYUV > 30):
VisionLink.sendYUVPlane()
framesLastSentYUV = 0
if (haveObstacle > 3):
shouldGrab = True
moveTo = RIGHT
if False: #all > 0 :# and counter == 60:
print
print "all:", allBox, "endall"
print "left:", leftBox, "endleft"
print "right:", rightBox, "endright"
print "leftlong:", leftLongBox, "endleft"
print "rightlong:", rightLongBox, "endright"
counter = 0
counter = counter + 1
framesLastSentYUV = framesLastSentYUV + 1
def kickOffKickToGap():
global playCounter, whenGrabbed
global bestHeading, gBestHeading
(myX, myY) = Global.selfLoc.getPos()
myH = Global.selfLoc.getHeading()
if pReady.chosenFormation == 0:
print "chosenFormation =", pReady.chosenFormation, "- going right"
(kickX, kickY) = (Constant.MAX_GOALBOX_EDGE_X,
Constant.TOP_GOALBOX_EDGE_Y)
(suppX, suppY) = (kickX + 40, kickY - 40)
(lKickX, lKickY) = hMath.getLocalCoordinate(myX, myY, myH, kickX,
kickY)
else:
print "chosenFormation =", pReady.chosenFormation, "- going left"
(kickX, kickY) = (Constant.MIN_GOALBOX_EDGE_X,
Constant.TOP_GOALBOX_EDGE_Y)
(suppX, suppY) = (kickX - 40, kickY - 40)
(lKickX, lKickY) = hMath.getLocalCoordinate(myX, myY, myH, kickX,
kickY)
if pReady.isCentre(Global.kickOffPos):
if playCounter > 120: # 6 sec, just in case
return False
if whenGrabbed == 0:
#print "grabbing"
if sGrab.grabbingCount == 0:
# Find the obstacle gap just before we drop the head to grab
(gapLeft, gapRight, bestHeading, angle) = \
VisionLink.getBestGap(lKickX, lKickY,
GAP_MAX_DIST,GAP_MIN_DIST,
GAP_MIN,GAP_MIN_INTENSITY,
Constant.OBS_USE_SHARED)
gBestHeading = hMath.local2GlobalHeading(
Global.selfLoc, bestHeading)
#gBestHeading = hMath.getHeadingBetween(Constant.FIELD_WIDTH/2, Constant.FIELD_LENGTH/2, kickX, kickY)
print "local target", lKickX, lKickY
print "gap", bestHeading, gBestHeading
if sGrab.perform() == Constant.STATE_FAILED:
return False
if sGrab.isGrabbed:
print "grabbed, best gap at local", bestHeading, \
"global", gBestHeading
whenGrabbed = playCounter
return True
# Don't prevent other forward from being attacker
Global.myRole = Global.myLastRole = Constant.SUPPORTER
relH = gBestHeading - Global.selfLoc.getHeading()
if abs(relH) >= 3:
if sGrabDribble.perform(0, 0, relH) == Constant.STATE_FAILED:
return False
return True
Action.kick(Action.SoftTapWT)
if Action.shouldIContinueKick():
Action.continueKick()
return True
sGrab.resetPerform()
return False
elif pReady.isForward(Global.kickOffPos):
if playCounter < 80:
sDodgyDog.dodgyDogTo(suppX, suppY)
hTrack.stationaryLocalise(6, 30, -30)
return True
elif playCounter < 120:
if hTrack.canSeeBall and Global.ballD < 30:
Global.myRole = Global.myLastRole = Constant.ATTACKER
return False
h = hMath.normalizeAngle_0_360(myH + Global.ballH)
hTrack.saGoToTargetFacingHeading(suppX, suppY, h)
hFWHead.DecideNextAction()
return True
else:
Global.myRole = Global.myLastRole = Constant.ATTACKER
return False
else:
return False
return False
def kickOffPushToSupporter():
global playCounter, whenGrabbed
global bestHeading, gBestHeading
(myX, myY) = Global.selfLoc.getPos()
myH = Global.selfLoc.getHeading()
if pReady.chosenFormation == 0:
(suppX, suppY) = (PUSH_TO_X + 40, PUSH_TO_Y - 40)
else:
(suppX, suppY) = (PUSH_TO_X - 40, PUSH_TO_Y - 40)
if pReady.isCentre(Global.kickOffPos):
if playCounter > 180: # 6 sec, just in case
return False
if whenGrabbed == 0:
#print "grabbing"
if sGrab.grabbingCount == 0:
# Find the obstacle gap just before we drop the head to grab
#(gapLeft, gapRight, bestHeading, angle) = \
# VisionLink.getBestGap(lKickX, lKickY,
# GAP_MAX_DIST,GAP_MIN_DIST,
# GAP_MIN,GAP_MIN_INTENSITY,
# Constant.OBS_USE_SHARED)
gap = getGapTo(PUSH_TO_X, PUSH_TO_Y)
if gap != None:
(gapLeft, gapRight, bestHeading, angle) = gap
else:
bestHeading = hMath.getHeadingToAbsolute(
Global.selfLoc, PUSH_TO_X, PUSH_TO_Y)
gBestHeading = hMath.local2GlobalHeading(
Global.selfLoc, bestHeading)
print "gap ", bestHeading, gBestHeading
if sGrab.perform() == Constant.STATE_FAILED:
return False
if sGrab.isGrabbed:
#print "grabbed, best gap at local", bestHeading, \
# "global", gBestHeading
whenGrabbed = playCounter
return True
# Don't prevent other forward from being attacker
Global.myRole = Global.myLastRole = Constant.SUPPORTER
relH = gBestHeading - Global.selfLoc.getHeading()
if playCounter - whenGrabbed < 40:
if sGrabDribble.perform(Action.MAX_FORWARD, 0, relH) \
== Constant.STATE_FAILED:
return False
return True
Action.kick(Action.GTChestPushWT)
if Action.shouldIContinueKick():
Action.continueKick()
return True
sGrab.resetPerform()
return False
elif pReady.isForward(Global.kickOffPos):
if playCounter < 80:
sDodgyDog.dodgyDogTo(suppX, suppY)
hTrack.stationaryLocalise(6, 30, -30)
return True
elif playCounter < 120:
if hTrack.canSeeBall and Global.ballD < 30:
Global.myRole = Global.myLastRole = Constant.ATTACKER
return False
h = hMath.normalizeAngle_0_360(myH + Global.ballH)
hTrack.saGoToTargetFacingHeading(suppX, suppY, h)
hFWHead.DecideNextAction()
return True
else:
Global.myRole = Global.myLastRole = Constant.ATTACKER
return False
else:
return False
return False
def DecideNextAction():
global lastSendTime
# Reset the 12 leds on face pattern.
Indicator.setDefault()
blueLeft = Global.VisualObject(
*VisionLink.getVisualObject(Constant.vobBlueLeftBeacon))
blueRight = Global.VisualObject(
*VisionLink.getVisualObject(Constant.vobBlueRightBeacon))
yellowLeft = Global.VisualObject(
*VisionLink.getVisualObject(Constant.vobYellowLeftBeacon))
yellowRight = Global.VisualObject(
*VisionLink.getVisualObject(Constant.vobYellowRightBeacon))
yellowGoal = Global.VisualObject(
*VisionLink.getVisualObject(Constant.vobYellowGoal))
blueGoal = Global.VisualObject(
*VisionLink.getVisualObject(Constant.vobBlueGoal))
ball = Global.VisualObject(*VisionLink.getVisualObject(Constant.vobBall))
if blueLeft.getConfidence() > 0:
Indicator.finalValues[Indicator.Led7] = Indicator.LED3_INTENSITY_MAX
if blueRight.getConfidence() > 0:
Indicator.finalValues[Indicator.Led8] = Indicator.LED3_INTENSITY_MAX
if blueGoal.getConfidence() > 0:
Indicator.finalValues[Indicator.Led11] = Indicator.LED3_INTENSITY_MAX
if yellowLeft.getConfidence() > 0:
Indicator.finalValues[Indicator.Led3] = Indicator.LED3_INTENSITY_MAX
if yellowRight.getConfidence() > 0:
Indicator.finalValues[Indicator.Led4] = Indicator.LED3_INTENSITY_MAX
if yellowGoal.getConfidence() > 0:
Indicator.finalValues[Indicator.Led12] = Indicator.LED3_INTENSITY_MAX
if redDogSeen():
Indicator.finalValues[Indicator.Led5] = Indicator.LED3_INTENSITY_MAX
if blueDogSeen():
Indicator.finalValues[Indicator.Led6] = Indicator.LED3_INTENSITY_MAX
Indicator.superDebug()
#send unexpected cplanes
unexpectedCPlane = False
if expectBall == (ball.getConfidence() == 0):
unexpectedCPlane = True
if expectBlueGoal == (blueGoal.getConfidence() == 0):
unexpectedCPlane = True
if expectYellowGoal == (yellowGoal.getConfidence() == 0):
unexpectedCPlane = True
if expectBlueLeft == (blueLeft.getConfidence() == 0):
unexpectedCPlane = True
if expectBlueRight == (blueRight.getConfidence() == 0):
unexpectedCPlane = True
if expectYellowLeft == (yellowLeft.getConfidence() == 0):
unexpectedCPlane = True
if expectYellowRight == (yellowRight.getConfidence() == 0):
unexpectedCPlane = True
if expectRedDog == (not redDogSeen()):
unexpectedCPlane = True
if expectBlueDog == (not blueDogSeen()):
unexpectedCPlane = True
if bSendCPlane and unexpectedCPlane:
if Global.frame - lastSendTime > NO_SENDING_CPLANE:
lastSendTime = Global.frame
VisionLink.sendCompressedCPlane()
if headCommand == "spin":
hTrack.spinningLocalise()
elif headCommand == "sloc":
hTrack.stationaryLocalise()
elif headCommand is not None:
Action.setHeadParams(*headCommand)
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 readyMatchPlaying():
global debug, timer, inStartPos, isFacingTargetGoal, getOutOfGoalDirection, lastTurningDirection, gUseDodgyDog
selfX = Global.selfLoc.getPos()[0]
selfY = Global.selfLoc.getPos()[1]
selfH = Global.selfLoc.getHeading()
Action.closeMouth()
# Player number detection. The goalie *must* be player one. Freak if not.
if Global.myPlayerNum == 1 and not Global.isGoalieBehaviour:
Action.kick(Action.ChestPushWT)
return
elif Global.myPlayerNum != 1 and Global.isGoalieBehaviour:
Action.kick(Action.ChestPushWT)
return
if not Global.penaltyShot and hStuck.amIStuckForward(dodgeTime=9):
return
# Need to move toward the field ?
if shouldITurnTowardTheField():
Action.walk(0, 0, getOutOfGoalDirection)
# Am I in the correct starting position and facing toward the target goal
# (or facing upward)?
if inStartPos and isFacingTargetGoal:
# use the ball distance to try to move as close as the centre circle
#if VisionLink.getKickOffTeam() == VisionLink.getTeamColor() and \
# useBallDistanceSetKickOffPosition():
# return
# Do I need to move to a new position if I am in a wrong position?
if Debug.allowReadyMove:
if hMath.getDistanceBetween(selfX, selfY, \
Global.kickOffPos[0], Global.kickOffPos[1]) \
> Constant.POS_OFFSET_OUT_KICK:
inStartPos = False
return
# Turn myself if I am not facing toward the desired heading. At the
# moment 90 is the desired heading angle.
# TODO: Accoiding to different positions, different desired heading.
if abs(hMath.normalizeAngle_180(selfH - 90)) > \
Constant.READY_HEADING_OFFSET:
isFacingTargetGoal = False
return
inPositionAndFacingTargetLocalise()
return
# Am I in the correct starting position but not facing the target goal (or
# facing upward)?
if inStartPos:
# Do I need to move to a new position if I am in a wrong position?
if Debug.allowReadyMove:
if hMath.getDistanceBetween(selfX, selfY, \
Global.kickOffPos[0], Global.kickOffPos[1])\
> Constant.POS_OFFSET_OUT_KICK:
inStartPos = False
return
# Is me facing toward the target goal now? If I am facing the right
# direction, then better reset the turning direction variable.
if abs(selfH - 90) < Constant.READY_HEADING_OFFSET:
isFacingTargetGoal = True
lastTurningDirection = None
# If I am not facing toward the target goal, then turn slowly without
# moving forward or left.
else:
# Determine the best turning direction and stick with it.
if lastTurningDirection == None:
# I haven't determine my turning direction yet. Determine now.
if Global.selfLoc.getHeading() >= 270 or \
Global.selfLoc.getHeading() <= 90: # -90 <= angle <= 90
lastTurningDirection = Constant.dANTICLOCKWISE
if Debug.readyDebug:
print "Anti-clockwise direction!"
else:
# 90 < angle < 270
lastTurningDirection = Constant.dCLOCKWISE
if Debug.readyDebug:
print "Clockwise direction!"
if lastTurningDirection == Constant.dCLOCKWISE:
Action.walk(0, 0, -15)
else:
Action.walk(0, 0, 15)
return
# Phase1: I am not in the correct starting position. First step for me is
# to stand still and beacon localise.
if timer <= INITIAL_LOCALISE_TIME:
if Debug.readyDebug and timer == 1:
print "Phase 1 has been executed!"
if timer <= 79:
Global.kickOffState = Constant.NOTDECIDED
Action.stopLegs()
# Need to move toward the field ?
if shouldITurnTowardTheField():
Action.walk(0, 0, getOutOfGoalDirection)
hTrack.stationaryLocalise()
timer = timer + 1
if timer >= 80:
# Now decide my starting kickoff position dynamically now.
if hTeam.robotAlive() != 1:
getMyKickOffPosition()
return
lastTurningDirection = None
# Now we can either use the static result from getMyKickOffPosition() or
# dynamically calculated again,
if hTeam.robotAlive() != 1:
getMyKickOffPosition()
# If I am unable to find the kickoff position earlier (possibly because of
# wirless problem), then probably only 2 or less robots are on the field
# now. Notice that I can't be a goalie in this case. I must be either
# player 2 forward or player 3 forward. Not player 4 forward because it
# determines its kicking position without worrying about any other
# forwards.
if Global.kickOffState == Constant.NOTDECIDED or Global.kickOffPos == None:
alive = hTeam.forwardAlive()
if alive >= 3:
if Debug.readyDebug:
print "Something went wrong, three forwards without assigning! Automatical assign!"
##~ # This case may happen in friendly games where not all four robots are present.
##~ # Firstly if there are three forwards alive, but I am not assigned a position. Then:
##~ # 1. I can't be player 1 because this is a goalie
##~ # 2. I can't be player number 4 forward.
##~ # So I must be either player 2 or player 3 forward.
##~ #
##~ # If I am a player 3, then probably the three forwards are: 1, 2, 3
##~ # If I am a player 2, then probably the three forwards are: 1, 2, 4
##~
if Global.myPlayerNum == 3:
_, Global.kickOffPos, _, _ = getAllPossibleKickingOffPosition()
Global.kickOffState = Constant.DECIDEPOSITION_B
elif Global.myPlayerNum == 2:
# Try to get player 4's decision.
if Global.teamPlayers[3].getCounter() > 0:
position = Global.teamPlayers[3].getKickedOff()
if position == Constant.DECIDEPOSITION_A:
determineMyKickingOffPositionIfOnlyMeForwardAlive(
False, True, True)
elif position == Constant.DECIDEPOSITION_B:
determineMyKickingOffPositionIfOnlyMeForwardAlive(
True, False, True)
elif position == Constant.DECIDEPOSITION_C:
determineMyKickingOffPositionIfOnlyMeForwardAlive(
True, True, False)
else:
Debug.goAndMakeTraceBack()
return
else:
# Well, lets wait.
return
elif alive == 2:
if Debug.readyDebug:
print "Only two alive case - What the hell?"
if Global.myPlayerNum == 2:
Global.kickOffPos, b, c, d = getAllPossibleKickingOffPosition()
Global.kickOffState = Constant.DECIDEPOSITION_A
elif Global.myPlayerNum == 3:
a, Global.kickOffPos, c, d = getAllPossibleKickingOffPosition()
Global.kickOffState = Constant.DECIDEPOSITION_B
else:
if Debug.readyDebug:
print "Something went wrong, I am not player 2 or 3, why not decided position?"
elif alive == 1:
if Debug.readyDebug:
print "Only one alive case!"
a, b, c, d = getAllPossibleKickingOffPosition()
if Global.myPlayerNum == 2:
Global.kickOffPos = a
Global.kickOffState = Constant.DECIDEPOSITION_A
elif Global.myPlayerNum == 3:
Global.kickOffPos = b
Global.kickOffState = Constant.DECIDEPOSITION_B
elif Global.myPlayerNum == 4:
Global.kickOffPos = c
Global.kickOffState = Constant.DECIDEPOSITION_C
else: # Goalie
Global.kickOffPos = d
Global.kickOffState = Constant.DECIDEPOSITION_D
# At this point the kicking off position must been determined.
showKickOffHeadIndicator()
# Set the initial roles based on position
if Global.kickOffPos == P_REC_CENTRE:
Global.myRole = Constant.ATTACKER
elif Global.kickOffPos == P_REC_LEFT_WIDE \
or Global.kickOffPos == P_REC_RIGHT_WIDE:
Global.myRole = Constant.DEFENDER
elif Global.kickOffPos == P_REC_RIGHT_CLOSE \
or Global.kickOffPos == P_REC_LEFT_CLOSE:
Global.myRole = Constant.SUPPORTER
elif Global.kickOffPos == P_KICK_KICKER:
Global.myRole = Constant.ATTACKER
elif Global.kickOffPos == P_KICK_LEFT_BACK \
or Global.kickOffPos == P_KICK_RIGHT_BACK:
Global.myRole = Constant.DEFENDER
elif Global.kickOffPos == P_KICK_RIGHT_FORWARD \
or Global.kickOffPos== P_KICK_LEFT_FORWARD:
Global.myRole = Constant.SUPPORTER
# Testing the kickoff position without actually moving them.
##~ if Debug.readyDebug and Debug.testKickOffPosition:
##~ if Global.kickOffState == None:
##~ print "My kickoff position is: None %f" %Global.myPlayerNum
##~ else:
##~ print "My player number: %f" %Global.myPlayerNum
##~ print "My kickoff position is: %f" %Global.kickOffState
##~ return
# Move from phase 2 to phase 3. Am I in the correct starting position now?
dist = hMath.getDistanceBetween(selfX, selfY, Global.kickOffPos[0],
Global.kickOffPos[1])
if dist < Constant.POS_OFFSET_IN_KICK:
# Ok, I am in the correct starting position. I stop myself now.
Action.stopLegs()
inStartPos = True
# Phase2: I need to walk to my starting position. Localise by moving the
# head while I move.
else:
if dist < sDodgyDog.MIN_DODGY_TARGET_DIST:
hTrack.stationaryLocalise()
else:
# Move head less while dodgy to see obstacles
hTrack.stationaryLocalise(5, 30, -30)
if gUseDodgyDog and \
sDodgyDog.shouldIBeDodgy(Global.kickOffPos[0],
Global.kickOffPos[1], True,
Constant.POS_OFFSET_OUT_KICK):
if sDodgyDog.dodgyDogTo(
Global.kickOffPos[0], Global.kickOffPos[1], True,
Constant.POS_OFFSET_OUT_KICK) == Constant.STATE_SUCCESS:
inStartPos = True
else:
movingAction()
