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