def perform(params=None): global setPlay, playCounter id(params) # ignore # Check for set plays to execute if setPlay == None: if pSet.doKickOff: if VisionLink.getKickOffTeam() == VisionLink.getTeamColor(): setPlay = selectKickOffSetPlay() else: #setPlay = receiveKickOffCharge setPlay = receiveKickOffPincer #setPlay = receiveKickOffEchelon pSet.doKickOff = False if setPlay != None: print "executing set play:", setPlay if setPlay != None: playCounter += 1 Indicator.showRedEye(True, True) stillGoing = setPlay() if not stillGoing: print "Set play complete" sGrab.resetPerform() rAttacker.doKickReset() setPlay = None return False return True Indicator.showRedEye(False, False) resetPerform() return False
def perform(aimToGoal = True, kickType=Action.HeadTapWT, \ tX = Constant.TARGET_GOAL_X, tY = Constant.TARGET_GOAL_Y): global gLastFrameCalled global gtkGenerator global isAimToGoal, targetX, targetY, gKickType if not sGrab.isGrabbed: resetPerform() return Constant.STATE_FAILED elif not sGrab.isBallUnderChin(): sGrab.resetPerform() resetPerform() return Constant.STATE_FAILED if gLastFrameCalled != Global.frame - 1: resetPerform() gLastFrameCalled = Global.frame if gtkGenerator == None: isAimToGoal = aimToGoal if not isAimToGoal: targetX = tX targetY = tY gKickType = kickType gtkGenerator = gtkGeneratorFunc() try: Global.lostBall = 0 gtkGenerator.next() except StopIteration: resetPerform() return Constant.STATE_SUCCESS return Constant.STATE_EXECUTING
def processFrame(): VisionLink.startProfile("Behaviou.py") global profilerInstance, bStopMotion sec, usec = VisionLink.getCurrentTime() profileFunc(hFrameReset.framePreset) if Debug.frameDebug: print "RLNK{", VisionLink.getCurrentPreviousFrameID()[0], "}" if not bStopDecideNextAction: # If first call to ready state, reset the readyplayer's globals kickOffTeam = VisionLink.getKickOffTeam() if VisionLink.getTheCurrentMode() == Constant.READYSTATE\ and (Global.state != Constant.READYSTATE\ or Global.kickOffTeam != kickOffTeam): Global.kickOffTeam = kickOffTeam pReady.initReadyMatchPlaying() profileFunc(hFrameReset.frameReset) # If it's not playing state, then we should reset grab. # Because we don't want to turn off gps vision update. if Global.state != Constant.PLAYINGSTATE: sGrab.resetPerform() # Ready state - let me move to my correct position. if Global.state == Constant.READYSTATE or Debug.mustReady: pReady.readyMatchPlaying() hTeam.sendWirelessInfo() # Set state - I can't move my legs, but I can move head to look for # ball. elif Global.state == Constant.SETSTATE: pSet.DecideNextAction() hTeam.sendWirelessInfo() elif Global.state == Constant.FINISHEDSTATE: hTeam.sendWirelessInfo() else: profileFunc(Player.player.DecideNextAction) debugOnDemand() dynamic_gain() hFrameReset.framePostset() if bStopLegOnly: Action.stopLegs() if not bStopMotion: VisionLink.sendAtomicAction(*tuple(Action.finalValues)) VisionLink.stopProfile("Behaviou.py")
def DecideNextAction(): global frameSinceLastGrabKick if sGrab.isGrabbed: if perform() != Constant.STATE_EXECUTING: sGrab.resetPerform() frameSinceLastGrabKick = Global.frame resetPerform() elif Global.frame - frameSinceLastGrabKick > 60: sGrab.perform() else: sFindBall.perform()
def perform(forward, left, turnCCW, walkType=None, minorWalkType=None, checkForBallUnderChin=True): if not sGrab.isGrabbed: resetPerform() return Constant.STATE_FAILED elif checkForBallUnderChin and not sGrab.isBallUnderChin(): sGrab.resetPerform() resetPerform() return Constant.STATE_FAILED Action.openMouth() moveHeadForward() wt = Action.SkellipticalWalkWT if forward != 0 and left != 0 and turnCCW != 0: mwt = Action.GrabDribbleMWT elif forward != 0 and left != 0: mwt = Action.GrabDribbleMWT elif forward != 0 and turnCCW != 0: if abs(turnCCW) < 10: mwt = Action.SkeFastForwardMWT else: mwt = Action.SkeGrabDribbleMWT elif left != 0 and turnCCW != 0: mwt = Action.SkeGrabDribbleMWT elif forward != 0: mwt = Action.SkeFastForwardMWT elif left != 0: mwt = Action.SkeGrabDribbleMWT elif turnCCW != 0: mwt = Action.SkeGrabDribbleMWT else: mwt = Action.SkeGrabDribbleMWT if walkType != None: wt = walkType if minorWalkType != None: mwt = minorWalkType Action.walk(forward, left, turnCCW, "sdd", wt, mwt) return Constant.STATE_EXECUTING
def doKick(): global gSelectedKick global gIsKickTriggering global gLastKickedFrame # After you released the ball, don't grab for a second. if Global.frame - gLastKickedFrame < 75: sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_LOTS) return kickType = gSelectedKick[0] # grab actions if sSelKick.doINeedToGrab(kickType)\ or sGrab.isGrabbed: r = doKickWithGrab() # non grab actions else: r = doKickWithoutGrab() # When return value from skill is not executing, # Do some resetting business if r != Constant.STATE_EXECUTING: # Resetting grab and all other kick modules sGrab.resetPerform() doKickReset() if kickType != sSelKick.KT_AVOID_OWN_GOAL\ and r == Constant.STATE_SUCCESS: gLastKickedFrame = Global.frame gIsKickTriggering = False gSelectedKick = None if kickType == sSelKick.KT_GRAB_DRIBBLE: pass elif kickType == sSelKick.KT_DRIBBLE\ or kickType == sSelKick.KT_PAW_KICK: hTrack.panLow = True else: hTrack.panLow = False doTrackBall() showKickTypeIndicator(kickType)
def performDirection(direction, isClockwise): global gLastFrameCalled global gDirection if not sGrab.isGrabbed: resetPerform() return Constant.STATE_FAILED elif not isBallUnderChin(): sGrab.resetPerform() resetPerform() return Constant.STATE_FAILED if gLastFrameCalled != Global.frame - 1: resetPerform() gLastFrameCalled = Global.frame if gDirection == None: gDirection = hMath.normalizeAngle_180(direction) if isClockwise == None: gDirection = direction elif isClockwise and gDirection > 0: gDirection = gDirection - 360 elif not isClockwise and gDirection < 0: gDirection = 360 + gDirection print "TURNKICK Direction!: ", gDirection if gKickCounter == 0: setTurnKick() doTurnKick() if Global.vBall.isVisible() or gHeadLiftCounter > 12: resetPerform() sGrab.resetPerform() if (gDirection < 0): sFindBall.setHint(60, 60) else: sFindBall.setHint(60, 120) return Constant.STATE_SUCCESS else: return Constant.STATE_EXECUTING
def DecideNextAction(): global grabCounter grabCounter -= 1 #testKick() #return Action.openMouth() sGrab.moveHeadForward() Action.walk(0, 0, 0) obsFront = VisionLink.getNoObstacleInBox(-25, 100, 25, 20, Constant.MIN_VIS_OBSTACLE_BOX, Constant.OBS_USE_LOCAL) obsLeft = VisionLink.getNoObstacleInBox(-80, 80, -40, -80, Constant.MIN_VIS_OBSTACLE_BOX, Constant.OBS_USE_LOCAL) obsRight = VisionLink.getNoObstacleInBox(40, 80, 80, -80, Constant.MIN_VIS_OBSTACLE_BOX, Constant.OBS_USE_LOCAL) print "Obstacle Front : ", obsFront, " Left : ", obsLeft, " Right: ", obsRight return #print "Camera Frame : ", Global.cameraFrame Indicator.setDefault() if not sGrab.isGrabbed or performToTargetGoal( ) != Constant.STATE_EXECUTING: if grabCounter == 60: hTrack.panLow = True resetPerform() sGrab.resetPerform() if grabCounter > 0: sFindBall.perform() else: sGrab.perform() grabCounter -= 1 else: grabCounter = 60
def kickOffUPenn(): global playCounter, whenGrabbed if pReady.chosenFormation == 0: kickLeft = False else: kickLeft = True if pReady.isCentre(Global.kickOffPos): if playCounter > 150: return False if whenGrabbed == 0: print "grabbing" if sGrab.perform() == Constant.STATE_FAILED: return False if sGrab.isGrabbed: whenGrabbed = playCounter return True print "Grabbed!" if sUpennKick.perform(kickLeft) == Constant.STATE_SUCCESS: sGrab.resetPerform() return False sFindBall.perform(True) return True elif pReady.isForward(Global.kickOffPos): if playCounter > 40: return False hTrack.saGoToTarget(Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y) hFWHead.DecideNextAction() else: return False return True
def kickOffShootGoal(): global playCounter, whenGrabbed if pReady.isCentre(Global.kickOffPos): if playCounter > 150: sGrab.resetPerform() return False if whenGrabbed == 0: #print "grabbing" sGrab.perform() if sGrab.isGrabbed: #print "grabbed" whenGrabbed = playCounter return True if playCounter < whenGrabbed + 40: #print "walking" sGrabDribble(0, Action.MAX_LEFT, 0) return True # not aiming to visual goal if sGrabTurnKick.perform(False) == Constant.STATE_SUCCESS: #print "aiming" sGrab.resetPerform() return False elif pReady.isForward(Global.kickOffPos): if playCounter > 40: return False hTrack.saGoToTarget(Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y) else: return False hFWHead.DecideNextAction() return True
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 perform(forward, left, turnCCW, walkType=None, minorWalkType=None, checkForBallUnderChin=True): global gLastActionFrame global gTurnCCWSpd if not sGrab.isGrabbed: print "Grab failed" resetPerform() return Constant.STATE_FAILED elif checkForBallUnderChin and not sGrab.isBallUnderChin(): sGrab.resetPerform() resetPerform() return Constant.STATE_FAILED Action.openMouth() moveHeadForward() gLastActionFrame = Global.frame wt = Action.SkellipticalWalkWT if forward != 0 and left != 0 and turnCCW != 0: mwt = Action.GrabDribbleMWT elif forward != 0 and left != 0: mwt = Action.GrabDribbleMWT elif forward != 0 and turnCCW != 0: if abs(turnCCW) < 10: mwt = Action.SkeFastForwardMWT else: mwt = Action.SkeGrabDribbleMWT elif left != 0 and turnCCW != 0: mwt = Action.SkeGrabDribbleMWT elif forward != 0: mwt = Action.SkeFastForwardMWT elif left != 0: mwt = Action.SkeGrabDribbleMWT elif turnCCW != 0: mwt = Action.SkeGrabDribbleMWT else: mwt = Action.SkeGrabDribbleMWT if walkType != None: wt = walkType if minorWalkType != None: mwt = minorWalkType Action.walk(forward, left, turnCCW, "sdd", wt, mwt) if gTurnCCWSpd != None: Action.finalValues[Action.TurnCCW] = gTurnCCWSpd gTurnCCWSpd = None return Constant.STATE_EXECUTING
def GrabGenerator(): global counter, grabbing_time_when_stop, moveTo, isCorrectstandingPosition grab_counter = 0 sensor_counter = 0 kickType = None # grabbing local_isGrabed = False if not local_isGrabed: sGrab.resetPerform() while sGrab.perform() == Constant.STATE_EXECUTING: if not isCorrectstandingPosition and Global.ballD <= 30: #sGrab.REALLY_SLOW_DIST : isCorrectstandingPosition = True print "distance ok to stop" return yield True while local_isGrabed: if grabbing_time_when_stop > MAX_GRABBING_TIME_WHEN_STOP: grabbing_time_when_stop = 0 local_isGrabed = True sGrab.moveHeadForward() Action.openMouth() grabbing_time_when_stop = grabbing_time_when_stop + 1 yield True while not kickType: sGrab.moveHeadForward() Action.openMouth() #posx,posy = Global.selfLoc.getPos() #print "float:",posx, posy #posx = int(posx) #posy = int(posy) #print "int:",posx, posy if moveTo == None: print "WARNING moveTo == None" print "This is broken" #sGrabDribble.GrabSideWalk(moveTo) #kickType == Action.DiveKickWT grab_counter += 1 if grab_counter % 15 != 0: if VisionLink.getAnySensor(Constant.ssCHEST_INFRARED) > 138000: sensor_counter += 1 else: if sensor_counter < 3: print "Oops, fail grabbing the ball.." reset() return else: sensor_counter = 0 # Check if we've grabbed for too long if grab_counter > GRAB_FRAME_MAX: kickType = Action.DiveKickWT #headColor((True, False, False)) #Action.standStill() #reset() #return # Raise a StopIteration exception yield True # Start kick # if Debug.TurnAndSideMoveDebug : print "kicking" if kickType == Action.DiveKickWT: kickCount = 15 #30 else: kickCount = 22 # Do kick for _ in range(kickCount): Action.kick(kickType) Action.openMouth() yield True isCorrectstandingPosition = False Global.lostBall = 0 reset()
def gtkGeneratorFunc(): global frameSinceLastGrabKick, aimCounter # Ok, we've grabbed the ball, now spin towards the target aimCounter = 0 #print "*** GTK" # if we have enough time to do turn/side kick while hMath.getTimeElapsed( sGrab.gLastGrabTime, VisionLink.getCurrentTime()) < Constant.GRAB_TIMEOUT: # print "Vframe", Global.frame, isLinedUp = False # now decide what to do #print "lining up,", if isAimToGoal: isLinedUp = performAimToGoal() else: isLinedUp = performNotAimToGoal() # If visual aim fails (we turn too far) stop now if isLinedUp == Constant.STATE_FAILED: break # Im lined up, so kick! # DON'T wait for step complete if isLinedUp == Constant.STATE_SUCCESS: #print "kicking" Action.forceStepComplete() Action.kick(gKickType) while Action.shouldIContinueKick(): Action.continueKick() yield True for _ in range(sGrabDribble.NU_FWD_DURATION): yield True frameSinceLastGrabKick = Global.frame sFindBall.setHint(200, 90) sFindBall.perform() Action.closeMouth() resetPerform() return # else line up still executing if SEND_DEBUG_INFO_TO_ROBOLINK: sendDebugInfo() yield True #print "timeout or turned too far...", ### The line-up has failed minH, maxH = selectGap(True) if minH < 0 < maxH: #print "shooting" Action.forceStepComplete() Action.kick(gKickType) while Action.shouldIContinueKick(): Action.continueKick() yield True for _ in sGrabDribble.NU_FWD_DURATION: yield True frameSinceLastGrabKick = Global.frame sFindBall.setHint(200, 90) sFindBall.perform() Action.closeMouth() resetPerform() return sFindBall.setHint(20, 90) # directly in front sFindBall.perform() Action.closeMouth() resetPerform() sGrab.resetPerform() return
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 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 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 performTimeElapsedAction(selType): global gForceTimeElapse global gKickType global gLastKickFrame global gLastBreakFrame global gLastLocaliseFrame global gKickCounter global gBreakCounter global gLocaliseCounter gForceTimeElapse = True 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: # 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 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 = Action.NUFwdKickWT Action.kick(gKickType) perform(0, 0, 0) moveHeadForwardTight() elif gKickType == JUST_SHOOT: selfH = Global.selfLoc.getHeading() if gKickHead - selfH > 5: turnCCW = (gKickHead - selfH) / 2 # if (turnCCW > 0): # left = Action.MAX_LEFT # else: # left = -Action.MAX_LEFT # # perform(5,left,hMath.CLIP(turnCCW,30)) perform(0, 0, turnCCW) # resetting until we are lined up to the gap. gKickCounter = SOFT_TAP_DURATION else: gKickType = Action.HandKickLeftWT Action.kick(gKickType) perform(0, 0, 0) moveHeadForwardTight() else: # 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 = False gKickCounter -= 1 gLastKickFrame = Global.frame elif gBreakCounter > 0: # 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 > 5: 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.8 / rate Action.walk(fwd, 0, 0, minorWalkType=Action.SkeFastForwardMWT) Action.openMouth() else: Global.lostBall = 8 - gBreakCounter + Constant.LOST_BALL_LAST_VISUAL sFindBall.perform() Action.openMouth() hTrack.panLow = True gBreakCounter -= 1 gLastBreakFrame = Global.frame elif Global.frame - gLastKickFrame > 1\ and Global.frame - gLastBreakFrame > 1: #print "Kick!!!!" selectKick(selType) else: resetPerform() sGrab.resetPerform() return Constant.STATE_SUCCESS return Constant.STATE_EXECUTING
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()