def getOutOfTheCircle(centerX, centerY, targetH, doForce=False, radius=40):
selfX, selfY = Global.selfLoc.getPos()
dist = hMath.getDistanceBetween(centerX, centerY, selfX, selfY)
heading = hMath.getHeadingBetween(centerX, centerY, selfX, selfY)
if amIInTheCircle(centerX,centerY,radius)\
or doForce:
if dist > radius:
relX = dist * 2 * math.cos(hMath.DEG2RAD(heading))
relY = dist * 2 * math.sin(hMath.DEG2RAD(heading))
else:
relX = radius * 2 * math.cos(hMath.DEG2RAD(heading))
relY = radius * 2 * math.sin(hMath.DEG2RAD(heading))
destX = centerX + relX
destY = centerY + relY
hTrack.saGoToTargetFacingHeading(destX, destY, targetH)
Indicator.showFacePattern([0, 2, 0, 2, 0])
return Constant.STATE_EXECUTING
return Constant.STATE_SUCCESS
def receiveKickOffPincer():
global playCounter
if playCounter > 85:
return False
if pReady.isCentre(Global.kickOffPos):
if playCounter < 65:
Global.myRole = Global.myLastRole = Constant.DEFENDER
if sBlock.checkThenBlock(True, 3.0, bothSides=True):
return
sFindBall.perform(True)
hTrack.saGoToTargetFacingHeading(Constant.FIELD_WIDTH / 2,
Constant.FIELD_LENGTH * 0.29,
90,
maxSpeed=20)
#rDefender.DecideNextAction()
#Action.walk(0.01, 0, 0) # don't go anywhere
return True
return False
# Wide players
hFWHead.DecideNextAction()
sFindBall.walkToBall(Global.ballD, Global.ballH)
return True
def 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.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 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 moveToMyPositionDirectly():
# Firstly get my kickoff position and return a fatal warning if there is none.
if Global.kickOffPos == None:
print "Warning: pReady.py - directPositionToMyposition() no kick off position?"
Debug.goAndMakeTraceBack()
return False
selfX = Global.selfLoc.getPos()[0]
selfY = Global.selfLoc.getPos()[1]
kickX = Global.kickOffPos[0]
kickY = Global.kickOffPos[1]
# Now I am sure I have the kickoff position, then I should run to it directly.
# Calculate the distance, heading to the kickoff position.
distance = hMath.getDistanceBetween(selfX, selfY, kickX, kickY)
heading = hMath.getHeadingToFaceAt(Global.selfLoc, kickX, kickY)
if Debug.directPosDebug:
print "Distance: %f" % distance
print "Heading: %f" % heading
# If the distance is small, do saGoToTargetFacingHeading(). With such small
# distance, better go and turn toward the target goal.
if distance <= 50:
if Debug.readyDebug:
print "Distance too small case!", Global.selfLoc.getPos(), \
Global.kickOffPos
hTrack.saGoToTargetFacingHeading(kickX, kickY, 90, 12, 50)
return False
# If the heading is too large, it would be better to stationary and turn.
if abs(heading) >= 30:
if Debug.readyDebug:
print "Heading too large case!", heading
Action.walk(0, 0, hMath.CLIP(heading, 60))
return False
# Should this or / and sign?
if not anyVisionRobotBlockingMyPath() and not anyGPSRobotBlockingMyPath():
if Debug.readyDebug:
print "No robot block my path, lets go as quickly as possible!"
Action.walk(Action.MAX_FORWARD, 0, 0)
return False
# Now the heading is not too large, draw a vector directly to the kickoff
# position.
Action.walk(Action.MAX_FORWARD, 0, 0)
return 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 findBallDefender():
time = Global.lostBall % TIMER_3
if time < TIMER_1:
turnccw = hMath.CLIP(Global.ballH,80)
Action.walk(0,0,turnccw, minorWalkType = Action.SkeFastForwardMWT)
hTrack.scan()
elif time < TIMER_2:
hFWHead.compulsoryAction = hFWHead.doNothing
sFindBall.findBySpin()
else:
targetH = Global.selfLoc.getHeading() + Global.ballH
hTrack.saGoToTargetFacingHeading(Global.selfLoc.getX(), \
Constant.FIELD_LENGTH * 0.25, targetH)
def findBallStriker():
time = Global.lostBall % TIMER_3
if time < TIMER_1:
turnccw = hMath.CLIP(Global.ballH, 80)
Action.walk(0, 0, turnccw, minorWalkType=Action.SkeFastForwardMWT)
hTrack.scan()
elif time < TIMER_2:
hFWHead.compulsoryAction = hFWHead.doNothing
sFindBall.findBySpin()
else:
targetH = Global.selfLoc.getHeading() + Global.ballH
targetX, targetY, _rangeX = hWhere.getStrikerPos(True)
hTrack.saGoToTargetFacingHeading(targetX, targetY, targetH)
def DecideNextAction():
import hFWHead
import hWhere
import Indicator
targetX, targetY = hWhere.getSupporterPos()
targetH = Global.selfLoc.getHeading() + Global.ballH
r = perform(Global.ballX,Global.ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y,\
targetH)
if r == Constant.STATE_EXECUTING:
Indicator.showHeadColor([True, True, True])
else:
Indicator.showHeadColor([False, False, False])
hTrack.saGoToTargetFacingHeading(targetX, targetY, targetH)
hFWHead.DecideNextAction()
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 perform(dkd = 90, side = None, bx = None, by = None):
# This implementation is very similar to sGetBehindBall (based on 2003)
# but the ball is on the bottom edge of the circle, not the centre.
global onCircle
if side != None:
print "Warning: sGetBesideBall.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()
# Determine the centre of the circle, which is CIRCLE_RADIUS towards
# dkd from the ball. Global coords.
cx = bx + math.cos(hMath.DEG2RAD(dkd)) * CIRCLE_RADIUS
cy = by + math.sin(hMath.DEG2RAD(dkd)) * CIRCLE_RADIUS
# If we are backward of the ball or really close just run at it
ballRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
ballDSquared = hMath.getDistSquaredBetween(myx, myy, bx, by)
if (abs(ballRobotH) > 90 or ballDSquared < hMath.SQUARE(20)):
Indicator.showHeadColor(Indicator.RGB_PURPLE)
ballH = hMath.getHeadingBetween(myx, myy, bx, by)
hTrack.saGoToTargetFacingHeading(bx, by, ballH)
return
# Work out if we are left or right of the centre (relative to DKD as 0)
robotH = hMath.getHeadingToMe(cx, cy, dkd, myx, myy)
# FIXME: allow choice of direction
if (robotH > 0): # robot to the left
#print "robot to left of ball",
direction = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
direction = Constant.dCLOCKWISE
# The circling point can be calculated as looking from the centre
# towards the robot at CIRCLE_DEGREES to the left/right, and distance
# CIRCLE_RADIUS. CircleAng is from the centre facing the robot with
# positive x at zero degrees.
# There are two modes here. In the first we are well outside the and
# running to make a tangent with the circle. In the second we are close
# to or inside the circle and tracing the circumference
centreDSquared = hMath.getDistSquaredBetween(myx, myy, cx, cy)
if (centreDSquared > hMath.SQUARE(CIRCLE_RADIUS + 20)):
#print "Outside circle, running to tangent"
onCircle = False
Indicator.showHeadColor(Indicator.RGB_GREEN)
if direction == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
else:
#print "On circle, tracing circumference"
onCircle = True
Indicator.showHeadColor(Indicator.RGB_YELLOW)
if direction == Constant.dANTICLOCKWISE:
circleAng = 110
else:
circleAng = 70
# print "me", int(myx), int(myy), "ball", int(bx), int(by), \
# "centre", int(cx), int(cy), "robotH", int(robotH),
# relative to centre facing robot
circleRelX = math.cos(hMath.DEG2RAD(circleAng)) * CIRCLE_RADIUS
circleRelY = math.sin(hMath.DEG2RAD(circleAng)) * CIRCLE_RADIUS
#print "circleAng", circleAng, "rel circle pos", circleRelX, circleRelY
robotH = hMath.normalizeAngle_180(robotH + dkd) # now global
(circleX, circleY) = hMath.getGlobalCoordinate(cx, cy, robotH, \
circleRelX, circleRelY)
# print "gRobotH", robotH, "circle pos", int(circleX), int(circleY)
# circleX/Y now is the global coords of the circle point, so walk there.
# ballH = hMath.getHeadingBetween(myx, myy, bx, by) # global
if onCircle:
# Calls the walk directly to ensure smoothness: no stopping to turn
relX = circleX - myx
relY = circleY - myy
relD = hMath.getLength((relX, relY))
relTheta = hMath.RAD2DEG(hMath.getHeadingToRelative(relX, relY))
# Don't turn outwards much, even if we are inside the circle. Walking
# forward will put us back on it. Nobu can you fix this?
# print "relTheta", relTheta, "=>",
# if direction == Constant.dANTICLOCKWISE and relTheta < 0:
# #relTheta = hMath.CLIP(relTheta, 15)
# relTheta = 0
# elif direction == Constant.dCLOCKWISE and relTheta > 0:
# #relTheta = hMath.CLIP(relTheta, 15)
# relTheta = 0
# print relTheta
left = 0
if abs(relTheta) < 30:
Action.walk(Action.MAX_FORWARD, left, relTheta)
else:
Action.walk(Action.MAX_FORWARD, left, relTheta)
else:
hTrack.saGoToTarget(circleX, circleY)
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 perform(fromX,
fromY,
toX,
toY,
targetH,
doForce=False,
getOutOfTheWayDist=60):
selfX, selfY = Global.selfLoc.getPos()
# If you are really further away from the "from" cords, then don't do anything.
if hMath.getDistSquaredBetween(selfX, selfY, fromX,
fromY) > hMath.SQUARE(250):
return Constant.STATE_SUCCESS
dist, head = amIInTheWay(fromX, fromY, toX, toY)
if dist != None\
and (doForce or abs(dist) < getOutOfTheWayDist):
rise = fromY - toY
run = fromX - toX
if rise == 0:
rise = 0.01
if run == 0:
run = 0.01
# gradient of the line between from and to
m1 = rise / (run + 0.0)
b1 = fromY - m1 * fromX
# perpendicular gradient to the line between from and to
m2 = -run / (rise + 0.0)
b2 = selfY - m2 * selfX
# on the right side
if dist > 0:
newX = selfX + 50
newY = m2 * newX + b2
# on the left side
else:
newX = selfX - 50
newY = m2 * newX + b2
# Adjust newY, so that the robot would move downwards.
temp = m1 * newX + b1
if newY > temp:
newY = selfY
else:
newY -= (50.0 / abs(getOutOfTheWayDist)) * abs(dist)
newX = max(newX, 0)
newX = min(newX, Constant.FIELD_WIDTH)
newY = max(newY, 0)
newY = min(newY, Constant.FIELD_LENGTH)
hTrack.saGoToTargetFacingHeading(newX, newY, targetH)
Indicator.showFacePattern([0, 0, 2, 0, 0])
return Constant.STATE_EXECUTING
else:
return Constant.STATE_SUCCESS
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 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
def doFindSpace():
hTrack.saGoToTargetFacingHeading(
centerPos[0], centerPos[1],
hMath.getHeadingBetween(pos[0], pos[1], Global.ballX, Global.ballY))
def perform(params=None):
global gLastFrameCalled
global gLastDecisionFunction
global pos, friendPos, isPlayerSet, friend, friendloc, centerPos
hTeam.sendWirelessInfo()
id(params) # ignore
if not isPlayerSet:
if Global.myPlayerNum == 3:
friend = Global.teamPlayers[1]
friendloc = Global.teammatesLoc[1]
centerPos = (Constant.FIELD_WIDTH / 2, Constant.GOALBOX_DEPTH)
else:
friend = Global.teamPlayers[2]
friendloc = Global.teammatesLoc[2]
centerPos = (Constant.FIELD_WIDTH / 2,
Constant.FIELD_LENGTH - Constant.GOALBOX_DEPTH)
isPlayerSet = True
print "Player no:", Global.myPlayerNum
friendPos = friendloc.getPos()
pos = Global.selfLoc.getPos()
#if friend grab, go to center position, while looking at obstacle, get facing ball.
#if see ball far away, get behind ball (don't pass half the field) while scanning for ball and obstacle.
#if see ball < 1/2 field, chase and grab, turn no obstacle, until facing friend or gap upfield. and kick if obstacle near, if not stop realease, and scan look for obstacle and ball.
#if got wirelessball get behind wireless ball on keep a distance with obstacle, scan.
#if doesn't see and doesn't have wireless ball, find ball and goto base spot to spin again.
calculateObstacleBox()
shouldIBeDodgy = gUseDodgyDog
if gLastFrameCalled != Global.frame - 1:
resetPerform()
gLastFrameCalled = Global.frame
# elif shouldIKick():
# #shouldIBeDodgy = False #Really?
# doKick()
if (sGrab.isGrabbed):
print "friend is in :", friendPos[0], " ", friendPos[1]
r = scGrabDribble.performToPosition((friendPos[0], friendPos[1]))
# if r == Constant.STATE_EXECUTING:
# return
# else:
# scGrabDribble.resetPerform()
#align to friend gps, while avoiding the obstacle.
#and the pass to him.
elif (sGrab.grabbingCount > 0):
#print "grabbing"
r = sGrab.perform(doGetBehindv=sFindBall.GET_BEHIND_NONE)
if r == Constant.STATE_EXECUTING:
return
elif r == Constant.STATE_FAILED:
sGrab.resetPerform()
elif shouldIFindSpace():
#print "Find space"
doFindSpace()
elif not shouldIEndActivelyLocalise()\
or shouldIActivelyLocalise():
#print "active loc"
doActivelyLocalise()
elif (Global.vBall.isVisible() or Global.lostBall < Constant.LOST_BALL_GPS
or friend.hasSeenBall()):
#print "see a ball"
#if (friend.getTimeToReachBall() < selfobj.getTimeToReachBall() + 1000):
if (centerPos[1] < Constant.FIELD_LENGTH / 2):
isOtherHalf = Global.ballY > Constant.FIELD_LENGTH / 2
else:
isOtherHalf = Global.ballY < Constant.FIELD_LENGTH / 2
if (isOtherHalf): #And no near obstacle and Global.ballD > 30):
#print "ball the other end"
#if hMath.getDistSquaredBetween(pos[0],pos[1],Global.ballX,Global.ballY) > YDISTANCE_TO_BALL_SQR:
if (abs(Global.ballX - pos[0]) > YDISTANCE_TO_BALL):
sFindBall.perform(True)
#time to ball ? to get the ball if the other one is slow
if (centerPos[1] < Constant.FIELD_LENGTH / 2):
hTrack.saGoToTarget(
Global.ballX,
max(pos[1], Global.ballY - YDISTANCE_TO_BALL)
) #and ywhere the same or more if y diff less than 2 meter)
else:
hTrack.saGoToTarget(
Global.ballX,
max(pos[1], Global.ballY + YDISTANCE_TO_BALL)
) #and ywhere the same or more if y diff less than 2 meter)
else:
sFindBall.perform(True)
if (centerPos[1] < Constant.FIELD_LENGTH / 2):
movePoint = (max(
LOOK_OFFSETX,
min(Global.ballX,
Constant.FIELD_WIDTH - LOOK_OFFSETX)),
Global.ballY - YDISTANCE_TO_BALL)
else:
movePoint = (max(
LOOK_OFFSETX,
min(Global.ballX,
Constant.FIELD_WIDTH - LOOK_OFFSETX)),
Global.ballY + YDISTANCE_TO_BALL)
hTrack.saGoToTargetFacingHeading(
movePoint[0], movePoint[1],
hMath.getHeadingBetween(pos[0], pos[1], Global.ballX,
Global.ballY))
else:
#print "Grab"
r = sGrab.perform(doGetBehindv=sFindBall.GET_BEHIND_NONE)
if r == Constant.STATE_EXECUTING:
return
elif r == Constant.STATE_FAILED:
sGrab.resetPerform()
else: #if shouldIGoToCenter():
#print "goto center"
doGotoCenter()
# Use dodgy dog if required
fwdCmd = Action.finalValues[Action.Forward]
shouldIBeDodgy = shouldIBeDodgy and (not sGrab.isGrabbed) \
and sGrab.grabbingCount == 0 \
and Global.ballD > sGrab.CLOSE_DIST \
and fwdCmd > Action.MAX_SKE_FWD_SPD/2 \
and fwdCmd > Action.finalValues[Action.Left] \
and sDodgyDog.shouldIBeDodgyToBall()
if shouldIBeDodgy:
sDodgyDog.dodgyDogToBall()
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