def frameReset(): # Hack for when the robot has grabbed the ball. if isGrabbed: # The ball x, y should be a little bit in front from the robot, # so that kick selection won't stuff up. Global.ballX, Global.ballY = hMath.getPointRelative( Global.selfLoc.getX(), Global.selfLoc.getY(), Global.selfLoc.getHeading(), Constant.BallRadius) Global.ballH = 0 Global.haveBall = 1 Global.lostBall = 0 VisionLink.resetBall(Global.ballX, Global.ballY, 0, 0) VisionLink.setIfGPSVisionUpdate(0) VisionLink.setGrabbed() else: VisionLink.setIfGPSVisionUpdate(1)
def isBallContested(): global gObsToBall, gObsBack global gLastObstacleFrame global gLastStuckFrame global gLastPawFrame global gLastContestedCalledFrame global gflp, gfrp, gLastflp, gLastfrp if Global.frame != gLastContestedCalledFrame: x, y = hMath.getPointRelative(0, 0, Global.ballH, Global.ballD + 30) # Count obstacles in corridor between us and the ball gObsToBall = VisionLink.getNoObstacleBetween(0, 0, int(x), int(y), 50, 0, 10, Constant.OBS_USE_GPS) # Count obstacles in corridor behind me (these hang around in GPS # for a while (we think)) gObsBack = VisionLink.getNoObstacleBetween(0, -35, 0, 0, 50, 0, 10, Constant.OBS_USE_GPS) gLastContestedCalledFrame = Global.frame if gUseObstacleDetect and gObsToBall + gObsBack > MIN_OBS_CONTESTED: gLastObstacleFrame = Global.frame rlr = VisionLink.getLastMaxStepPWM(Constant.ssRL_ROTATOR) rrr = VisionLink.getLastMaxStepPWM(Constant.ssRR_ROTATOR) if gUseStuckDetect\ and Action.finalValues[Action.Forward] > 0\ and Action.finalValues[Action.ForwardStep] > 0\ and (rlr[1] > 800 or rrr[1] > 800): gLastStuckFrame = Global.frame if gUsePawDetect and (gflp > 0 or gfrp > 0): gLastPawFrame = Global.frame return Global.frame - gLastObstacleFrame < 15\ or Global.frame - gLastStuckFrame < 15\ or Global.frame - gLastPawFrame < 15
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 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 doKickWithGrab(): global gSelectedKick if not sGrab.isGrabbed: # If the ball is in the edge, do more get behind. if hWhere.ballOnEdge(): r = sGrab.perform(sFindBall.GET_BEHIND_PRIORITY) else: r = sGrab.perform() if r == Constant.STATE_FAILED: hTrack.panLow = True return r elif r == Constant.STATE_EXECUTING: return r # Grab success else: # override kick selection selectedKick = sSelKick.perform() #print "kick reselected" # If the selected kick is different, then do kick reset. if gSelectedKick != None and selectedKick[0] != gSelectedKick[0]: doKickReset() gSelectedKick = selectedKick # If kick selection returns paw kick, change it to # grab dribble. # Because we can't paw kick or dribble from grab. kickType = gSelectedKick[0] if kickType == sSelKick.KT_DRIBBLE\ or kickType == sSelKick.KT_PAW_KICK\ or kickType == sSelKick.KT_AVOID_OWN_GOAL: gSelectedKick = (sSelKick.KT_GRAB_DRIBBLE, gSelectedKick[1], gSelectedKick[2]) r = Constant.STATE_EXECUTING if sGrab.isGrabbed: if not sGrab.isBallUnderChin(): return Constant.STATE_FAILED kickType, dkd, direction = gSelectedKick # Find a global co-ordinate on the dkd tx, ty = hMath.getPointRelative(Global.ballX, Global.ballY, dkd, 200) # Setting isClockwise isClockwise = None if direction == Constant.dCLOCKWISE: isClockwise = True elif direction == Constant.dANTICLOCKWISE: isClockwise = False if kickType == sSelKick.KT_GRAB_TURN_KICK: r = sGrabTurnKick.perform(False, Action.HeadTapWT, tx, ty) #r = sGrabDribble.performToDKD(dkd, 0) elif kickType == sSelKick.KT_GRAB_TURN_SHOOT: r = sGrabTurnKick.perform() elif kickType == sSelKick.KT_GRAB_DRIBBLE_GOAL: r = sGrabDribble.performToTargetGoal() elif kickType == sSelKick.KT_GRAB_DRIBBLE_STOP: r = sGrabDribble.performToDKD(dkd, minTimeToDribble=2500, dontKick=True) elif kickType == sSelKick.KT_GRAB_DRIBBLE: r = sGrabDribble.performToDKD(dkd) elif kickType == sSelKick.KT_TURN_KICK: r = sTurnKick.perform(dkd, isClockwise=isClockwise) elif kickType == sSelKick.KT_UPENN_LEFT: #print "grab upeen left" r = sUpennKick.perform(dkd, True) elif kickType == sSelKick.KT_UPENN_RIGHT: #print "grab upeen right" r = sUpennKick.perform(dkd, False) else: print "rAttacker.py : No appropriate kick for grab chosen??? ", kickType r = Constant.STATE_FAILED return r
def DecideNextAction(radius, aa, turndir, taOff, raOff, kp, ki, kd): global lastBx, lastBy, sum_reqTurn, last_reqTurn, lastFrame, lastDir skippedFrames = Global.frame - (lastFrame + 1) # Reset the running sum if the routine is not called. lastFrame = Global.frame # record the last frame Indicator.showFacePattern(Constant.FP_DIR_KICK) # No need to showFacePattern, because rAttacker has called already before entering this. myx, myy = Global.selfLoc.getPos() # My (the dog) position and its heading myh = Global.selfLoc.getHeading() if Global.vBall.getConfidence() > 2: bx, by = Global.vBall.getPos() # ball position lastBx, lastBy = Global.vBall.getPos() else: bx = lastBx by = lastBy # Find the lp, given the ball position, taOff and the attack angle # aa+180 because u want the tagental offset extended to behind the ball # the point where the dog leaves the locus (leaving point) lpx, lpy = hMath.getPointRelative(bx, by, (aa + 180), taOff) # Find the center of the circle, given the lp, the radius and attack angle, and where u are. angleFromBall = hMath.getHeadingBetween(bx, by, myx, myy) angleRelativeToAA = hMath.normalizeAngle_180(angleFromBall - aa) if angleRelativeToAA >= 0 and angleRelativeToAA <= 180: turndir = Constant.dANTICLOCKWISE else: turndir = Constant.dCLOCKWISE if turndir == Constant.dCLOCKWISE: cx, cy = hMath.getPointRelative(lpx, lpy, (aa - 90), (radius + raOff)) else: cx, cy = hMath.getPointRelative(lpx, lpy, (aa + 90), (radius + raOff)) c2my = hMath.getDistanceBetween(cx, cy, myx, myy) # distance btw me and center errL = radius - c2my # Indicates inside or outside of the circle. Used as adjustment in vector field. # If not outside of the circle, no point to use alpha(Angle between center and cloest tangent point) if c2my <= radius: alpha = 0 else: alpha = hMath.RAD2DEG(math.asin(radius / c2my)) # 0 <= alpha <= 90 beta = hMath.getHeadingBetween( myx, myy, cx, cy) # Angle between global x-axis and center # Fing the current vector if turndir == Constant.dANTICLOCKWISE: vectorArr = beta - 90 # Find the tangent vector perp to the c2my line first if errL > 0: vectorArr = vectorArr - (errL / radius * 45.0 ) # linear discrepency elif errL < 0: # When outside of the circle, the vector is vectorArr = beta - alpha # the tangent closest to the circle. else: vectorArr = beta + 90 if errL > 0: vectorArr = vectorArr + (errL / radius * 45.0) elif errL < 0: vectorArr = beta + alpha b2my = hMath.getDistanceBetween(bx, by, myx, myy) angleFromCenToMe = hMath.normalizeAngle_180( hMath.getHeadingBetween(cx, cy, myx, myy) - aa) distanceToAALine = b2my * math.sin(hMath.DEG2RAD(angleRelativeToAA)) ##~ verticalDistToBall = b2my * math.cos(hMath.DEG2RAD(angleRelativeToAA)) if (raOff > 0 and abs(distanceToAALine) < raOff): vectorArr = aa ##~ leftAdjust = -hMath.CLIP(distanceToAALine,3) elif (turndir == Constant.dANTICLOCKWISE and angleFromCenToMe < 0 and angleFromCenToMe > -90 and abs(distanceToAALine) < (raOff + radius)): sHoverToBall.DecideNextAction() Indicator.showFacePattern([1, 2, 1, 2, 0]) ##~ reverse(aa,turndir,taOff,verticalDistToBall) return elif (turndir == Constant.dCLOCKWISE and angleFromCenToMe < 90 and angleFromCenToMe > 0 and abs(distanceToAALine) < (raOff + radius)): sHoverToBall.DecideNextAction() Indicator.showFacePattern([1, 2, 1, 2, 0]) ##~ reverse(aa,turndir,taOff,verticalDistToBall) return # After the vector is found, the PID controller will handle the job. # Note: # When u tune, first tune the KP to improve the rise time, then the KD to improve # the overshoot, finally the KI for better steady state. # better see http:##www.engin.umich.edu/group/ctm/PID/PID.html before u touch them # thisTurn - The vector that brings the dog heading to the vector arrow thisTurn = hMath.CLIP(hMath.normalizeAngle_180(vectorArr - myh), 30.0) # For normal walk , kp = 40 ki = 8 kd = 5 KP = kp / 100.0 #0.5 ## Less Than 1 KD = kd / 100.0 #0.1 KI = ki / 100.0 #0.1 ## Has to be very small if skippedFrames > 0: sum_reqTurn = sum_reqTurn * math.pow(0.8, skippedFrames) last_reqTurn = 0 # clip the running sum to be as most causing a 8 degree change. if abs(sum_reqTurn) > (3.0 / KI): sum_reqTurn = hMath.CLIP(sum_reqTurn, 3.0 / KI) # if far away, there is no I and D effects, only the P. if c2my > (4.0 * radius): sum_reqTurn = 0 last_reqTurn = thisTurn # The real PID calculations #print thisTurn #print last_reqTurn #print sum_reqTurn ##~ print "==========new frame" ##~ print "ball pos %.2f , %.2f and my h %.2f" % (bx,by,myh) ##~ print "lp pos %.2f , %.2f and cx pos %.2f , %.2f" % (lpx,lpy,cx,cy) ##~ print "vectorArr %.2f and thisTurn %.2f " % (vectorArr,thisTurn) ##~ print "last %.2f and sum_reqTurn %.2f) " % (last_reqTurn,sum_reqTurn) ##~ print "P %.5f I %.5f D %.5f " % (KP * thisTurn,KI * sum_reqTurn,\ ##~ KD * (thisTurn - last_reqTurn)) ##~ reqTurn = (KP * thisTurn) + (KI * sum_reqTurn) + (KD * (thisTurn - last_reqTurn)) ##~ print "the reqTurn is %.2f" % (reqTurn) sum_reqTurn = sum_reqTurn + thisTurn # The integral part last_reqTurn = thisTurn # Used in the derivative part # Print the debugging info Action.walk(7, 0, hMath.CLIP(reqTurn, 30), Action.EllipticalWalk) ##~ print "Direction: " , turndir , "vectorArr %.2f" % (vectorArr) if ((angleRelativeToAA > 160 or angleRelativeToAA < -160) and abs(hMath.normalizeAngle_180(myh - aa)) < 20): sPawKick.firesPawKick(sPawKick.FIRE_PAWKICK_AUTO) Indicator.showFacePattern([1, 1, 1, 1, 0])