def shouldIBeDodgy(destX, destY, fixedDest = False,\
fixedDestVar = FIXED_DEST_VAR):
global gUseMinDist, gUseMinIntensity
#return False #HACK: Make dodgydog off all the time
#return True #HACK: Make dodgydog on all the time
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
dist = hMath.getDistanceBetween(selfX, selfY, destX, destY)
localDest = hMath.getLocalCoordinate(selfX, selfY, selfH, destX, destY)
localHead = hMath.cartToPolar(*localDest) # zero right
if dist <= MIN_DODGY_TARGET_DIST:
return False
if fixedDest and dist < fixedDestVar:
return False
# clip distance we count obstacles to MAX_DODGY_OBSTACLE_DIST or
# (dist - MIN_DODGY_TARGET_DIST) so we don't dodge obstacles that are
# miles away, and don't dodge obstacles that are right next to the
# target.
dist -= MIN_DODGY_TARGET_DIST
if dist >= MAX_DODGY_OBSTACLE_DIST:
dist = MAX_DODGY_OBSTACLE_DIST
localDest = hMath.polarToCart(dist, localHead)
tmp = VisionLink.getNoObstacleBetween(0, 0,
int(localDest[0]), int(localDest[1]),
CORRIDOR_WIDTH, gUseMinDist,
gUseMinIntensity, Constant.OBS_USE_NONE)
if Debug.dodgyDebug:
print "shouldIBeDodgy: obs in corr lhead, dist (", localHead, \
", ", dist, ") = ", tmp,
if tmp > MIN_OBS_IN_CORRIDOR:
#when get a better sidestep, take out mindist
#requirement and when close add left component?
if Debug.dodgyDebug:
print "-> True"
return True
if Debug.dodgyDebug:
print "-> False"
return False
def walkToStillBall(ballD, ballH, getBehind = GET_BEHIND_NONE):
global gLastTurnFrame
global gGoalie
if getBehind == GET_BEHIND_GOALIE:
getBehind = GET_BEHIND_PRIORITY
gGoalie = True
fwd = Action.MAX_FORWARD
# Lighting challenge and penalty shooter don't get behind. It makes the
# grab a bit harder and takes time
if Global.lightingChallenge or Global.penaltyShot:
getBehind = GET_BEHIND_NONE
# Don't get behind when close to our goal box since
# this moves us towards it (unless we are the goalie)
if not gGoalie and hWhere.selfInOwnGoalBox(20, sideoffset = 10):
getBehind = GET_BEHIND_NONE
# Calculate getBehind adjustment
leftAdj = 0
if getBehind > GET_BEHIND_NONE:
myx, myy = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
ballx, bally = hMath.getGlobalCoordinate(myx, myy, myh,
*hMath.polarToCart(ballD, ballH+90))
# In defense we get between our goal and the ball. In attack we line
# up the ball and target goal. In the (small) midfield we line up with
# upfield
reversePoints = False
if bally < Constant.FIELD_LENGTH/2 - 20 \
or hTeam.amIFurthestBack(ignoreRoles = []):
lineX, lineY = Constant.OWN_GOAL_X, Constant.OWN_GOAL_Y
elif bally > Constant.FIELD_LENGTH/2 + 20:
lineX, lineY = Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y
reversePoints = True
else:
lineX, lineY = ballx, 0
# Draw a line between ball and goal. Find self offset from the line
dist = ((ballx - lineX)*(lineY - myy)-(lineX - myx)*(bally - lineY)) /\
math.sqrt(hMath.SQUARE(ballx - lineX) \
+ hMath.SQUARE(bally - lineY))
# If dist > 0 we are to the right of the line, so adjustment left
# These adjustments will only be applied assuming we are facing
# the ball
if dist > 15:
leftAdj = 14
elif dist < -15:
leftAdj = -12
elif myy > bally: # Always sidestep from in front
if dist >= 0:
leftAdj = 14
elif dist < 0:
leftAdj = -12
else:
leftAdj = 0
if leftAdj != 0:
# At this point check that sidestepping in this direction is not
# going to put obstacles between us and the ball. If so it would be
# better to go direct (and let dodgy dog do its thing)
(lBallX, lBallY) = Global.gpsLocalBall.getPos()
nobs = VisionLink.getNoObstacleBetween(leftAdj, 0,
int(lBallX) + leftAdj,
int(lBallY) - 30,
abs(leftAdj), 0,
Constant.MIN_GPS_OBSTACLE_BOX,
Constant.OBS_USE_GPS)
if nobs > 2 * Constant.MIN_GPS_OBSTACLE_BOX:
#Indicator.showFacePattern([3, 2, 0, 2, 3])
leftAdj = 0
# Multiply getBehind in some cases. Only one of these can apply at
# at time so put them in order of importance.
leftAdjMult = 1
if getBehind == GET_BEHIND_DEFAULT:
# Last man attacker must be behind
if hTeam.amIFurthestBack(ignoreRoles = []):
if myy < bally:
getBehind = GET_BEHIND_PRIORITY
else:
getBehind = GET_BEHIND_LOTS
# In defense do lots of getBehind
elif bally < Constant.FIELD_LENGTH * 0.25:
getBehind = GET_BEHIND_LOTS
# From in front of the ball do a little more to get behind the
# ball
elif myy > bally:
getBehind = GET_BEHIND_MORE
# Lots of getBehind if requested
if getBehind >= GET_BEHIND_ONLY:
fwd = 10 # Walk in a circular arc
leftAdjMult = 2
elif getBehind >= GET_BEHIND_PRIORITY:
fwd = 30 # Slow down so getBehind has more effect
leftAdjMult = 2
elif getBehind >= GET_BEHIND_LOTS:
leftAdjMult = 2
elif getBehind >= GET_BEHIND_MORE:
leftAdjMult = 1.5
leftAdj *= leftAdjMult
# If the line was to a target to line up, reverse direction
if reversePoints:
leftAdj = -leftAdj
#Indicator.showFacePattern([0]*5)
# if leftAdj > 0:
# Indicator.showFacePattern([0, 0, 0, 2, 3])
# elif leftAdj < 0:
# Indicator.showFacePattern([3, 2, 0, 0, 0])
# if gGoalie:
# Indicator.setDefault()
# # DANGER CASE: Goalie Facing Backwards
# if gGoalie and (225 <= myh < 315):
# Indicator.showHeadColor(Indicator.RGB_ORANGE)
# #ball to goalie's right
# if ballx <= myx:
# print "Ball to my RIGHT"
# Action.walk(-5,Action.MAX_LEFT,-Action.MAX_TURN,
# "ddd",minorWalkType=Action.SkeFastForwardMWT)
# #ball to goalie's left
# else:
# print "Ball to my LEFT"
# Action.walk(-5,-Action.MAX_LEFT,Action.MAX_TURN,
# "ddd",minorWalkType=Action.SkeFastForwardMWT)
# Walk direct - with getBehind adjustment
if abs(ballH) < 30:
# if gGoalie:
# Indicator.showHeadColor(Indicator.RGB_GREEN)
Action.walk(fwd, leftAdj, ballH,
minorWalkType=Action.SkeFastForwardMWT)
# Walk and turn to face
elif abs(ballH) < 80:
gLastTurnFrame = Global.frame
fwd = max(ballD * math.cos(ballH),0)
left = ballD * math.sin(ballH)
# if gGoalie:
# left = leftAdj
# Indicator.showHeadColor(Indicator.RGB_BLUE)
Action.walk(fwd,left,ballH,"ddd",minorWalkType=Action.SkeFastForwardMWT)
# Turn on the spot
else:
gLastTurnFrame = Global.frame
fwd = 0
left = 0
turnccw = hMath.CLIP(ballH,60)
# if gGoalie:
# fwd = -Action.MAX_FORWARD
#
# if abs(ballx - myx) > Constant.DOG_WIDTH * 2:
# left = (myx - ballx) * 0.7
# turnccw = 0
# Indicator.showHeadColor(Indicator.RGB_PURPLE)
Action.walk(fwd,left,turnccw,"ddd",minorWalkType=Action.SkeFastForwardMWT)
def findByHint():
x, y = hMath.polarToCart(*gHint)
Action.setHeadParams(x, Constant.BallDiameter, y, Action.HTAbs_xyz)
def dodgyDogTo(destX, destY, fixedDest = False, fixedDestVar = FIXED_DEST_VAR):
global gLastBestHeading, gNewHeading
global gBestHeadingInfluence, gLastBestHeadingInfluence
global gUseMinGap, gUseMinIntensity, gUseMinDist
global gSidestepCounter, gLeft
myPos = Global.selfLoc.getPos()
myHeading = Global.selfLoc.getHeading()
localDest = hMath.getLocalCoordinate(myPos[0], myPos[1], myHeading,
destX, destY)
localHead = hMath.cartToPolar(*localDest) - 90 # zero forward
distToDest = hMath.getDistanceBetween(myPos[0],myPos[1],destX,destY)
if fixedDest and distToDest < fixedDestVar:
return Constant.STATE_SUCCESS
# bestGap = (hLeft, hRight, hBest, gapsize) degrees
bestGap = VisionLink.getBestGap(localDest[0], localDest[1],
GAP_MAX_DIST, gUseMinDist, gUseMinGap,
gUseMinIntensity, Constant.OBS_USE_NONE)
if bestGap != None:
if Debug.dodgyDebug:
print "Dodgy: localxy_dest", localDest, "dist", distToDest
print "bestgap (left, right, best, size) =", bestGap
thisBestHeading = bestGap[2]
# Don't try to head more than x away from direct heading
if abs(localHead - thisBestHeading) > MAX_DODGY_DODGE:
if thisBestHeading > localHead:
thisBestHeading = localHead + MAX_DODGY_DODGE
elif thisBestHeading < localHead:
thisBestHeading = localHead - MAX_DODGY_DODGE
if gNewHeading:
gLastBestHeading = thisBestHeading
gNewHeading = False
bestHeading = thisBestHeading * gBestHeadingInfluence
bestHeading += gLastBestHeading * gLastBestHeadingInfluence
bestHeading /= gBestHeadingInfluence + gLastBestHeadingInfluence
gLastBestHeading = bestHeading
# Don't try to turn more than 30 in one go
bestHeading = hMath.CLIP(bestHeading, 30)
if Debug.dodgyDebug:
if VisionLink.ObstacleGPSValid():
print "dodgyDog: heading ", thisBestHeading, "->", \
bestHeading, " (GPS)"
else:
print "dodgyDog: heading ", thisBestHeading, "->", \
bestHeading, " (Local)"
if abs(bestHeading - Global.ballH) < 10:
Indicator.showFacePattern([0,2,2,2,0])
elif bestHeading < Global.ballH - 60:
Indicator.showFacePattern([3,0,0,0,0])
elif bestHeading < Global.ballH:
Indicator.showFacePattern([3,2,0,0,0])
elif bestHeading > Global.ballH + 60:
Indicator.showFacePattern([0,0,0,0,3])
elif bestHeading > Global.ballH:
Indicator.showFacePattern([0,0,0,2,3])
# Wag tail in direction of dodge.
tailH = Indicator.TAIL_H_CENTRED
if bestHeading > localHead + 10:
tailH = Indicator.TAIL_LEFT * 3/4
elif bestHeading < localHead - 10:
tailH = Indicator.TAIL_RIGHT * 3/4
Indicator.finalValues[Indicator.TailH] = tailH
#Indicator.finalValues[Indicator.TailV] = tailV
fwd = Action.MAX_FORWARD
turnccw = bestHeading
closeDist = CLOSE_DODGY_OBSTACLE_DIST
if closeDist > distToDest - MIN_DODGY_TARGET_DIST:
closeDist = distToDest - MIN_DODGY_TARGET_DIST
closePoint = hMath.polarToCart(closeDist, localHead)
closeObsIntensity = VisionLink.getNoObstacleBetween(0, 0,
int(closePoint[0]), int(closePoint[1]),
CLOSE_CORRIDOR_WIDTH, gUseMinDist,
gUseMinIntensity, Constant.OBS_USE_NONE)
if closeObsIntensity > MIN_OBS_IN_CORRIDOR:
if bestHeading > localHead:
gLeft = Action.MAX_LEFT
else:
gLeft = -Action.MAX_LEFT
#turnccw /= 2.0
gSidestepCounter = SIDESTEP_FOR
else:
if gSidestepCounter > 0:
gSidestepCounter -= 1
else:
gLeft = 0
if Debug.dodgyDebug and gLeft != 0:
if bestHeading > localHead:
print "sDodgyDog: Sidestepping LEFT (", gSidestepCounter, ")"
else:
print "sDodgyDog: Sidestepping RIGHT (", gSidestepCounter, ")"
Action.walk(fwd, gLeft, turnccw, minorWalkType=Action.SkeFastForwardMWT)
return Constant.STATE_EXECUTING
else:
if Debug.dodgyDebug:
print "getBestGap failed - can't be dodgy"
return Constant.STATE_FAILED
def dodgyDogAlongHeading(heading):
# Pick a point out on the heading and dodgy to that point
heading = hMath.local2GlobalHeading(Global.selfLoc, heading)
pos = hMath.polarToCart(100, heading)
return dodgyDogTo(pos[0], pos[1], False)
def shouldIBeDodgyAlongHeading(heading):
# Pick a point out on the heading and dodgy to that point
heading = hMath.local2GlobalHeading(Global.selfLoc, heading)
pos = hMath.polarToCart(100, heading)
#print "shouldIBeDodgyAlongHeading", heading, pos
return shouldIBeDodgy(pos[0], pos[1], False)
def walkToStillBall(ballD, ballH, getBehind=False):
# Calculate getBehind adjustment
leftAdj = 0
if getBehind:
myx, myy = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
ballx, bally = hMath.getGlobalCoordinate(
myx, myy, myh, *hMath.polarToCart(ballD, ballH + 90))
# In defense we get between our goal and the ball. In attack we line
# up the ball and target goal. In the (small) midfield we line up with
# upfield
reversePoints = False
if bally < Constant.FIELD_LENGTH / 2 - 20:
lineX, lineY = Constant.OWN_GOAL_X, Constant.OWN_GOAL_Y
elif bally > Constant.FIELD_LENGTH / 2 + 20:
lineX, lineY = Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y
reversePoints = True
else:
lineX, lineY = ballx, 0
# Draw a line between ball and own goal. Find self offset from the line
dist = ((ballx - lineX)*(lineY - myy)-(lineX - myx)*(bally - lineY)) /\
math.sqrt(hMath.SQUARE(ballx - lineX) \
+ hMath.SQUARE(bally - lineY))
# If dist > 0 we are to the right of the line, so adjustment left
# These adjustments will only be applied assuming we are facing
# the ball
if dist > 15:
leftAdj = 14
elif dist < -15:
leftAdj = -12
else:
leftAdj = 0
# From in front of the ball adjust even more to get more behind
if myy > bally:
leftAdj *= 1.5
# If the line was to a target to line up, reverse direction
if reversePoints:
leftAdj = -leftAdj
Indicator.showFacePattern([0] * 5)
if ballD < MID_BALL_DIST and abs(ballH) > 30:
# Turn on the spot
ballH = hMath.CLIP(ballH, 70)
Action.walk(0, 0, ballH, minorWalkType=Action.SkeFastForwardMWT)
else:
# Walk direct - with getBehind adjustment
if leftAdj > 0:
Indicator.showFacePattern([0, 0, 0, 2, 3])
elif leftAdj < 0:
Indicator.showFacePattern([3, 2, 0, 0, 0])
if abs(ballH) < 30:
Action.walk(Action.MAX_FORWARD,
leftAdj,
ballH,
minorWalkType=Action.SkeFastForwardMWT)
else:
ballH = hMath.CLIP(ballH, 70)
Action.walk(0, 0, ballH, minorWalkType=Action.SkeFastForwardMWT)
"""