def DecideNextAction(): global isGoal #print "rAttacker: Global.selfLoc at", ("%x" % id(Global.selfLoc)),\ # "=", Global.selfLoc Indicator.setDefault() #VisionLink.setGPSPaused(0) if playMode: if Global.vBall.isVisible(): isGoal = False for pressSensor in range(1, 4, 1): # Used to react to back sensors if VisionLink.getPressSensorCount(pressSensor) > 8: print "It's a goal!!" isGoal = True if isGoal: VisionLink.resetBall(Constant.FIELD_WIDTH / 2.0, Constant.FIELD_LENGTH / 2.0, 0.0, 0.0) pos = Global.selfLoc.getPos() if (hMath.getDistanceBetween(pos[0], pos[1], Constant.FIELD_WIDTH / 2.0, Constant.FIELD_LENGTH / 2.0) < 50): scFindBall.perform() else: hTrack.stationaryLocalise() hTrack.saGoToTarget(Constant.FIELD_WIDTH / 2.0, Constant.FIELD_LENGTH / 2.0) else: perform() else: ObsMapping()
def doGotoCenter(): global lastGoToCenter, startGoToCenter if Global.frame - lastGoToCenter > 1: startGoToCenter = Global.frame lastGoToCenter = Global.frame hTrack.stationaryLocalise() hTrack.saGoToTarget(Constant.FIELD_WIDTH / 2.0, Constant.FIELD_LENGTH / 2.0 + 100)
def start(): global headMode, localiseTimer hTrack.stationaryLocalise() localiseTimer += 1 if localiseTimer > ORIENT_TIME: headMode = ballTrack localiseTimer = 0 return False
def ObsMapping(): global playMode for pressSensor in range(1, 4, 1): # Used to react to back sensors if VisionLink.getPressSensorCount(pressSensor) > 8: print "Stop mapping and play the game." VisionLink.setIfObstacleUpdate(False) playMode = True VisionLink.setIfObstacleUpdate(False) return VisionLink.setIfObstacleUpdate(True) hTrack.stationaryLocalise()
def inPositionAndFacingTargetLocalise(): # Now what localise method to use? We have some selections here. # 1. Stationary Localise 2. Smart Localise 3. Beacon Localise #if VisionLink.getKickOffTeam() == VisionLink.getTeamColor() and \ # Global.kickOffState == Constant.DECIDEPOSITION_B: # # Kicker looks for ball as well as beacons # #hTrack.scanLikeGermanTeam() # hTrack.scan(maxPan = 60, minPan = -60, highCrane = 35, # lowSpeed = 8, highSpeed = 8) #else: hTrack.stationaryLocalise()
def spinWalkGenFunc(): for _ in xrange(150): Indicator.showHeadColor(Indicator.RGB_GREEN) hTrack.stationaryLocalise(5) VisionLink.setIfGPSVisionUpdate(True) yield True while True: VisionLink.setIfGPSVisionUpdate(False) # turn off vision update Indicator.showHeadColor(Indicator.RGB_YELLOW) target = (Constant.FIELD_WIDTH/2, Constant.FIELD_LENGTH/2) walk(target, Global.selfLoc.getPos(), Global.selfLoc.getHeading()) yield True
def doGotoCenter(): global lastGoToCenter, startGoToCenter if Global.frame - lastGoToCenter > 1: startGoToCenter = Global.frame lastGoToCenter = Global.frame movePoint = (centerPos[0], centerPos[1]) distanceToPoint = hMath.getDistanceBetween(movePoint[0], movePoint[1], pos[0], pos[1]) if (distanceToPoint < POS_ACCURACY): sFindBall.perform() else: hTrack.stationaryLocalise() hTrack.saGoToTarget(movePoint[0], movePoint[1])
def DecideNextAction(): current = VisionLink.getBatteryLevel() if current < 40: # if battery is low Action.stopLegs() hTrack.stationaryLocalise() return if command == "std" or command == "adj" or not currentMode: adjWalk() elif command == "cal" or command == "lrn": calWalk() else: return # for all other commands, do nothing # indicate indicateReceivedCommand() Indicator.showBatteryStatus() if shouldOpenMouth or currentMode == grabTurn: Action.openMouth() else: Action.closeMouth()
def DecideNextAction(): global doKickOff resetEachFrame() rSetPlay.resetPerform() Global.firstPlayFrame = Global.frame + 1 #import Constant #print rSetPlay.getGapTo(Constant.MAX_GOALBOX_EDGE_X, Constant.TOP_GOALBOX_EDGE_Y) #print rSetPlay.getObstaclesToGoal() # Receiving team look at the ball # FIXME: look to front for obstacles too if VisionLink.getKickOffTeam() != VisionLink.getTeamColor(): if pReady.isCentre(Global.kickOffPos): hTrack.stationaryLocalise() else: hFWHead.DecideNextAction() # Kicker looks at ball (no localise). Others scan to see obstacles elif pReady.isCentre(Global.kickOffPos): sFindBall.perform(True) elif pReady.isForward(Global.kickOffPos): hTrack.stationaryLocalise(4, 30, -30) else: hTrack.stationaryLocalise() Indicator.showBatteryStatus() doKickOff = True
def DecideNextAction(): global focusTotalTim global focusDuration global timerSinceLastLocalise global isLocalising global minHeadVariance global minPosVariance #--------------------------------------------------------------------------- # Special cases. #--------------------------------------------------------------------------- # Sometime players want to handle the head itself if compulsoryAction == doNothing: return # Sometimes players want to see the ball no matter what. if compulsoryAction == mustSeeBall: hTrack.trackVisualBall() return # Force to track wireless ball. if compulsoryAction == mustSeeWirelessBall: hTrack.trackWirelessBall() return # Force to track gps ball. if compulsoryAction == mustSeeGpsBall: hTrack.trackGpsBall() return # Force a beacon localisation - caller will set its own timers. # JOSH: WARNING: this does not appear to work. # ALEX: it doesn't work because calling SmartSetBeacon every frame is wrong #if compulsoryAction == mustLocalise: # isLocalising = True # focusDuration = 0 # sActiveLocalise.SmartSetBeacon() # sActiveLocalise.DecideNextAction() # return # Force to stationary localise - head swiping if compulsoryAction == mustStatLocalise: hTrack.stationaryLocalise() return # Force to track a point given in local coords if compulsoryAction == mustSeeLocalPoint: global localPointX, localPointY if localPointX == 0 and localPointY == 0: print "hFWHead: mustSeeLocalPoint: local point is (0,0), possibly uninitialised?" hTrack.trackLocalPoint(localPointX, localPointY) return #--------------------------------------------------------------------------- # Default behaviour of the head. #--------------------------------------------------------------------------- # # Notice that winger, supporter etc are using the default behaviour. headColor(Indicator.RGB_NONE) # print "posVar: ", Global.selfLoc.getPosVar(), "headVar: ", Global.selfLoc.getHeadingVar() ballv = VisionLink.getGPSBallVInfo(Constant.CTLocal) #--------------------------------------------------------------------------- # Already localising. if isLocalising: if focusDuration < focusTotalTime: sActiveLocalise.DecideNextAction() focusDuration += 1 headColor(Indicator.RGB_ORANGE) # Counter-act the effect of increasing lostBall counter while # localising, if not have this, locateBall will be (inappropriate) # quickly triggereds. Global.lostBall = hMath.DECREMENT(Global.lostBall) else: # Reset variables if finished focusing on beacon. isLocalising = False timerSinceLastLocalise = 0 # Track or find the ball sFindBall.perform(True) #--------------------------------------------------------------------------- # Trigger the active localise only if # a) Dog not sure where it is, or where it is heading, OR # b) Haven't done it for a while (var is small doesn't mean the position # is definitely correct) # c) AND haven't just localised # d) AND ball velocity is low # elif (Global.selfLoc.getPosVar() > hMath.get95var(minPosVariance)\ or Global.selfLoc.getHeadingVar() > hMath.get95var(minHeadVariance)\ or timerSinceLastLocalise > MAX_BEACON_LOOK_INTERVAL)\ and not (Global.vBall.isVisible()\ and Global.ballD <= 40)\ and not (timerSinceLastLocalise < MIN_BEACON_LOOK_INTERVAL)\ and ballv[2] < 2: focusDuration = 0 # Increase from this moment on. isLocalising = True sActiveLocalise.SmartSetBeacon() sActiveLocalise.DecideNextAction() # Look at the ball else: headColor(Indicator.RGB_GREEN) timerSinceLastLocalise += 1 # Track or find the ball sFindBall.perform(True)
def perform(params=None): VisionLink.startProfile("rAttacker.py") global gLastFrameCalled global gLastDecisionFunction global isGoal id(params) # ignore shouldIBeDodgy = gUseDodgyDog if gLastFrameCalled != Global.frame - 1: resetPerform() gLastFrameCalled = Global.frame ### Variable lighting challenge ### if Global.lightingChallenge and playMode: if Global.vBall.isVisible(): isGoal = False for pressSensor in range(1, 4, 1): # Used to react to back sensors if VisionLink.getPressSensorCount(pressSensor) > 8: print "It's a goal!!" isGoal = True if isGoal: VisionLink.resetBall(Constant.FIELD_WIDTH / 2.0, Constant.FIELD_LENGTH / 2.0, 0.0, 0.0) pos = Global.selfLoc.getPos() if (hMath.getDistanceBetween(pos[0], pos[1], Constant.FIELD_WIDTH / 2.0, Constant.FIELD_LENGTH / 2.0) < 50): sFindBall.perform() else: hTrack.stationaryLocalise() hTrack.saGoToTarget(Constant.FIELD_WIDTH / 2.0, Constant.FIELD_LENGTH / 2.0) return else: pass # perform normal attacker elif Global.lightingChallenge: ObsMapping() return ### End variable lighting challenge ### ### Penalty Shooter #### if Global.penaltyShot: if Global.frame - Global.firstPlayFrame < 90: hTrack.stationaryLocalise() return elif Global.frame - Global.firstPlayFrame < 115: sFindBall.perform(True) return #print "rAttacker frame", # Last man attacker can block # if not hTeam.amIFurthestBack(ignoreRoles = []): # print "not furthest back" # elif sGrab.grabbingCount > 0: # print "grabbing count", sGrab.grabbingCount # elif sGrab.isGrabbed: # print "grabbed" # elif Global.frame - sGrabDribble.gLastActionFrame < 60: # print "grabdribbleaction", sGrabDribble.gLastActionFrame ## if hTeam.amIFurthestBack(ignoreRoles = []) \ ## and not sGrab.grabbingCount > 0 \ ## and not sGrab.isGrabbed \ ## and hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > 7000 \ ## and not Global.penaltyShot \ ## and Global.frame - sGrabDribble.gLastActionFrame > 60 \ ## and Global.selfLoc.getHeading() < 180 \ ## and Global.selfLoc.getY() < Constant.FIELD_LENGTH * 0.7: ## #print __name__, "candidate block" ## #_, _, speed, dirDegrees, _, _ = VisionLink.getGPSBallVInfo(Constant.CTLocal) ## #print "speed", speed ## # This speed really needs to be 4.5 to remove all noise but then it ## # nearly never blocks. I'm under pressure to keep it possible in the ## # face of all the other checks above, hence 4 ## if sBlock.checkThenBlock(onlyVisualBall = True, minBallSpeed = 3, ## minDist = 40, maxDist = 80, ## bothSides = False, dontBlockCentre = True): ## print __name__, hMath.getTimeElapsed(sGrab.gLastGrabTime, ## VisionLink.getCurrentTime()) ## print __name__, Global.frame, "blocking, gLastActionFrame =", sGrabDribble.gLastActionFrame ## print ## return # Back off if stuck if hStuck.amIStuckForward()\ and Global.ballD > 200\ and Global.gpsLocalBall.getDistance() > 200\ and not sGrab.isGrabbed\ and sGrab.grabbingCount == 0\ and Global.frame - sGrab.gLastApproachFrame >= 5\ and (not Global.penaltyShot)\ and (not Global.lightingChallenge): sFindBall.perform(True) return # Don't go into own goal box if hWhere.ballInOwnGoalBox(0)\ and (not sGrab.isGrabbed)\ and sGrab.grabbingCount == 0\ and (not Global.penaltyShot)\ and (not Global.lightingChallenge): #print "Ball in goal box" #print "avoiding" doAvoidGoalBox() hFWHead.DecideNextAction() return # no dodgy if not shouldIEndActivelyLocalise()\ or shouldIActivelyLocalise(): #print "localising" doActivelyLocalise() elif shouldIKick(): #shouldIBeDodgy = False #Really? #print "kicking" doKick() elif Global.lightingChallenge and shouldIGoToCenter(): doGotoCenter() else: #print "tracking" doTrackBall() # Use dodgy dog if required fwdCmd = Action.finalValues[Action.Forward] shouldIBeDodgy = shouldIBeDodgy \ and (not sGrab.isGrabbed) \ and sGrab.grabbingCount == 0 \ and Global.frame - sGrab.gLastApproachFrame >= 5 \ and Global.ballD >= sGrab.CLOSE_DIST \ and Global.frame - sFindBall.gLastSpinFrame >= 15 \ and Global.frame - sGetBehindBall.gLastCalledFrame >= 5 \ and fwdCmd > Action.MAX_SKE_FWD_SPD/2 \ and fwdCmd > abs(Action.finalValues[Action.Left]) \ and sDodgyDog.shouldIBeDodgyToBall() if shouldIBeDodgy: #print "and dodging" sDodgyDog.dodgyDogToBall() VisionLink.stopProfile("rAttacker.py")
def localise(): hTrack.stationaryLocalise(4, 50, -50)
def DecideNextAction(): global indCycle, grabObject, counter, shouldGrab, moveTo global haveObstacle, isCorrectstandingPosition, distanceOK, framesLastSentYUV global localiseTime, lastHeading, lostObstacle global allBox, leftBox, rightBox, leftLongBox, rightLongBox Indicator.superDebug() point4 = [SIDE_MAX_DETECTION, 50] point_leftbox_topleftlong = [-SIDE_MAX_DETECTION, 150] shouldHeading = None calculateObstacleBox() if False: print print "isCorrectstandingPosition:", isCorrectstandingPosition print "shouldGrab:", shouldGrab print "haveObstacle:", haveObstacle print "lostObstacle:", lostObstacle print "Global.BallD:", Global.ballD isCorrectstandingPosition = True if (Global.vBall.isVisible() and Global.ballD > 30): isCorrectstandingPosition = False if shouldGrab or not isCorrectstandingPosition: print "shouldGrab or find ball and stop" if grabObject == None: grabObject = GrabGenerator() try: grabObject.next() except StopIteration: reset() else: print "stand still" Action.standStill() result =\ VisionLink.getPointToNearestObstacleInBox(\ point_leftbox_topleftlong[0],\ point_leftbox_topleftlong[1],\ point4[0],point4[1], 5\ ) # if (leftlong > MIN_POINTS_CONSIDERED_AS_OBSTACLE and leftlong > rightlong): # haveObstacle = haveObstacle + 1 # print "haveObstacle", haveObstacle # print "checking left" # if result == None: # print "error it should be checking left but return None" # shouldHeading = None # localise() # else: # pointX, pointY = result # shouldHeading = hMath.getHeadingToMe(0,0,0,pointX, pointY) # elif (rightlong > MIN_POINTS_CONSIDERED_AS_OBSTACLE and rightlong > leftlong): # haveObstacle = haveObstacle + 1 # print "haveObstacle", haveObstacle # print "checking right" # if result == None: # print "error it should be checking right but return None" # shouldHeading = None # localise() # else: # pointX, pointY = result # shouldHeading = hMath.getHeadingToMe(0,0,0,pointX, pointY) # else : # localise() # if haveObstacle > 0: # print "haveObstacle", haveObstacle # haveObstacle = 0 if result == None: if (lostObstacle > 3 and lastHeading != None): shouldHeading = lastHeading print "just lost" else: localise() print "no nearest obstacle" haveObstacle = 0 lostObstacle = lostObstacle + 1 lastHeading = None else: print "result is not none", result pointX, pointY = result shouldHeading = hMath.getHeadingToMe(0, 0, 0, pointX, pointY) print "pointing", -pointX, ":", pointY print "heading:", shouldHeading lastHeading = shouldHeading if (shouldHeading != None): #localise() calculate = int((shouldHeading - 90.0) / 90 * 80) if (framesLastSentYUV > 30): VisionLink.sendYUVPlane() framesLastSentYUV = 0 hTrack.stationaryLocalise(2, calculate + 3, calculate - 3) #Action.setHeadParams(-pointX, 0, pointY, Action.HTAbs_xyz) print "calculate:", calculate if result != None and (leftBox > MIN_POINTS_CONSIDERED_AS_OBSTACLE and leftBox > rightBox): print "move left:", haveObstacle haveObstacle = haveObstacle + 1 lostObstacle = 0 if (framesLastSentYUV > 30): VisionLink.sendYUVPlane() framesLastSentYUV = 0 if (haveObstacle > 3): shouldGrab = True moveTo = LEFT elif result != None and (rightBox > MIN_POINTS_CONSIDERED_AS_OBSTACLE and rightBox > leftBox): print "move right:", haveObstacle haveObstacle = haveObstacle + 1 lostObstacle = 0 if (framesLastSentYUV > 30): VisionLink.sendYUVPlane() framesLastSentYUV = 0 if (haveObstacle > 3): shouldGrab = True moveTo = RIGHT if False: #all > 0 :# and counter == 60: print print "all:", allBox, "endall" print "left:", leftBox, "endleft" print "right:", rightBox, "endright" print "leftlong:", leftLongBox, "endleft" print "rightlong:", rightLongBox, "endright" counter = 0 counter = counter + 1 framesLastSentYUV = framesLastSentYUV + 1
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 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 DecideNextAction(): global lastSendTime # Reset the 12 leds on face pattern. Indicator.setDefault() blueLeft = Global.VisualObject( *VisionLink.getVisualObject(Constant.vobBlueLeftBeacon)) blueRight = Global.VisualObject( *VisionLink.getVisualObject(Constant.vobBlueRightBeacon)) yellowLeft = Global.VisualObject( *VisionLink.getVisualObject(Constant.vobYellowLeftBeacon)) yellowRight = Global.VisualObject( *VisionLink.getVisualObject(Constant.vobYellowRightBeacon)) yellowGoal = Global.VisualObject( *VisionLink.getVisualObject(Constant.vobYellowGoal)) blueGoal = Global.VisualObject( *VisionLink.getVisualObject(Constant.vobBlueGoal)) ball = Global.VisualObject(*VisionLink.getVisualObject(Constant.vobBall)) if blueLeft.getConfidence() > 0: Indicator.finalValues[Indicator.Led7] = Indicator.LED3_INTENSITY_MAX if blueRight.getConfidence() > 0: Indicator.finalValues[Indicator.Led8] = Indicator.LED3_INTENSITY_MAX if blueGoal.getConfidence() > 0: Indicator.finalValues[Indicator.Led11] = Indicator.LED3_INTENSITY_MAX if yellowLeft.getConfidence() > 0: Indicator.finalValues[Indicator.Led3] = Indicator.LED3_INTENSITY_MAX if yellowRight.getConfidence() > 0: Indicator.finalValues[Indicator.Led4] = Indicator.LED3_INTENSITY_MAX if yellowGoal.getConfidence() > 0: Indicator.finalValues[Indicator.Led12] = Indicator.LED3_INTENSITY_MAX if redDogSeen(): Indicator.finalValues[Indicator.Led5] = Indicator.LED3_INTENSITY_MAX if blueDogSeen(): Indicator.finalValues[Indicator.Led6] = Indicator.LED3_INTENSITY_MAX Indicator.superDebug() #send unexpected cplanes unexpectedCPlane = False if expectBall == (ball.getConfidence() == 0): unexpectedCPlane = True if expectBlueGoal == (blueGoal.getConfidence() == 0): unexpectedCPlane = True if expectYellowGoal == (yellowGoal.getConfidence() == 0): unexpectedCPlane = True if expectBlueLeft == (blueLeft.getConfidence() == 0): unexpectedCPlane = True if expectBlueRight == (blueRight.getConfidence() == 0): unexpectedCPlane = True if expectYellowLeft == (yellowLeft.getConfidence() == 0): unexpectedCPlane = True if expectYellowRight == (yellowRight.getConfidence() == 0): unexpectedCPlane = True if expectRedDog == (not redDogSeen()): unexpectedCPlane = True if expectBlueDog == (not blueDogSeen()): unexpectedCPlane = True if bSendCPlane and unexpectedCPlane: if Global.frame - lastSendTime > NO_SENDING_CPLANE: lastSendTime = Global.frame VisionLink.sendCompressedCPlane() if headCommand == "spin": hTrack.spinningLocalise() elif headCommand == "sloc": hTrack.stationaryLocalise() elif headCommand is not None: Action.setHeadParams(*headCommand)
def performTimeElapsedAction(selType): global gLastActionFrame global gForceTimeElapse global gKickType global gLastKickFrame global gLastBreakFrame global gLastLocaliseFrame global gKickCounter global gBreakCounter global gLocaliseCounter gLastActionFrame = Global.frame gForceTimeElapse = True #print "performTimeElapsed(", selType, ")" #if selType == SEL_OFFENSIVE: # import traceback # traceback.print_stack() if Action.shouldIContinueKick(): #print "Waiting for kick to activate ...", gKickCounter Action.continueKick() moveHeadForwardTight() elif gKickCounter > 0: #print "Kicking ...", gKickCounter if gKickType == AVOID_OWN_GOAL: if gKickCounter > 10: perform(0, 0, Action.MAX_TURN) elif gKickCounter > 8: perform(0, 0, 0) else: perform(Action.MAX_FORWARD, 0, 0, minorWalkType=Action.SkeGrabDribbleMWT) elif gKickType == GAP_KICK: selfH = Global.selfLoc.getHeading() if abs(gKickHead - selfH) > 10: turnCCW = gKickHead - selfH perform(0, 0, turnCCW) # resetting until we are lined up to the gap. gKickCounter = SOFT_TAP_DURATION else: gKickType = Action.SoftTapWT Action.kick(gKickType) perform(0, 0, 0) moveHeadForwardTight() elif gKickType == SIDE_STEP_LEFT_SHOOT\ or gKickType == SIDE_STEP_RIGHT_SHOOT\ or gKickType == FWD_STEP_SHOOT: #print "gKickType = SIDESTEP_X_SHOOT" # Do side step shoot... if gKickCounter > FWD_STEP_SHOOT_DURATION: left = Action.MAX_LEFT if gKickType == SIDE_STEP_RIGHT_SHOOT: left = -Action.MAX_LEFT turnCCW = 0 # FIXME: Sidestep to Gap if Global.vTGoal.isVisible(): turnCCW = Global.vTGoal.getHeading() / 2 elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION: turnCCW = gLastVisTGoal.getHeading() / 2 turnCCW = hMath.CLIP(turnCCW, 30) perform(5, left, turnCCW) # Do forward step shoot... elif gKickCounter > NU_FWD_DURATION: fwd = Action.MAX_FORWARD turnCCW = 0 if Global.vTGoal.isVisible(): turnCCW = Global.vTGoal.getHeading() / 2 elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION: turnCCW = gLastVisTGoal.getHeading() / 2 turnCCW = hMath.CLIP(turnCCW, 15) perform(fwd, 0, turnCCW, minorWalkType=Action.SkeFastForwardMWT) # Do kick! else: gKickType = gNextKickType print "after sidestep setting kick type to", gNextKickType #perform(0,0,0) Action.kick(gKickType) moveHeadForwardTight() elif gKickType == RELEASE: #print "gKickType = RELEASE", gKickCounter Action.walk(0, 0, 0, minorWalkType=Action.SkeGrabDribbleHoldMWT) if gKickCounter > RELEASE_DURATION - 15: #print "stopping..." Action.openMouth() sGrab.moveHeadForward() elif gKickCounter > 15: #print "scanning" hTrack.stationaryLocalise(speed=6) Action.closeMouth() else: #print "regrabbing" sGrab.moveHeadForward() Action.openMouth() # Reset grab timers sGrab.gLastGrabTime = VisionLink.getCurrentTime() sGrab.gGrabCount = 0 gForceTimeElapse = False else: #print "gKickCounter =", gKickCounter, "- continuing kick" # Reset gps ball coordinates. # So when it goes back to find ball, it won't turn around and walk # straight. x, y = hMath.getPointRelative(Global.selfLoc.getX(), Global.selfLoc.getY(), Global.selfLoc.getHeading(), 160) VisionLink.resetBall(x, y, 0, 0) Action.continueKick() Action.openMouth() hTrack.panLow = True gKickCounter -= 1 gLastKickFrame = Global.frame if gKickType != RELEASE: sFindBall.setForce(sFindBall.FORCE_FOLLOW) elif gBreakCounter > 0: #print "gBreakCounter > 0" #if Global.penaltyShot or Global.lightingChallenge: # # Stop and release # gBreakCounter = 0 # gKickType = RELEASE # gKickCounter = RELEASE_DURATION # If we cannot see the ball, then reset gps ball coordinates. # So when it goes back to find ball, it won't turn around and walk straight. if not Global.vBall.isVisible(): x, y = hMath.getPointRelative(Global.selfLoc.getX(), Global.selfLoc.getY(), Global.selfLoc.getHeading(), 50) VisionLink.resetBall(x, y, 0, 0) if gBreakCounter > 10: Action.setHeadParams(0, 0, 0, Action.HTAbs_h) Action.walk(Action.MAX_FORWARD, 0, 0, minorWalkType=Action.SkeFastForwardMWT) elif gBreakCounter > 6: Global.lostBall = Constant.LOST_BALL_LAST_VISUAL sFindBall.perform(True) rate = Action.SKE_FF_PG * 8.0 * 2.0 / 1000.0 fwd = Action.MAX_SKE_FF_FWD_STP * 0.7 / rate Action.walk(fwd, 0, 0, minorWalkType=Action.SkeFastForwardMWT) Action.openMouth() else: Global.lostBall = 8 - gBreakCounter + Constant.LOST_BALL_LAST_VISUAL sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY) Action.openMouth() hTrack.panLow = True gBreakCounter -= 1 gLastBreakFrame = Global.frame elif Global.frame - gLastKickFrame > 1\ and Global.frame - gLastBreakFrame > 1: #print "gKickCounter = 0, Selecting kick", selType selectKick(selType) else: resetPerform() sGrab.resetPerform() return Constant.STATE_SUCCESS return Constant.STATE_EXECUTING
def readyMatchPlaying(): global debug, timer, inStartPos, isFacingTargetGoal, getOutOfGoalDirection, lastTurningDirection, gUseDodgyDog selfX = Global.selfLoc.getPos()[0] selfY = Global.selfLoc.getPos()[1] selfH = Global.selfLoc.getHeading() Action.closeMouth() # Player number detection. The goalie *must* be player one. Freak if not. if Global.myPlayerNum == 1 and not Global.isGoalieBehaviour: Action.kick(Action.ChestPushWT) return elif Global.myPlayerNum != 1 and Global.isGoalieBehaviour: Action.kick(Action.ChestPushWT) return if not Global.penaltyShot and hStuck.amIStuckForward(dodgeTime=9): return # Need to move toward the field ? if shouldITurnTowardTheField(): Action.walk(0, 0, getOutOfGoalDirection) # Am I in the correct starting position and facing toward the target goal # (or facing upward)? if inStartPos and isFacingTargetGoal: # use the ball distance to try to move as close as the centre circle #if VisionLink.getKickOffTeam() == VisionLink.getTeamColor() and \ # useBallDistanceSetKickOffPosition(): # return # Do I need to move to a new position if I am in a wrong position? if Debug.allowReadyMove: if hMath.getDistanceBetween(selfX, selfY, \ Global.kickOffPos[0], Global.kickOffPos[1]) \ > Constant.POS_OFFSET_OUT_KICK: inStartPos = False return # Turn myself if I am not facing toward the desired heading. At the # moment 90 is the desired heading angle. # TODO: Accoiding to different positions, different desired heading. if abs(hMath.normalizeAngle_180(selfH - 90)) > \ Constant.READY_HEADING_OFFSET: isFacingTargetGoal = False return inPositionAndFacingTargetLocalise() return # Am I in the correct starting position but not facing the target goal (or # facing upward)? if inStartPos: # Do I need to move to a new position if I am in a wrong position? if Debug.allowReadyMove: if hMath.getDistanceBetween(selfX, selfY, \ Global.kickOffPos[0], Global.kickOffPos[1])\ > Constant.POS_OFFSET_OUT_KICK: inStartPos = False return # Is me facing toward the target goal now? If I am facing the right # direction, then better reset the turning direction variable. if abs(selfH - 90) < Constant.READY_HEADING_OFFSET: isFacingTargetGoal = True lastTurningDirection = None # If I am not facing toward the target goal, then turn slowly without # moving forward or left. else: # Determine the best turning direction and stick with it. if lastTurningDirection == None: # I haven't determine my turning direction yet. Determine now. if Global.selfLoc.getHeading() >= 270 or \ Global.selfLoc.getHeading() <= 90: # -90 <= angle <= 90 lastTurningDirection = Constant.dANTICLOCKWISE if Debug.readyDebug: print "Anti-clockwise direction!" else: # 90 < angle < 270 lastTurningDirection = Constant.dCLOCKWISE if Debug.readyDebug: print "Clockwise direction!" if lastTurningDirection == Constant.dCLOCKWISE: Action.walk(0, 0, -15) else: Action.walk(0, 0, 15) return # Phase1: I am not in the correct starting position. First step for me is # to stand still and beacon localise. if timer <= INITIAL_LOCALISE_TIME: if Debug.readyDebug and timer == 1: print "Phase 1 has been executed!" if timer <= 79: Global.kickOffState = Constant.NOTDECIDED Action.stopLegs() # Need to move toward the field ? if shouldITurnTowardTheField(): Action.walk(0, 0, getOutOfGoalDirection) hTrack.stationaryLocalise() timer = timer + 1 if timer >= 80: # Now decide my starting kickoff position dynamically now. if hTeam.robotAlive() != 1: getMyKickOffPosition() return lastTurningDirection = None # Now we can either use the static result from getMyKickOffPosition() or # dynamically calculated again, if hTeam.robotAlive() != 1: getMyKickOffPosition() # If I am unable to find the kickoff position earlier (possibly because of # wirless problem), then probably only 2 or less robots are on the field # now. Notice that I can't be a goalie in this case. I must be either # player 2 forward or player 3 forward. Not player 4 forward because it # determines its kicking position without worrying about any other # forwards. if Global.kickOffState == Constant.NOTDECIDED or Global.kickOffPos == None: alive = hTeam.forwardAlive() if alive >= 3: if Debug.readyDebug: print "Something went wrong, three forwards without assigning! Automatical assign!" ##~ # This case may happen in friendly games where not all four robots are present. ##~ # Firstly if there are three forwards alive, but I am not assigned a position. Then: ##~ # 1. I can't be player 1 because this is a goalie ##~ # 2. I can't be player number 4 forward. ##~ # So I must be either player 2 or player 3 forward. ##~ # ##~ # If I am a player 3, then probably the three forwards are: 1, 2, 3 ##~ # If I am a player 2, then probably the three forwards are: 1, 2, 4 ##~ if Global.myPlayerNum == 3: _, Global.kickOffPos, _, _ = getAllPossibleKickingOffPosition() Global.kickOffState = Constant.DECIDEPOSITION_B elif Global.myPlayerNum == 2: # Try to get player 4's decision. if Global.teamPlayers[3].getCounter() > 0: position = Global.teamPlayers[3].getKickedOff() if position == Constant.DECIDEPOSITION_A: determineMyKickingOffPositionIfOnlyMeForwardAlive( False, True, True) elif position == Constant.DECIDEPOSITION_B: determineMyKickingOffPositionIfOnlyMeForwardAlive( True, False, True) elif position == Constant.DECIDEPOSITION_C: determineMyKickingOffPositionIfOnlyMeForwardAlive( True, True, False) else: Debug.goAndMakeTraceBack() return else: # Well, lets wait. return elif alive == 2: if Debug.readyDebug: print "Only two alive case - What the hell?" if Global.myPlayerNum == 2: Global.kickOffPos, b, c, d = getAllPossibleKickingOffPosition() Global.kickOffState = Constant.DECIDEPOSITION_A elif Global.myPlayerNum == 3: a, Global.kickOffPos, c, d = getAllPossibleKickingOffPosition() Global.kickOffState = Constant.DECIDEPOSITION_B else: if Debug.readyDebug: print "Something went wrong, I am not player 2 or 3, why not decided position?" elif alive == 1: if Debug.readyDebug: print "Only one alive case!" a, b, c, d = getAllPossibleKickingOffPosition() if Global.myPlayerNum == 2: Global.kickOffPos = a Global.kickOffState = Constant.DECIDEPOSITION_A elif Global.myPlayerNum == 3: Global.kickOffPos = b Global.kickOffState = Constant.DECIDEPOSITION_B elif Global.myPlayerNum == 4: Global.kickOffPos = c Global.kickOffState = Constant.DECIDEPOSITION_C else: # Goalie Global.kickOffPos = d Global.kickOffState = Constant.DECIDEPOSITION_D # At this point the kicking off position must been determined. showKickOffHeadIndicator() # Set the initial roles based on position if Global.kickOffPos == P_REC_CENTRE: Global.myRole = Constant.ATTACKER elif Global.kickOffPos == P_REC_LEFT_WIDE \ or Global.kickOffPos == P_REC_RIGHT_WIDE: Global.myRole = Constant.DEFENDER elif Global.kickOffPos == P_REC_RIGHT_CLOSE \ or Global.kickOffPos == P_REC_LEFT_CLOSE: Global.myRole = Constant.SUPPORTER elif Global.kickOffPos == P_KICK_KICKER: Global.myRole = Constant.ATTACKER elif Global.kickOffPos == P_KICK_LEFT_BACK \ or Global.kickOffPos == P_KICK_RIGHT_BACK: Global.myRole = Constant.DEFENDER elif Global.kickOffPos == P_KICK_RIGHT_FORWARD \ or Global.kickOffPos== P_KICK_LEFT_FORWARD: Global.myRole = Constant.SUPPORTER # Testing the kickoff position without actually moving them. ##~ if Debug.readyDebug and Debug.testKickOffPosition: ##~ if Global.kickOffState == None: ##~ print "My kickoff position is: None %f" %Global.myPlayerNum ##~ else: ##~ print "My player number: %f" %Global.myPlayerNum ##~ print "My kickoff position is: %f" %Global.kickOffState ##~ return # Move from phase 2 to phase 3. Am I in the correct starting position now? dist = hMath.getDistanceBetween(selfX, selfY, Global.kickOffPos[0], Global.kickOffPos[1]) if dist < Constant.POS_OFFSET_IN_KICK: # Ok, I am in the correct starting position. I stop myself now. Action.stopLegs() inStartPos = True # Phase2: I need to walk to my starting position. Localise by moving the # head while I move. else: if dist < sDodgyDog.MIN_DODGY_TARGET_DIST: hTrack.stationaryLocalise() else: # Move head less while dodgy to see obstacles hTrack.stationaryLocalise(5, 30, -30) if gUseDodgyDog and \ sDodgyDog.shouldIBeDodgy(Global.kickOffPos[0], Global.kickOffPos[1], True, Constant.POS_OFFSET_OUT_KICK): if sDodgyDog.dodgyDogTo( Global.kickOffPos[0], Global.kickOffPos[1], True, Constant.POS_OFFSET_OUT_KICK) == Constant.STATE_SUCCESS: inStartPos = True else: movingAction()