def DecideNextAction():
global BallPosX, BallPosY, BallDirX, BallPosConfidence, panDir
global lastAction, lastFrame
if lastFrame == Global.frame: # if this function has been called
return
lastFrame = Global.frame
Indicator.showBallIndicator()
GetBallInfo()
if BallPosConfidence < BALL_LOST_DEFINITELY:
SearchBySpinning(BallDirX)
elif BallPosConfidence < BALL_LOST:
Action.stopLegs()
SearchByHeadMoving(BallDirX)
else:
WalkToBall(BallPosX, BallPosY)
if BALL_LOST_MAYBE < BallPosConfidence < 1:
Action.setHeadToLastPoint()
else:
if lastAction == SPINNING:
Action.setHeadParams(0, 0, 0, Action.HTAbs_h)
else:
hTrack.trackVisualBall()
#PointHeadAtBall(BallPosX, BallPosY)
if canSeeBall() and BallPosY > 80:
# if not close to the ball
sWLocalise.DecideNextAction()
if sWLocalise.isLocalising:
BallPosConfidence = 1.0
def GetReadyGenerator():
selfx = Global.selfLoc.getX()
selfy = Global.selfLoc.getY()
selfh = hMath.normalizeAngle_180(Global.selfLoc.getHeading())
centerx = Constant.FIELD_WIDTH / 2
centery = Constant.FIELD_LENGTH / 2
heading = hMath.getHeadingBetween(selfx,selfy,centerx,centery)
# 1st quad
if selfx <= centerx and selfy <= centery:
turn = heading - selfh
# 2nd quad
elif selfx <= centerx and selfy >= centery:
turn = heading + selfh
# 3rd quad
elif selfx >= centerx and selfy <= centery:
turn = - heading + selfh
# 4th quad
else:
turn = - heading - selfh
print "Turn Angle is " + str(turn) + " Heading is " + str(heading)
period = turn / 30 * 8
for _ in range(period):
sGrabWalk.Perform(0,0,30)
yield Constant.STATE_EXECUTING
for _ in range(15):
Action.kick(Action.DiveKickWT)
yield Constant.STATE_EXECUTING
Action.forceStepComplete()
i = 0
while 1:
i += 1
if Global.vBall.getConfidence() > 0:
# Wait at least one second before checking this..
# So that the ball goes somewhere far
if i > 75\
and Global.vBall.getDistance() < 60\
and Global.haveBall > 5:
break
turn = hMath.CLIP(Global.vBall.getHeading(),20)
Action.walk(3,0,turn)
hTrack.trackVisualBall()
else:
Action.walk(3,0,5)
hTrack.spinningLocalise()
yield Constant.STATE_EXECUTING
while 1:
yield Constant.STATE_SUCCESS
def findByLastVisual():
pan_sensor = hMath.MICRORAD2DEG(VisionLink.getAnySensor(Constant.ssHEAD_PAN))
# Clip the desired values to the min / max sensor values
pan = min(max(Global.desiredPan,-88),88)
hTrack.trackVisualBall()
if Global.frame - gLastSpinFrame > SPIN_BREAK_TIME:
if abs(pan - pan_sensor) > 12:
Global.lostBall = max(Global.lostBall-1,0)
if not gHeadOnly:
walkToBall(Global.fstVisBallDist, Global.fstVisBallHead,
getBehind = gGetBehind)
def trackBall():
global gIsClockwise
# ignore first two visual ball info when the robot was spinning.
if Global.frame - gLastSpinFrame > 1:
vel = VisionLink.getGPSBallVInfo(Constant.CTLocal)
velX, velY = vel[0], vel[1]
#print " vel (",vel[0],",",vel[1],")",
if abs(velX) > 2 or abs(velY) > 2:
#print " velocity, ",
ballX = Global.gpsLocalBall.getX() - velX # * 5
ballY = Global.gpsLocalBall.getY() + velY # * 5
ballD = hMath.getDistanceBetween(0, 0, ballX, ballY)
else:
ballD = Global.weightedVisBallDist
if ballD < 40:
hTrack.panLow = True
else:
hTrack.panLow = False
# Check which direction does the ball appear from the image
# 1 = top right, 2 = bottom right,
# 3 = top left, 4 = bottom left
imgDir = Global.vBall.getImgDirection()
#if imgDir == 1 or imgDir == 4:
# hTrack.panLow = False
#elif imgDir == 2 or imgDir == 3:
# hTrack.panLow = True
#else:
# hTrack.panLow = True
if imgDir == 1 or imgDir == 2:
hTrack.panDirection = Constant.dCLOCKWISE
gIsClockwise = True
elif imgDir == 3 or imgDir == 4:
hTrack.panDirection = Constant.dANTICLOCKWISE
gIsClockwise = False
if not gHeadOnly:
walkToBall(Global.ballD, Global.ballH, getBehind=True)
hTrack.trackVisualBall()
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 performGenerator():
current_branch = 0
BRANCH_FAR = 1
BRANCH_NEAR = 2
BRANCH_READY = 3
while 1:
Action.closeMouth()
if Global.vBall.getConfidence() > 0:
ballD, ballH = Global.vBall.getDistance(), Global.vBall.getHeading()
ballH *= 0.8
hTrack.trackVisualBall()
# Adding hysterisis here..
if ballD > PARAM_NEAR_DISTANCE\
or (ballD > PARAM_NEAR_DISTANCE - 5 and current_branch == BRANCH_FAR):
current_branch = BRANCH_FAR
turnccw = hMath.CLIP(ballH,PARAM_FAR_TURNCCW)
Action.walk(PARAM_FAR_FORWARD,0,turnccw)
Action.finalValues[Action.Pg] = PARAM_FAR_PG
Action.finalValues[Action.Hdf] = PARAM_FAR_HDF
Action.finalValues[Action.Hdb] = PARAM_FAR_HDB
yield Constant.STATE_EXECUTING
else:
if ballD > PARAM_READY_DISTANCE:
current_branch = BRANCH_NEAR
if abs(ballH) > PARAM_READY_ANGLE\
or abs(Global.desiredPan) > PARAM_READY_ANGLE:
turnccw = hMath.CLIP(ballH,PARAM_NEAR_TURNCCW)
left = hMath.CLIP(-ballH,3)
Action.walk(1,left,turnccw)
Action.finalValues[Action.Pg] = PARAM_FIX_PG
Action.finalValues[Action.Hf] = PARAM_NEAR_HF
Action.finalValues[Action.Hb] = PARAM_NEAR_HB
Action.finalValues[Action.Hdf] = PARAM_NEAR_HDF
Action.finalValues[Action.Hdb] = PARAM_NEAR_HDB
yield Constant.STATE_EXECUTING
else:
turnccw = hMath.CLIP(ballH,PARAM_NEAR_TURNCCW)
Action.walk(PARAM_NEAR_FORWARD,0,turnccw)
Action.finalValues[Action.Pg] = PARAM_NEAR_PG
Action.finalValues[Action.Hf] = PARAM_NEAR_HF
Action.finalValues[Action.Hb] = PARAM_NEAR_HB
Action.finalValues[Action.Hdf] = PARAM_NEAR_HDF
Action.finalValues[Action.Hdb] = PARAM_NEAR_HDB
Action.finalValues[Action.Fso] = PARAM_NEAR_FSO
yield Constant.STATE_EXECUTING
else:
current_branch = BRANCH_READY
turnccw = hMath.CLIP(ballH,10)
for _ in range(15):
Action.walk(PARAM_READY_FORWARD,0,turnccw)
Action.finalValues[Action.Pg] = PARAM_READY_PG
Action.finalValues[Action.Hf] = PARAM_READY_HF
Action.finalValues[Action.Hb] = PARAM_READY_HB
Action.finalValues[Action.Hdf] = PARAM_READY_HDF
Action.finalValues[Action.Hdb] = PARAM_READY_HDB
Action.finalValues[Action.Ffo] = PARAM_READY_FFO
Action.finalValues[Action.Fso] = PARAM_READY_FSO
Action.finalValues[Action.HeadType] = Action.HTAbs_h
Action.finalValues[Action.Panx] = 0
Action.finalValues[Action.Tilty] = PARAM_READY_TILT
Action.finalValues[Action.Cranez] = PARAM_READY_CRANE
yield Constant.STATE_EXECUTING
yield Constant.STATE_SUCCESS
else:
if Global.lostBall < 3:
hTrack.trackVisualBall()
else:
if Global.lostBall < 8:
hTrack.trackGpsBall()
else:
sFindBall.perform()
yield Constant.STATE_EXECUTING