def selectKickInLeftEdge():
selfH2BallH = hMath.normalizeAngle_0_360(selfH + ballH)
ballH2TGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y)
selfH2TGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2TGoalH)
ballH2CenterH = hMath.getHeadingBetween(ballX,ballY,\
Constant.FIELD_WIDTH/2.0,ballY+200)
selfH2CenterH = hMath.normalizeAngle_180(selfH2BallH - ballH2CenterH)
# We are safe to do something...
if 0 <= selfH2BallH <= 135:
#
if 45 <= abs(selfH2TGoalH) <= 60:
if selfH2TGoalH > 0:
return (KT_UPENN_LEFT, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_UPENN_RIGHT, ballH2TGoalH, Constant.dAUTO)
#
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# Otherwise, if we just approach, the ball might go out.
# Hence, get-around-the-ball action.
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
def doGetToTargetPoint(targetX, targetY):
global gTargetPointReqAccuracy
selfX, selfY = Global.selfLoc.getX(), Global.selfLoc.getY()
ballX, ballY, ballH = Global.ballX, Global.ballY, Global.ballH
h = hMath.normalizeAngle_0_360(hMath.RAD2DEG(math.atan2(\
ballY - selfY, ballX - selfX)))
#angle = hMath.absAngleBetweenTwoPointsFromPivotPoint(ballX, ballY, \
# targetX, targetY, \
# selfX, selfY)
distSquared = hMath.getDistSquaredBetween(targetX,targetY,selfX,selfY)
## if dist > 100 and angle < 80:
## hTrack.saGoToTarget(targetX,targetY)
## else:
## hTrack.saGoToTargetFacingHeading(targetX,targetY,h)
hTrack.saGoToTargetFacingHeading(targetX,targetY,h)
# Hysterisis for whether or not your at the defender point.
if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy):
gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE
if abs(ballH) < 15:
Action.stopLegs()
else:
gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE
if abs(targetX - selfX) < 30 and abs(targetY - selfY) < 100:
checkThenBlock()
def supportBall():
global gTargetPointReqAccuracy
targetX, targetY, rangeX = hWhere.getStrikerPos()
targetX, targetY = getAdjustedTarget(targetX, targetY, rangeX)
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
ballH = Global.ballH
h = hMath.normalizeAngle_0_360(selfH + ballH)
distSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY)
# From outside a metre walk fast, else turn to face ball
#if dist > 100:
# hTrack.saGoToTarget(targetX, targetY)
#else:
# hTrack.saGoToTargetFacingHeading(targetX, targetY, h)
hTrack.saGoToTargetFacingHeading(targetX, targetY, h)
# Hysterisis for whether or not you are at the striker point.
if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy):
gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE
if abs(ballH) < 5:
Action.stopLegs()
else:
gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE
return targetX, targetY
def supportBall():
global gTargetPointReqAccuracy
targetX, targetY, rangeX = hWhere.getSupporterPos(False)
targetX, targetY = getAdjustedTarget(targetX, targetY, rangeX)
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
ballH = Global.ballH
h = hMath.normalizeAngle_0_360(selfH + ballH)
distSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY)
# From outside a metre walk fast, else turn to face ball
#if dist > 100:
# hTrack.saGoToTarget(targetX, targetY)
#else:
# hTrack.saGoToTargetFacingHeading(targetX, targetY, h)
hTrack.saGoToTargetFacingHeading(targetX, targetY, h)
# Hysterisis for whether or not you are at the supporter point.
if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy):
gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE
if abs(ballH) < 5:
Action.stopLegs()
if 0 <= Global.selfLoc.getHeading(
) <= 180 and not Global.vOGoal.isVisible():
# only block if my heading is right and I cannot see my own goal
sBlock.checkThenBlock(minBallSpeed=2, bothSides=True)
else:
gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE
return targetX, targetY
def isInRange(min_val, max_val):
realmin = hMath.normalizeAngle_0_360(min_val)
realmax = hMath.normalizeAngle_0_360(max_val)
if realmax < realmin:
realmax = realmax + 360
head = Global.selfLoc.getHeading()
if head < realmin:
realmin = realmin - 360
realmax = realmax - 360
if head > realmax:
realmin = realmin + 360
realmax = realmax + 360
return min_val <= head and head <= max_val
def setGetBehind(dkd):
global gTargetAngle
global gIsGetBehindNeeded
if gUseHeadLeft:
gTargetAngle = dkd + HEAD_OFFSET_ANGLE
else:
gTargetAngle = dkd - HEAD_OFFSET_ANGLE
gTargetAngle = hMath.normalizeAngle_0_360(gTargetAngle)
diff = hMath.normalizeAngle_0_360(Global.selfLoc.getHeading() -
gTargetAngle)
if abs(diff) < 20:
gIsGetBehindNeeded = False
elif 0 <= Global.selfLoc.getHeading() <= 180:
gIsGetBehindNeeded = False
else:
gIsGetBehindNeeded = True
def DecideNextAction():
global STRIKER_ACC_SMALL_CIRCLE, STRIKER_ACC_LARGE_CIRCLE, strikerPointReqAccuracy
global strikerPtX, strikerPtY
Global.myRole = Constant.WINGER
selfX, selfY = Global.selfLoc.getPos()
if (strikerPtX is None or strikerPtY is None):
strikerPtX, strikerPtY = getStrikerPointXPositioning()
##~ print "ball position : myx: %.2f myy: %.2f" % (Global.ballX,Global.ballY)
##~ print "striker point : strikeX: %.2f strikeY: %.2f" % (strikerPtX,strikerPtY)
##~ print ""
h = HelpMath.normalizeAngle_0_360(HelpMath.RAD2DEG \
(math.atan2(Global.ballY - selfY, Global.ballX - selfX)))
##~ print "==========="
##~ print "heading is %.2f" % (h)
##~ print "myloc %.2f, %.2f" % Global.selfLoc.getPos()
##~ print "ballLoc %.2f, %.2f" % (Global.ballX,Global.ballY)
# If facing away the ball, then dont. Use the slower saGoToTargetFacingHeading
angleBetweenStrikerPtBall = HelpMath.absAngleBetweenTwoPointsFromPivotPoint \
(Global.ballX,Global.ballY,strikerPtX,strikerPtY,selfX,selfY)
if (angleBetweenStrikerPtBall > 90):
HelpTrack.saGoToTargetFacingHeading(strikerPtX, strikerPtY, h)
# Otherwsie if you can see the ball constantly, use the fast way
else:
HoverToBall.saGoToTargetFacingHeading(strikerPtX, strikerPtY, h)
# Hysterisis for whether or not your at the striker point.
dist = math.sqrt(HelpMath.SQUARE(strikerPtX - selfX) \
+ HelpMath.SQUARE(strikerPtY - selfY))
##~ print "StrikeX: %f" % strikerPtX
##~ print "StrikeY: %f" % strikerPtY
##~ print "Distance: %f" % dist
##~ print "StrikerPointReqAccuracy: %f" % strikerPointReqAccuracy
if dist <= strikerPointReqAccuracy:
strikerPointReqAccuracy = STRIKER_ACC_LARGE_CIRCLE
Action.walk(0, 0, None, None)
else:
strikerPointReqAccuracy = STRIKER_ACC_SMALL_CIRCLE
def doGetToTargetPoint():
global gTargetPointReqAccuracy
targetX, targetY = hWhere.getWingerPos()
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
ballH = Global.ballH
h = hMath.normalizeAngle_0_360(selfH + ballH)
hTrack.saGoToTargetFacingHeading(targetX, targetY, h)
# Hysterisis for whether or not your at the striker point.
distSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY)
if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy):
gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE
if abs(ballH) < 5:
Action.stopLegs()
else:
gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE
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 getOutOfEveryonesWay():
global gGetOutOfTheShotCounter
global gGetOutOfTheAttackerCounter
# global gGetAwayFromTheBallCounter
global gGetAwayFromTheAttackerCounter
# ---------------------------------------------------------------
# Work out who is in what role. We cater for at most one attacker
attacker = None
for i in Global.otherValidForwards:
mate = Global.teamPlayers[i]
mateLoc = Global.teammatesLoc[i]
if mate.isAttacker():
attacker = (mate, mateLoc)
# ---------------------------------------------------------------
# Continue any already-running get-out-of-the-ways
targetH = hMath.normalizeAngle_0_360(Global.selfLoc.getHeading()\
+ Global.ballH)
# Away from the goal-shoot line
if gGetOutOfTheShotCounter > 0:
r = sGetOutOfTheWay.perform(Global.ballX,Global.ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y,\
targetH,True)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheShotCounter -= 1
return True
gGetOutOfTheShotCounter = 0
# Away from the attacker->ball line
if attacker != None and gGetOutOfTheAttackerCounter > 0:
r = sGetOutOfTheWay.perform(Global.ballX, Global.ballY,\
attacker[1].getX(), attacker[1].getY(),\
targetH,True)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheAttackerCounter -= 1
return True
gGetOutOfTheAttackerCounter = 0
else:
gGetOutOfTheAttackerCounter = 0
# if gGetAwayFromTheBallCounter > 0: # Away from the ball
# r = sGetOutOfTheWay.getOutOfTheCircle(Global.ballX, Global.ballY, \
# targetH, True)
# if r == Constant.STATE_EXECUTING:
# gGetAwayFromTheBallCounter -= 1
# return True
# gGetAwayFromTheBallCounter = 0
# Away from the attacker robot
if attacker != None and gGetAwayFromTheAttackerCounter > 0:
r = sGetOutOfTheWay.getOutOfTheCircle(attacker[1].getX(),\
attacker[1].getY(), targetH, True)
if r == Constant.STATE_EXECUTING:
gGetAwayFromTheAttackerCounter -= 1
return True
gGetAwayFromTheAttackerCounter = 0
else:
gGetAwayFromTheAttackerCounter = 0
#------- Check for any get-out-of-the-way I should start ----------------
# Get away from between ball and goal if attacker close to ball or the line
# is short
if attacker != None\
and (attacker[0].getTimeToReachBall() < 1500\
or hMath.getDistSquaredBetween(Global.ballX, Global.ballY,\
Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y) < hMath.SQUARE(150))\
and not (Global.ballSource == Constant.GPS_BALL and Global.lostBall >= Constant.LOST_BALL_GPS):
r = sGetOutOfTheWay.perform(Global.ballX,Global.ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y,\
targetH)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheShotCounter = GET_OUT_DURATION
return True
# Get out from between the attacker and the ball
if attacker != None and attacker[0].getTimeToReachBall() < 3000\
and not (Global.ballSource == Constant.GPS_BALL and Global.lostBall >= Constant.LOST_BALL_GPS):
r = sGetOutOfTheWay.perform(Global.ballX, Global.ballY,\
attacker[1].getX(), attacker[1].getY(),\
targetH)
if r == Constant.STATE_EXECUTING:
gGetOutOfTheAttackerCounter = GET_OUT_DURATION
return True
# Stay clear of the attacker robot
if attacker != None:
r = sGetOutOfTheWay.getOutOfTheCircle(attacker[1].getX(), \
attacker[1].getY(),\
targetH, radius = 30)
if r == Constant.STATE_EXECUTING:
gGetAwayFromTheAttackerCounter = GET_OUT_DURATION
return True
# Get away from the ball
# if not (Global.ballSource == Constant.GPS_BALL and Global.lostBall >= Constant.LOST_BALL_GPS):
# r = sGetOutOfTheWay.getOutOfTheCircle(Global.ballX, Global.ballY, targetH)
# if r == Constant.STATE_EXECUTING:
# gGetAwayFromTheBallCounter = GET_OUT_DURATION
# return True
# All clear
return False
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 doAvoidGoalBox(ownGoalBox=True):
headToBall = hMath.normalizeAngle_0_360(Global.ballH + \
Global.selfLoc.getHeading())
ballH2OGoalH = hMath.getHeadingBetween(Global.ballX,Global.ballY,\
Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y)
selfH2OGoalH = hMath.normalizeAngle_180(headToBall - ballH2OGoalH)
xOffset = 50
if (Global.selfLoc.getX() > Constant.FIELD_WIDTH/2 \
and Global.ballX < Constant.FIELD_WIDTH/2) \
or (Global.selfLoc.getX() < Constant.FIELD_WIDTH/2 \
and Global.ballX > Constant.FIELD_WIDTH/2):
xOffset += abs(Global.ballX - Constant.FIELD_WIDTH / 2) / 2
# Far away? Just go as normal
if not hWhere.inGoalBox(Global.selfLoc.getX(), Global.selfLoc.getY(),
ownGoalBox, Constant.GOALBOX_WIDTH):
doTrackBall()
# If next to the goal box walk to the corner of it
elif ownGoalBox and Global.selfLoc.getY() < Constant.GOALBOX_DEPTH - 10:
if Global.selfLoc.getX() < Constant.FIELD_WIDTH / 2:
hTrack.saGoToTargetFacingHeading(Constant.MIN_GOALBOX_EDGE_X,
Constant.OWN_GOALBOX_EDGE_Y + 25,
headToBall)
else:
hTrack.saGoToTargetFacingHeading(Constant.MAX_GOALBOX_EDGE_X,
Constant.OWN_GOALBOX_EDGE_Y + 25,
headToBall)
elif (not ownGoalBox
) and Global.selfLoc.getY() > Constant.TOP_GOALBOX_EDGE_Y + 10:
if Global.selfLoc.getX() < Constant.FIELD_WIDTH / 2:
hTrack.saGoToTargetFacingHeading(Constant.MIN_GOALBOX_EDGE_X,
Constant.TOP_GOALBOX_EDGE_Y - 25,
headToBall)
else:
hTrack.saGoToTargetFacingHeading(Constant.MAX_GOALBOX_EDGE_X,
Constant.TOP_GOALBOX_EDGE_Y - 25,
headToBall)
# Else if we are in front of goalbox walk to line up ouselves with
# the ball and goal
elif ownGoalBox:
# Move to the side of the ball, so we can localise as well.
# May need hysterisis here... not to switch sides frequently.
if selfH2OGoalH < 0:
adjX = xOffset
else:
adjX = -xOffset
hTrack.saGoToTargetFacingHeading(Global.ballX + adjX,
Constant.OWN_GOALBOX_EDGE_Y + 20,
headToBall)
elif not ownGoalBox:
# Move to the side of the ball, so we can localise as well.
# May need hysterisis here... not to switch sides frequently.
if selfH2OGoalH < 0:
adjX = xOffset
else:
adjX = -xOffset
hTrack.saGoToTargetFacingHeading(Global.ballX + adjX,
Constant.TOP_GOALBOX_EDGE_Y - 20,
headToBall)
def performBall(dkd=90, dist=20, direction=None, accuracy=20):
global gLastCalledFrame
global gDirection
global gLastCalledFrame
bx, by = Global.ballX, Global.ballY
myx, myy = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
# If this function wasn't called in previous frame, then reset the direction.
if Global.frame == gLastCalledFrame:
gDirection = None
gLastCalledFrame = Global.frame
# Work out if we are to the left or right of the ball (relative to DKD as 0)
lRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
if abs(lRobotH) > 70:
gDirection = None
if direction == None and gDirection == None:
if lRobotH < 0: # robot to the left
#print "robot to left of ball",
gDirection = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
gDirection = Constant.dCLOCKWISE
elif direction != None:
gDirection = direction
if gDirection == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
# This factor is used to adjust the dodgyness of the sidewards and
factor = max(min(abs(180 - lRobotH) / 90.0, 1), 0)
lcx = math.cos(hMath.DEG2RAD(circleAng)) * dist * factor
lcy = math.sin(hMath.DEG2RAD(circleAng)) * dist * factor
robotH = hMath.normalizeAngle_0_360(lRobotH + dkd) # now global
cx, cy = hMath.getGlobalCoordinate(bx, by, robotH, lcx, lcy)
bh = Global.ballH
lcx, lcy = hMath.getLocalCoordinate(myx, myy, myh, cx, cy)
if abs(bh) > 30 and Global.ballD > dist:
Action.walk(0, 0, bh, minorWalkType=Action.SkeFastForwardMWT)
else:
Action.walk(lcy,
lcx,
bh * 0.75,
"ddd",
minorWalkType=Action.SkeFastForwardMWT)
if abs(hMath.normalizeAngle_180(myh + bh -
dkd)) < accuracy and abs(bh) < accuracy:
resetPerform()
return Constant.STATE_SUCCESS
gLastCalledFrame = Global.frame
return Constant.STATE_EXECUTING
def continue_(desiredBH):
global lastDBH, currentBopMode, cornerCut
lastDBH = desiredBH
box = (Global.selfLoc.getPos()[1] < ((Constant.GOALBOX_DEPTH) + 25)) \
or (((currentBopMode == Constant.AVOIDBOX_BOP) \
or (currentBopMode == Constant.POSTAVOID_BOP)) \
and (Global.selfLoc.getPos()[1] < ((Constant.GOALBOX_DEPTH) + 50)))
# Alex Osaka - don't do goal box avoidance but we quit the bird earlier
box = False
#if box:
# Global.myRole = Constant.DEFENDERGOALBOX
# Global coordinate of where exactly I am heading at.
desiredHeading = hMath.normalizeAngle_0_360(Global.selfLoc.getHeading() + \
(targetH - desiredBH))
# Move with goalbox avoidance
if (box and (desiredBH > 0) and (Global.selfLoc.getPos()[0] < \
((Constant.FIELD_WIDTH + Constant.GOALBOX_WIDTH) / 2.0) - cornerCut) \
and (desiredHeading > 180) and (desiredHeading < (350))):
print "box avoid 1, desiredHeading =", desiredHeading
currentBopMode = Constant.AVOIDBOX_BOP
px = ((Constant.FIELD_WIDTH + Constant.GOALBOX_WIDTH) / 2.0) \
+ boxPointOffset
py = (Constant.GOALBOX_DEPTH) + boxPointOffset
phead = hMath.normalizeAngle_180(hMath.RAD2DEG(math.atan2(py \
- Global.selfLoc.getPos()[1], \
px - Global.selfLoc.getPos()[0])) - \
Global.selfLoc.getHeading())
Action.walk(Action.MAX_FORWARD, 0, hMath.CLIP(phead / 2.0, maxTurn), \
minorWalkType=Action.SkeFastForwardMWT)
return
elif (box and (desiredBH < 0) and (Global.selfLoc.getPos()[0] > \
((Constant.FIELD_WIDTH - Constant.GOALBOX_WIDTH) / 2.0) + cornerCut) \
and (desiredHeading > 190)):
print "box avoid 2, desiredHeading =", desiredHeading
currentBopMode = Constant.AVOIDBOX_BOP
px = ((Constant.FIELD_WIDTH - Constant.GOALBOX_WIDTH) / 2.0) - \
boxPointOffset
py = (Constant.GOALBOX_DEPTH) + boxPointOffset
phead = hMath.normalizeAngle_180(hMath.RAD2DEG(math.atan2(py - \
Global.selfLoc.getPos()[1],\
px - Global.selfLoc.getPos()[0])) - Global.selfLoc.getHeading())
Action.walk(Action.MAX_FORWARD, 0, hMath.CLIP(phead / 2.0, maxTurn), \
minorWalkType=Action.SkeFastForwardMWT)
return
elif (box and ((currentBopMode == Constant.AVOIDBOX_BOP) or \
(currentBopMode == Constant.POSTAVOID_BOP))):
currentBopMode = Constant.POSTAVOID_BOP
relh = targetH - desiredBH
if abs(relh) < 10:
currentBopMode = Constant.NORMAL_BOP
else:
Action.walk(Action.MAX_FORWARD,
0,
hMath.CLIP(relh / 2.0, maxTurn),
minorWalkType=Action.SkeFastForwardMWT)
if Debug.defenderDebug:
print "!!!!!"
return
else:
currentBopMode = Constant.NORMAL_BOP
if Debug.defenderDebug:
print "Before hoverToBall - currentBopMode: %f" % currentBopMode
# Hover To Ball
#relH = Global.ballH - desiredBH
relH = targetH - desiredBH
if Debug.defenderDebug:
print "relH: %f" % relH
#Global.ballH = relH # ARGH I HATE LAST YEAR'S TEAM! SERIOUSLY, WTF?!?!
# I agree! =p
distSquared = True
sHoverToBall.perform(targetDSquared, relH, distSquared)
if gUseDodgyDog and sDodgyDog.shouldIBeDodgyAlongHeading(relH):
sDodgyDog.dodgyDogAlongHeading(relH)
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 selectKickInOffensiveThird():
tGoalY = Constant.TARGET_GOAL_Y + 20
# If the ball's y is somehow futher away than Constant.TARGET_GOAL_Y,
# then hack the target goal y.
if ballY > tGoalY:
tGoalY = ballY + 50
selfH2BallH = hMath.normalizeAngle_0_360(selfH + ballH)
ballH2TGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.TARGET_GOAL_X,tGoalY)
selfH2TGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2TGoalH)
ballH2CrossingH = hMath.getHeadingBetween(ballX,ballY,\
Constant.TARGET_GOAL_X,tGoalY-40)
selfH2CrossingH = hMath.normalizeAngle_180(selfH2BallH - ballH2CrossingH)
ballH2CenterH = hMath.getHeadingBetween(ballX,ballY,\
Constant.FIELD_WIDTH/2.0,ballY+200)
selfH2CenterH = hMath.normalizeAngle_180(selfH2BallH - ballH2CenterH)
if sGrab.isGrabbed:
if hWhere.ballInTriTLCorner()\
or hWhere.ballInTriTRCorner():
return (KT_GRAB_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
elif hWhere.ballOnLEdge(100)\
or hWhere.ballOnREdge(100):
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
# If we are stuck in offensive area, do something!!!
# FIXME : smart strategy
elif hStuck.gUseContestDetect and hStuck.isBallContested():
dkd = ballH2TGoalH
if hWhere.ballInTriTLCorner() or hWhere.ballInTriTRCorner():
dkd = ballH2CrossingH
selfH2dkd = hMath.normalizeAngle_180(selfH2BallH - dkd)
if 40 <= abs(selfH2dkd) < 75:
if selfH2dkd < 0:
return (KT_UPENN_RIGHT, dkd, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, dkd, Constant.dAUTO)
elif (ballD < sGrab.CLOSE_DIST or Global.frame - sGrab.gLastApproachFrame < 5)\
and (hWhere.isOnLEdge(EDGE_DIST)
or hWhere.isOnREdge(EDGE_DIST)
or hWhere.ballInTriTLCorner()
or hWhere.ballInTriTRCorner()
or selfH >= 180):
return (KT_GRAB_DRIBBLE, dkd, Constant.dAUTO)
else:
return (KT_DRIBBLE, dkd, Constant.dAUTO)
# If the ball is in the corner, kick it across the field.
elif hWhere.ballInTriTLCorner():
if abs(selfH2CrossingH) < 10:
return (KT_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
elif 45 <= abs(selfH2CrossingH) < 60:
if selfH2CrossingH < 0:
return (KT_UPENN_RIGHT, ballH2CrossingH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2CrossingH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
# If there's already a supporter waiting, then TURN KICK????
# If the ball is in the left edge.
elif hWhere.ballOnLEdge(100):
if hStuck.isBallContested():
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the ball is in the corner, kick it across the field.
elif hWhere.ballInTriTRCorner():
if abs(selfH2CrossingH) < 10:
return (KT_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
elif 45 <= abs(selfH2CrossingH) < 60:
if selfH2CrossingH < 0:
return (KT_UPENN_RIGHT, ballH2CrossingH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2CrossingH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
# If there's already a supporter waiting, then TURN KICK????
# If the ball is in the right edge.
elif hWhere.ballOnREdge(100):
if hStuck.isBallContested():
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the ball is really close to the goal, dribble
elif not sGrab.isGrabbed\
and ballY > (Constant.FIELD_LENGTH - 30)\
and ballX > (Constant.FIELD_WIDTH/2 - Constant.GOAL_WIDTH/2 + 15)\
and ballX < (Constant.FIELD_WIDTH/2 + Constant.GOAL_WIDTH/2 - 15)\
and abs(selfH2TGoalH) < 45:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the ball is really close to the goal, grab dribble into goal
elif ballY > (Constant.FIELD_LENGTH - Constant.GOALBOX_DEPTH)\
and ballX > (Constant.FIELD_WIDTH/2 - Constant.GOAL_WIDTH/2 - 50)\
and ballX < (Constant.FIELD_WIDTH/2 + Constant.GOAL_WIDTH/2 + 50):
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
elif hStuck.isBallContested():
if sGrab.isGrabbed:
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
elif abs(selfH2TGoalH) < 40:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the robot is a little far, then dribble
elif ballY < Constant.FIELD_LENGTH * 0.65 and abs(selfH2TGoalH) < 15:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
def selectKickInMiddleThird():
global gLastAvoidOwnGoalFrame
selfH2BallH = hMath.normalizeAngle_0_360(selfH + ballH)
ballH2OGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y)
selfH2OGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2OGoalH)
ballH2TGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y-50)
selfH2TGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2TGoalH)
ballH2CenterH = hMath.getHeadingBetween(ballX,ballY,\
Constant.FIELD_WIDTH/2.0,ballY+200)
selfH2CenterH = hMath.normalizeAngle_180(selfH2BallH - ballH2CenterH)
if sGrab.isGrabbed:
if hWhere.ballOnLEdge() or hWhere.ballOnREdge():
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
elif abs(selfH2OGoalH) < 40 or Global.vOGoal.isVisible():
gLastAvoidOwnGoalFrame = Global.frame
return (KT_AVOID_OWN_GOAL, ballH2TGoalH, Constant.dAUTO)
# If we are stuck in midfield area, do something!!!
# FIXME : smart strategy
elif hStuck.gUseContestDetect and hStuck.isBallContested():
if 45 <= abs(selfH2TGoalH) < 60:
if selfH2TGoalH < 0:
return (KT_UPENN_RIGHT, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2TGoalH, Constant.dAUTO)
elif (ballD < sGrab.CLOSE_DIST or Global.frame - sGrab.gLastApproachFrame < 5)\
and ((hWhere.isOnLEdge(EDGE_DIST) and abs(selfH2TGoalH) > 10)
or (hWhere.isOnREdge(EDGE_DIST) and abs(selfH2TGoalH) > 10)
or selfH >= 180):
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the ball is in the left edge.
elif hWhere.ballOnLEdge():
return selectKickInLeftEdge()
# If the ball is in the right edge.
elif hWhere.ballOnREdge():
return selectKickInRightEdge()
# If the ball is not in the edges of the field
else:
if 45 <= abs(selfH2TGoalH) < 60:
if selfH2TGoalH < 0:
return (KT_UPENN_RIGHT, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2TGoalH, Constant.dAUTO)
elif abs(selfH2TGoalH) < 90:
if abs(selfH2TGoalH) < 10:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
def selectKickInDefensiveThird():
global gLastAvoidOwnGoalFrame
selfH2BallH = hMath.normalizeAngle_0_360(selfH + ballH)
ballH2OGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y)
ballH2MinOGoalBoxH = hMath.getHeadingBetween(ballX,ballY,\
Constant.MIN_GOALBOX_EDGE_X,Constant.OWN_GOAL_Y)
ballH2MaxOGoalBoxH = hMath.getHeadingBetween(ballX,ballY,\
Constant.MAX_GOALBOX_EDGE_X,Constant.OWN_GOAL_Y)
selfH2OGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2OGoalH)
ballH2CenterH = hMath.getHeadingBetween(ballX,ballY,\
Constant.FIELD_WIDTH/2.0,ballY+200)
selfH2CenterH = hMath.normalizeAngle_180(selfH2BallH - ballH2CenterH)
if sGrab.isGrabbed:
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
elif abs(selfH2OGoalH) < 60\
or ballH2MinOGoalBoxH < selfH2BallH < ballH2MaxOGoalBoxH\
or Global.vOGoal.isVisible():
gLastAvoidOwnGoalFrame = Global.frame
return (KT_AVOID_OWN_GOAL, ballH2CenterH, Constant.dAUTO)
# If we are stuck in defensive area, do something!!!
elif hStuck.gUseContestDetect and hStuck.isBallContested():
if 45 <= abs(selfH2CenterH) < 60:
if selfH2CenterH < 0:
return (KT_UPENN_RIGHT, ballH2CenterH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2CenterH, Constant.dAUTO)
elif (ballD < sGrab.CLOSE_DIST or Global.frame - sGrab.gLastApproachFrame < 5)\
and ((hWhere.isOnLEdge(EDGE_DIST) and abs(selfH2CenterH) > 10)
or (hWhere.isOnREdge(EDGE_DIST) and abs(selfH2CenterH) > 10)
or selfH >= 180):
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
else:
return (KT_DRIBBLE, ballH2CenterH, Constant.dAUTO)
# If the ball is in the left edge.
elif hWhere.ballOnLEdge():
return selectKickInLeftEdge()
# If the ball is in the right edge.
elif hWhere.ballOnREdge():
return selectKickInRightEdge()
# If the ball is not in the edge of the field
else:
if 45 <= abs(selfH2CenterH) < 60:
if selfH2CenterH < 0:
return (KT_UPENN_RIGHT, ballH2CenterH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2CenterH, Constant.dAUTO)
elif abs(selfH2CenterH) < 90:
if abs(selfH2CenterH) < 20:
return (KT_DRIBBLE, ballH2CenterH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
# Otherwise we should do get behind??
# turn kick
else:
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
def perform(dkd=90, dist=20, direction=None, bx=None, by=None, accuracy=20):
global gDirection
#if side != None:
# print "Warning: sGetBehindBall.perform: side is not yet implemented"
if bx == None or by == None:
(bx, by) = Global.gpsGlobalBall.getPos()
(myx, myy) = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
# Work out if we are to the left or right of the ball (relative to DKD as 0)
lRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
if direction == None and gDirection == None:
if lRobotH < 0: # robot to the left
#print "robot to left of ball",
gDirection = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
gDirection = Constant.dCLOCKWISE
elif direction != None:
gDirection = direction
# The circling point can be calculated as looking from the ball
# towards the robot at CIRCLE_DEGREES to the left/right, and distance
# dist. CircleAng is from the ball facing the robot with positive x
# at zero degrees
# if robotH > 180 - CIRCLE_DEGREES:
# # If we are within CIRCLE_DEGREES of the target point don't overshoot
# circleAng = 90 + (180 - robotH)
# elif robotH < -180 + CIRCLE_DEGREES:
# circleAng = 90 - (-180 + robotH)
# else:
if gDirection == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
#print ""
#print "local robot H ", lRobotH
# relative to target facing robot
# This factor is used to adjust the dodgyness of the sidewards and
# backwards ability of fast skelliptical walk.
factor = 1 #max(min(abs(180-lRobotH)/60.0,2),1)
lcx = math.cos(hMath.DEG2RAD(circleAng)) * dist * factor
lcy = math.sin(hMath.DEG2RAD(circleAng)) * dist * factor
#print "circleAng", circleAng, "rel circle pos", circleRelX, circleRelY
robotH = hMath.normalizeAngle_0_360(lRobotH + dkd) # now global
cx, cy = hMath.getGlobalCoordinate(bx, by, robotH, lcx, lcy)
#print " local circle pos : (", lcx ,",",lcy,")"
#print "my pos : (", myx, ",", myy, ",",myh,")"
#print "global robotH ", robotH
#print "global ball :(", bx, ",", by, ") global circle pos : (", cx ,",",cy,")"
# circleX/Y now is the global coords of the circle point, so walk there.
bh = hMath.getHeadingBetween(myx, myy, bx, by) # global
lbh = hMath.normalizeAngle_180(bh - myh)
lcx, lcy = hMath.getLocalCoordinate(myx, myy, myh, cx, cy)
#lcdSquared = hMath.getDistSquaredBetween(0,0,lcx,lcy)
Action.walk(lcy, lcx, lbh, "ddd", minorWalkType=Action.SkeFastForwardMWT)
if abs(hMath.normalizeAngle_180(bh -
dkd)) < accuracy and abs(lbh) < accuracy:
resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def getDKDRange():
nearEdgeDist = 40
debugDKD = False
length = Constant.FIELD_LENGTH
width = Constant.FIELD_WIDTH
# Make sure determine ballSource is called
if Global.ballSource is None:
print "Warning: in getDKDRange() havent determined the ball source before getDKD()"
return None
X_ = Global.ballX
Y_ = Global.ballY
if debugDKD:
print "Ball Source: ", Global.ballSource, " X_ ", X_, " Y_ ", Y_
# If you don't know where the ball is determine dkd based on where you are.
if Global.ballSource == Constant.GPS_BALL\
and VisionLink.getGPSBallMaxVar() >= hMath.get95var(35):
X_, Y_ = Global.selfLoc.getPos()
if debugDKD:
print "Warning: in getDKDRange(): I dont trust GPS ball right now: use my own pos is X_ ", X_, " Y_ ", Y_
Y_ = hMath.CLIPTO(Y_, 0, length)
X_ = hMath.CLIPTO(X_, 0, width)
# If the ball is near one of the side edges and not in corners then the direction becomes up the field.
if Y_ > nearEdgeDist:
if X_ < nearEdgeDist: # on the left edge
if debugDKD:
print "On the left edge"
return (90, 70, 100, 10, 110)
elif X_ > (width - nearEdgeDist): #on the right edge
if debugDKD:
print "On the right edge"
return (90, 80, 110, 70, 170)
# upper 60%, can I use voak?
if Y_ > 0.6 * length:
# If target goal seen and is close and not many obstacles, it has a good heading, get the best heading like VOAK
if (Global.vTGoal.getConfidence() > 3
and Global.ballSource == Constant.VISION_BALL):
toY = length - Y_
toX = width / 2.0 - X_
xRange = Constant.GOAL_WIDTH / 2.0 + (toY *
math.tan(hMath.DEG2RAD(30)))
inVOAKcal = abs(toX) < xRange
if inVOAKcal:
(lmin, lmax, rmin, rmax) = VisionLink.getHeadingToBestGap()
lmin = hMath.normalizeAngle_0_360(lmin +
Global.selfLoc.getHeading())
lmax = hMath.normalizeAngle_0_360(lmax +
Global.selfLoc.getHeading())
rmin = hMath.normalizeAngle_0_360(rmin +
Global.selfLoc.getHeading())
rmax = hMath.normalizeAngle_0_360(rmax +
Global.selfLoc.getHeading())
if abs(lmax - lmin) > abs(rmax - rmin):
tdkdmin, tdkdmax = lmin, lmax
dkd = (lmin + lmax) / 2.0
else:
tdkdmin, tdkdmax = rmin, rmax
dkd = (rmin + rmax) / 2.0
if (dkd < 150 and dkd > 30):
return (dkd, tdkdmin, tdkdmax, 10, 170)
# Find the dkd fron the center of the target and own goals
dkdup = hMath.getHeadingBetween(X_, Y_, (width / 2.0), length)
dkdbottom = hMath.getHeadingBetween((width / 2.0), 0, X_, Y_)
# if i cant use voak, then the heading to the center of target goal is used as dkd
if Y_ > 0.6 * length:
tdkdmin = hMath.getHeadingBetween(X_, Y_, Constant.RIGHT_GOAL_POST,
length)
tdkdmax = hMath.getHeadingBetween(X_, Y_, Constant.LEFT_GOAL_POST,
length)
if debugDKD:
print "close to target goal, dkd directed to the target goal", Global.DKD[
0]
return (dkdup, tdkdmin, tdkdmax, 10, 170)
# if close to own goal, direted out of own goal
elif Y_ < 0.4 * length:
if debugDKD:
print "close to own goal, dkd directed out of the own goal"
return (dkdbottom, 20, 160, 10, 170)
# Now, the middle 20% of the field should have a continous change of direction.
else:
myy = Y_ - 0.4 * length
ratio = myy / (0.2 * length)
dkd = dkdup * ratio + dkdbottom * (1 - ratio)
if debugDKD:
print "middle 20%, linear interpolation"
return (dkd, 40, 140, 20, 160)
def calcHead(self):
self.calcTheta()
# principle() not ported, I think the one below is equivalent - dan.
# self.__head = principle(self.__theta - HALF_CIRCLE / 2);
self.__head = hMath.normalizeAngle_0_360(self.__theta - 90)
def kickOffKickToCorner():
global playCounter, whenGrabbed
global bestHeading, gBestHeading
# Warning: this set play freezes the attacker when it fails.
# Don't use it yet
(myX, myY) = Global.selfLoc.getPos()
myH = Global.selfLoc.getHeading()
if pReady.chosenFormation == 0:
(kickX, kickY) = (Constant.FIELD_WIDTH, Constant.FIELD_LENGTH - 80)
(suppX, suppY) = (kickX + 40, kickY - 40)
else:
(kickX, kickY) = (0, Constant.FIELD_LENGTH - 80)
(suppX, suppY) = (kickX - 40, kickY - 40)
if pReady.isCentre(Global.kickOffPos):
print "kickToCorner: whenGrabbed", whenGrabbed,
if playCounter > 120: # 6 sec, just in case
return False
if whenGrabbed == 0:
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(kickX, kickY)
if gap != None:
(gapLeft, gapRight, bestHeading, angle) = gap
bestHeading += hMath.getSign(5, bestHeading)
else:
bestHeading = hMath.getHeadingToAbsolute(
Global.selfLoc, kickX, kickY)
gBestHeading = hMath.local2GlobalHeading(
Global.selfLoc, bestHeading)
print "gap", bestHeading, gBestHeading,
print "grabbing"
if sGrab.perform() == Constant.STATE_FAILED:
print "Grab failed in set play. Done."
sGrab.resetPerform()
Global.myRole = Global.myLastRole = Constant.ATTACKER
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
# Keep updating gap, but not with shared obs
gap = getGapTo(kickX, kickY)
if gap != None:
(gapLeft, gapRight, bestHeading, angle) = gap
bestHeading += hMath.getSign(5, bestHeading)
gBestHeading = hMath.local2GlobalHeading(Global.selfLoc,
bestHeading)
print "New gap global", gBestHeading,
else:
print "No gap left - using last global", gBestHeading,
#bestHeading = 0 # Kick now
relH = gBestHeading - Global.selfLoc.getHeading()
print "relH", relH,
if abs(relH) >= 4 and (playCounter - whenGrabbed) < 55:
print "dribbling"
if sGrabDribble.perform(0, 0, relH) == Constant.STATE_FAILED:
print "dribble failed. Done."
Global.myRole = Global.myLastRole = Constant.ATTACKER
return False
return True
print "Kicking",
Action.kick(Action.SoftTapWT)
if Action.shouldIContinueKick():
Action.continueKick()
return True
sGrab.resetPerform()
print "success. Done."
return False
elif pReady.isForward(Global.kickOffPos):
Global.myRole = Global.myLastRole = Constant.ATTACKER
if playCounter < 60:
hTrack.saGoToTarget(suppX, suppY, 25)
hFWHead.DecideNextAction()
return True
elif playCounter < 100:
if hTrack.canSeeBall and Global.ballD < 30:
return False
h = hMath.normalizeAngle_0_360(myH + Global.ballH)
hTrack.saGoToTargetFacingHeading(suppX, suppY, h)
hFWHead.DecideNextAction()
return True
else:
rAttacker.perform()
return False
else:
return False
return False
