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