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 saGoToTargetFacingHeading(targetX, targetY, targetH, \ maxSpeed = Action.MAX_FORWARD, maxTurn = Action.MAX_TURN): # Variables relative to self localisation. selfPos = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() cosSelf = math.cos(math.radians(selfH)) sinSelf = math.sin(math.radians(selfH)) relX = targetX - selfPos[0] relY = targetY - selfPos[1] relH = hMath.normalizeAngle_180(targetH - selfH) # now take into account that as we turn, the position of our head will change relX += Constant.DOG_LENGTH / 2.0 * (cosSelf - math.cos(math.radians(targetH))) relY += Constant.DOG_LENGTH / 2.0 * (sinSelf - math.sin(math.radians(targetH))) relD = hMath.getLength((relX, relY)) # Calculate approximate coordinates of all the dog's legs legs = [] for legNum in range(4): legPos = list(selfPos) if legNum >= 2: legPos[0] = legPos[0] - DOG_RECT_LENGTH * cosSelf legPos[1] = legPos[1] - DOG_RECT_LENGTH * sinSelf if (legNum % 2) == 0: legPos[0] = legPos[0] - DOG_RECT_WIDTH * sinSelf legPos[1] = legPos[1] + DOG_RECT_WIDTH * cosSelf else: legPos[0] = legPos[0] + DOG_RECT_WIDTH * sinSelf legPos[1] = legPos[1] - DOG_RECT_WIDTH * cosSelf legs.append(tuple(legPos)) # If a leg coordinate lies within the rear wall, clip the Y movement clipMove = False for legPos in legs: if abs(legPos[0] - Constant.FIELD_WIDTH / 2) > Constant.GOAL_WIDTH and\ legPos[1] < 2: clipMove = True if clipMove and (relY < 0): relY = 0 if relX == 0 and relY == 0: #on the dog, math.atan2(0,0) give "Value Error: math domain error" relTheta = 0 else: relTheta = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = hMath.CLIP(relD, maxSpeed) * math.cos(hMath.DEG2RAD(relTheta)) left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta)) turnCCW = hMath.CLIP(relH, maxTurn) Action.walk(forward, left, turnCCW, minorWalkType=Action.SkeFastForwardMWT)
def perform(): global gLastFrameCalled global gDirection global gNoGetBehindCounter if gLastFrameCalled != Global.frame - 1: resetPerform() gLastFrameCalled = Global.frame gNoGetBehindCounter = max(gNoGetBehindCounter - 1, 0) selfLoc = Global.selfLoc selfX, selfY = selfLoc.getPos() selfH = selfLoc.getHeading() ballX, ballY = Global.ballX, Global.ballY ballD = Global.ballD ballH = Global.ballH selfH2BallH = selfH + ballH ballH2OGoalH = hMath.getHeadingBetween(ballX,ballY,\ Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y) selfH2OGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2OGoalH) selfH2SafeH = hMath.normalizeAngle_180(selfH2OGoalH + 180) if abs(selfH2OGoalH) > 110: resetPerform() return Constant.STATE_SUCCESS # If we are far away from the ball, then just use our implicit get behind. #if Global.ballD > 60: # sFindBall.perform(doGetBehind = sFindBall.GET_BEHIND_LOTS) # # Reset the direction. # gDirection = None # return Constant.STATE_EXECUTING # First time to enter the function, so choose the direction. if gDirection == None: # go left if selfH2OGoalH < 0: gDirection = Constant.dANTICLOCKWISE # go right else: gDirection = Constant.dCLOCKWISE if gNoGetBehindCounter > 0: sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY) else: sFindBall.perform(True) r = sGetBehindBall.performBall(dist=25, direction=gDirection, accuracy=10) if r == Constant.STATE_SUCCESS: sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY) gNoGetBehindCounter = NO_GET_BEHIND_DURATION return Constant.STATE_EXECUTING
def findDefendPtAngle(ballX, ballY): if ballX <= 0: ballX = 0.01 if ballY <= 0: ballY = 0.01 if Debug.goalieDebug: print "findDefendPtAngle: ball(", ballX, ",", ballY, ")" ballX = ballX - Constant.OWN_GOAL_X # ballX relative to goal centre global defendAngle, defendPt # It's not that complex, really. We have a defensive line a fixed distance # in front of the goal line. We draw a ray from the ball to the centre of # the goal mouth and stand at the intersection of the ray with the # line defendAngle = math.atan2(ballY, ballX) defendAngle = math.degrees(defendAngle) - 90.0 defendAngle = hMath.normalizeAngle_180(defendAngle) defendAngle = hMath.CLIP(defendAngle, 90) global FORWARD_OFFSET if abs(defendAngle) < 45: FORWARD_OFFSET = 10 else: FORWARD_OFFSET = 3 defendAngle = hMath.CLIP(defendAngle, 60) defendPt = ballX - (ballX * ((ballY - FORWARD_OFFSET) / ballY)) defendPt = hMath.CLIPTO(defendPt, -Constant.GOAL_WIDTH / 2.0 + 5, Constant.GOAL_WIDTH / 2.0 - 5)
def reverse(aa, turndir, taOff, verticalDistToBall): adjustTurn = hMath.CLIP( hMath.normalizeAngle_180(Global.selfLoc.getHeading() - aa), 10) adjustTurn *= -1 ##~ correctionLen = max (0,taOff-2*Constant.BallDiameter) if (verticalDistToBall > -Constant.BallDiameter): Action.walk(-6, 0, adjustTurn) ##~ print "uppest" ##~ elif (verticalDistToBall > (Constant.BallDiameter-taOff) ): elif (verticalDistToBall > -taOff / 2.0): ##~ correctAmp = correctionLen-abs(Constant.BallDiameter+verticalDistToBall) ##~ reverseSpeed = -6.0 * correctAmp / correctionLen if (turndir == Constant.dANTICLOCKWISE): Action.walk(-3, -4, adjustTurn) else: Action.walk(-3, 4, adjustTurn) ##~ print "middle" else: if (turndir == Constant.dANTICLOCKWISE): Action.walk(-2, -4, adjustTurn) else: Action.walk(-2, 4, adjustTurn)
def perform(leftAngle,rightAngle,heading): global gLastStealth forward = Action.MAX_FORWARD_NORMAL left = 0 turn = heading badHeading = (leftAngle + rightAngle) / 2.0 trueHeading = hMath.normalizeAngle_180(Global.selfLoc.getHeading() + heading) if heading < badHeading + 10 * gLastStealth: if True or not (trueHeading < -90 or trueHeading > 120): stealthTurn = rightAngle - 45 if stealthTurn < turn: turn = stealthTurn gLastStealth = 1 Indicator.showFacePattern([3,3,0,0,0]) else: if True or not (trueHeading > -90 and trueHeading < 60): stealthTurn = leftAngle + 45; if stealthTurn > turn: turn = stealthTurn gLastStealth = -1 Indicator.showFacePattern([0,0,0,3,3]) turnccw = hMath.CLIP(turn / 2.0, Action.MAX_TURN_NORMAL) Action.walk(forward,left,turnccw)
def SmartSetBeacon(panLimit=90, distLimit=550): global localiseBeacon # Get shared variables from elsewhere. selfLoc = Global.selfLoc cov = VisionLink.getGPSSelfCovariance() # Calculate the direction of the larger axis of the self covariance b = -(cov[0][0] + cov[1][1]) c = cov[0][0] * cov[1][1] - cov[1][0] * cov[1][0] det = b * b - 4 * c if (det < 0.00001): sol = -b / 2 else: sol = (-b + math.sqrt(det)) / 2 if abs(cov[0][0] - sol) < 0.01 and cov[0][1] < 0.01: # If self position variance is circular, all beacons are equally good # to localise off, and so choose the one closest to current heading. if abs(cov[1][1] - sol) < 0.01: head = selfLoc.getHeading() else: head = hMath.RAD2DEG(math.atan2(-cov[1][0], cov[1][1] - sol)) else: head = hMath.RAD2DEG(math.atan2(sol - cov[0][0], cov[0][1])) # Find the beacon that is closest to the variance direction. beaconPos = VisionLink.getGPSCoordArray() curCompHead = 360 localiseBeacon = 0 for beacon in CHECK_BEACONS: relPos = map(lambda b, s: b - s, beaconPos[beacon], selfLoc.getPos()) beaconHead = hMath.RAD2DEG(math.atan2(relPos[1], relPos[0])) beaconDist = hMath.getLength(relPos) localHead = hMath.normalizeAngle_180(beaconHead - selfLoc.getHeading()) if abs(localHead) < panLimit and beaconDist < distLimit: compHead = abs(hMath.normalizeAngle_180(head - beaconHead)) if compHead > 90: compHead = 180 - compHead if compHead < curCompHead: curCompHead = compHead localiseBeacon = beacon
def saGoToTargetFacingHeading(targetX, targetY, targetH, \ maxSpeed = Action.MAX_FORWARD, maxTurn = Action.MAX_TURN): # ariables relative to self localisation. selfX = Global.selfLoc.getX() selfY = Global.selfLoc.getY() selfH = Global.selfLoc.getHeading() relX = targetX - selfX relY = targetY - selfY relH = hMath.normalizeAngle_180(targetH - selfH) relX += Constant.DOG_LENGTH/2.0/10.0*(math.cos(math.radians(selfH)) \ - math.cos(math.radians(targetH))) relY += Constant.DOG_LENGTH/2.0/10.0*(math.sin(math.radians(selfH)) \ - math.sin(math.radians(targetH))) relD = hMath.getLength((relX, relY)) distanceSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) inCircle = distanceSquared <= hMath.SQUARE(40) faceH = hMath.getHeadingToFaceAt(Global.selfLoc, targetX, targetY) ##~ print "faceH: ", faceH if not inCircle: if abs(faceH) >= 30: Action.walk(0, 0, faceH) else: #if sStealthDog.stealthDog(True): # hFWHead.compulsoryAction = hFWHead.mustSeeBall # hTrack.trackVisualBall() # return Action.walk(maxSpeed, 0, hMath.CLIP(faceH / 1.5, maxTurn)) else: if relX == 0 and relY == 0: # On the dog, math.atan2(0,0) give "Value Error: math domain error". relTheta = 0 else: relTheta = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = hMath.CLIP(relD, maxSpeed) * math.cos( hMath.DEG2RAD(relTheta)) left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta)) turnCCW = hMath.CLIP(relH, maxTurn) Action.walk(forward, left, turnCCW)
def GetReadyGenerator(): selfx = Global.selfLoc.getX() selfy = Global.selfLoc.getY() selfh = hMath.normalizeAngle_180(Global.selfLoc.getHeading()) centerx = Constant.FIELD_WIDTH / 2 centery = Constant.FIELD_LENGTH / 2 heading = hMath.getHeadingBetween(selfx,selfy,centerx,centery) # 1st quad if selfx <= centerx and selfy <= centery: turn = heading - selfh # 2nd quad elif selfx <= centerx and selfy >= centery: turn = heading + selfh # 3rd quad elif selfx >= centerx and selfy <= centery: turn = - heading + selfh # 4th quad else: turn = - heading - selfh print "Turn Angle is " + str(turn) + " Heading is " + str(heading) period = turn / 30 * 8 for _ in range(period): sGrabWalk.Perform(0,0,30) yield Constant.STATE_EXECUTING for _ in range(15): Action.kick(Action.DiveKickWT) yield Constant.STATE_EXECUTING Action.forceStepComplete() i = 0 while 1: i += 1 if Global.vBall.getConfidence() > 0: # Wait at least one second before checking this.. # So that the ball goes somewhere far if i > 75\ and Global.vBall.getDistance() < 60\ and Global.haveBall > 5: break turn = hMath.CLIP(Global.vBall.getHeading(),20) Action.walk(3,0,turn) hTrack.trackVisualBall() else: Action.walk(3,0,5) hTrack.spinningLocalise() yield Constant.STATE_EXECUTING while 1: yield Constant.STATE_SUCCESS
def getGPSBallInfo(context): if context == Constant.CTGlobal: (myx, myy, myh) = (0, 0, 0) else: (myx, myy, myh) = getSelfLocation() myh -= 90 (x, y) = (ball.getX() - myx, ball.getY() - myy) ang = hMath.normalizeAngle_180(hMath.RAD2DEG(math.atan2(y, x)) - myh) return (x, y, math.sqrt(x * x + y * y), ang, ang - 90, 0)
def saGoOnHeading(head, maxSpeed=10, maxTurn=30): relH = hMath.normalizeAngle_180(head - Global.selfLoc.getHeading()) + 90 forward = maxSpeed * math.cos(relH) left = maxSpeed * math.sin(relH) # turnCCW = relH turnCCW = 0 print "globalH", head, "myH", Global.selfLoc.getHeading(), "relH", relH Action.walk(hMath.CLIP(forward, maxSpeed), hMath.CLIP(left, maxSpeed), \ hMath.CLIP(turnCCW, maxTurn))
def performToDKD(dkd, minTimeToDribble=1500): global gForceToTargetGoal global gForceTimeElapse Indicator.showFacePattern([1, 0, 0, 0, 1]) if not sGrab.isGrabbed: resetPerform() return Constant.STATE_FAILED # If we are already doing edge behaviour, then continue. if gEdgeCounter > 0 and performEdge() == Constant.STATE_EXECUTING: return Constant.STATE_EXECUTING # If we are already doing dodging behaviour, then continue. if gDodgeCounter > 0 and performDodge() == Constant.STATE_EXECUTING: return Constant.STATE_EXECUTING if gForceTimeElapse\ or hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE: return performTimeElapsedAction(SEL_GPS) if performEdge() == Constant.STATE_EXECUTING: return Constant.STATE_EXECUTING turnCCW = hMath.normalizeAngle_180(dkd - Global.selfLoc.getHeading()) # If we are lined up to the dkd, do something smart. if abs(turnCCW) <= 10: # Dodge, if we need to. if performDodge() == Constant.STATE_EXECUTING: return Constant.STATE_EXECUTING # If we exceed the minTimeToDribble, then release the ball. if Global.frame - gLastTurnFrame >= TURN_DURATION\ and hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > minTimeToDribble: return performTimeElapsedAction(SEL_GPS) # If we are in outside third, use diagonal grab dribbling. selfX = Global.selfLoc.getX() if selfX < Constant.LEFT_GOAL_POST or selfX > Constant.RIGHT_GOAL_POST: if (0 < dkd < 180): left = hMath.CLIP(selfX - Constant.FIELD_WIDTH * 0.5, Action.MAX_SKE_LEFT_STP * 0.5) else: left = hMath.CLIP(Constant.FIELD_WIDTH * 0.5 - selfX, Action.MAX_SKE_LEFT_STP * 0.5) else: left = 0 return performDribble(Action.MAX_FORWARD, left, turnCCW)
def amIInTheWay(x, y, targetX, targetY): myX, myY = Global.selfLoc.getPos() if x == None or y == None or targetX == None or targetY == None: return (None, None) # if Global.frame % 30 == 0: # print "amIInTheWay", x, y, targetX, targetY, "me", myX, myY # We have a line in two point form, find offset of selfLoc from the line dist = ((targetX - x)*(y - myY) - (x - myX)*(targetY - y)) / \ math.sqrt(hMath.SQUARE(targetX - x) + hMath.SQUARE(targetY - y)) # if Global.frame % 30 == 0: # print "I'm", dist, "away from in the way", # If we're not between the points return None dx = x - targetX dy = y - targetY if abs(dx) > abs(dy) and \ ((myX > x and myX > targetX) or (myX < x and myX < targetX)): # if Global.frame % 30 == 0: # print "but not between the points (X)" return (None, None) elif abs(dy) > abs(dx) and \ ((myY > y and myY > targetY) or (myY < y and myY < targetY)): # if Global.frame % 30 == 0: # print "but not between the points (Y)" return (None, None) # The heading away is perpendicular to the target line if dy == 0: dy += 0.1 # prevent division by zero head = -dx / dy head = hMath.RAD2DEG(math.atan(head)) # Which way along the line is away? If dist is positive we are to the left # of the line and want heading in the left half if (dist > 0): head += 180 head = hMath.normalizeAngle_180(head) if Global.frame % 30 == 0: print "dx =", dx, "dy =", dy, "head =", head # if Global.frame % 30 == 0: # print "and between the points. Head", head, "to escape" return (dist, head)
def saGoToTargetFacingHeading(targetX, targetY, targetH, \ maxSpeed = None, maxTurn = None): if maxSpeed == None: maxSpeed = Action.MAX_FORWARD if maxTurn == None: maxTurn = Action.MAX_TURN # Variables relative to self localisation. selfPos = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() relX = targetX - selfPos[0] relY = targetY - selfPos[1] relH = hMath.normalizeAngle_180(targetH - selfH) # take into account that as we turn, the position of our head will change relX += Constant.DOG_NECK_TO_BODY_CENTER_OFFSET * \ (math.cos(math.radians(selfH)) - math.cos(math.radians(targetH))) relY += Constant.DOG_NECK_TO_BODY_CENTER_OFFSET * \ (math.sin(math.radians(selfH)) - math.sin(math.radians(targetH))) relD = hMath.getLength((relX, relY)) if relX == 0 and relY == 0: relTheta = 0 else: relTheta = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = hMath.CLIP(relD, maxSpeed) * math.cos(hMath.DEG2RAD(relTheta)) left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta)) turnccw = hMath.CLIP(relH, maxTurn) Action.walk(forward, left, turnccw, "ssd", minorWalkType=Action.SkeFastForwardMWT)
def AlternateSetBeacon(panLimit=90, distLimit=550): global localiseBeacon, lastBeacon selfLoc = Global.selfLoc beaconPos = getBeaconCoords() curCompHead = panLimit localiseBeacon = None for beacon in beaconPos: relPos = map(lambda b, s: b - s, beacon, selfLoc.getPos()) beaconHead = hMath.RAD2DEG(math.atan2(relPos[1], relPos[0])) beaconDist = hMath.getLength(relPos) localHead = hMath.normalizeAngle_180(beaconHead - selfLoc.getHeading()) if abs(localHead) < curCompHead and beaconDist < distLimit: curCompHead = localHead localiseBeacon = beacon lastBeacon = localiseBeacon
def determineBallSource(): global ballSource, ballX, ballY, ballD, ballH #-------------------------------------- # NOTE: ballX and ballY are global coordinate # BUT: ballH and ballD are relative to the robot neck base # Want a local one? Go to hMath.getLocalCoordinate (selfX, selfY) = selfLoc.getPos() selfH = selfLoc.getHeading() # If vision ball is available, use it since it is the most accurate. if vBall.isVisible(): ballSource = Constant.VISION_BALL ballX, ballY = vBall.getPos() ballH = vBall.getHeading() ballD = vBall.getVobDistance() # Check if wireless ball is accurate. # Maybe teammate got lost, then their "reliable" ball's gps coord will # be wrong. elif (sharedBallVar < (VisionLink.getGPSBallMaxVar() + \ hMath.get95var(Constant.USE_WIRELESS_BALL_VAR)) and lostBall > Constant.VBALL_LOST): ballSource = Constant.WIRELESS_BALL ballX = sharedBallX ballY = sharedBallY ballH = hMath.normalizeAngle_180(hMath.getHeadingBetween\ (selfX, selfY, ballX, ballY) - selfH) ballD = hMath.getDistanceBetween(ballX, ballY, selfX, selfY) # Use gps ball if nothing better. else: ballSource = Constant.GPS_BALL ballX, ballY = gpsGlobalBall.getPos() ballH = gpsLocalBall.getHeading() ballD = gpsLocalBall.getDistance() # Stop divides by zero if ballX == 0: ballX = 0.01 if ballY == 0: ballY = 0.01 if ballD == 0: ballD = 0.01 if ballH == 0: ballH = 0.01
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 saGoToTarget(targetX, targetY, maxSpeed=None, maxTurn=None): if maxSpeed == None: maxSpeed = Action.MAX_FORWARD if maxTurn == None: maxTurn = Action.MAX_TURN selfPos = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() relX = targetX - selfPos[0] relY = targetY - selfPos[1] relD = hMath.getLength((relX, relY)) if relX == 0 and relY == 0: relH = 0 else: relH = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = 0 left = 0 turnccw = 0 if relD < CLOSE_WALK_RANGE and abs(relH) > 50: forward = relY left = relX turnccw = relH * 0.8 # print "close walk and turn",relY,left,turnccw else: if abs(relH) > 80: # stop and turn turnccw = relH * 0.8 elif abs(relH) > 25: # walk and turn turnccw = relH * 0.8 forward = hMath.CLIP(relD, maxSpeed) * math.cos( hMath.DEG2RAD(relH)) # print "walk and turn",forward, 0, turnccw else: # ellipticalwalk and turn turnccw = hMath.CLIP(relH * 0.8, 25) forward = hMath.CLIP(relD, maxSpeed) * math.cos( hMath.DEG2RAD(relH)) # finally, we walk Action.walk(forward, left, turnccw, minorWalkType=Action.SkeFastForwardMWT)
def timeToReachPoint(x, y, h, myX=None, myY=None, myH=None,\ doTurn=True, doDKD=True, doObs=True): if myX == None: myX = Global.selfLoc.getX() myY = Global.selfLoc.getY() myH = Global.selfLoc.getHeading() relH = hMath.getHeadingToMe(myX, myY, myH, x, y) absH = hMath.getHeadingBetween(myX, myY, x, y) (lX, lY) = hMath.getLocalCoordinate(myX, myY, myH, x, y) # 1. Estimate the straight line time to get there # 2. Add time to turn and face (or slow walk due to turning on the run) # 3. Add time to face desired heading when we arrive (or getBehind) # 4. Add time for obstacles if there are any in the way # if (Global.frame % 10 == 0): # print "dist:", hMath.getDistanceBetween(myX, myY, x, y) /EST_WALK_SPEED # print "turn:", abs(relH) /EST_TURN_SPEED # print "dkd:", abs(hMath.normalizeAngle_180(absH - h)) /EST_TURN_SPEED time = hMath.getDistanceBetween(myX, myY, x, y) / EST_WALK_SPEED if doTurn: time += abs(relH) / EST_TURN_SPEED if doDKD: # Time to turn to dkd when we get there is penalised more harshly time += (abs(hMath.normalizeAngle_180(absH - h)) / EST_TURN_SPEED) * 1.3 if doObs: nobs = VisionLink.getNoObstacleBetween(0, 0, int(lX), int(lY), 30, 30, 0, Constant.OBS_USE_NONE) if nobs > 100: nobs = 50 #if x == Global.ballX: # print "Nobs=", nobs if nobs >= 5: # 10ms per obstacle point. A typical half-obstructed path might # count 60 obs -> 600ms. Max at nobs 100 -> 1 sec time += nobs * 10 return int(time)
def saGoToTarget(targetX, targetY, maxSpeed = Action.MAX_FORWARD, \ maxTurn = Action.MAX_TURN): selfX, selfY = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() relX = targetX - selfX relY = targetY - selfY relD = hMath.getLength((relX, relY)) distanceSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) inCircle = distanceSquared <= hMath.SQUARE(40) faceH = hMath.getHeadingToFaceAt(Global.selfLoc, targetX, targetY) if not inCircle and abs(faceH) >= 30: Action.walk(0, 0, hMath.CLIP(faceH, maxTurn)) elif not inCircle: #if sStealthDog.stealthDog(True): # hFWHead.compulsoryAction = hFWHead.mustSeeBall # hTrack.trackVisualBall() # return Action.walk(maxSpeed, 0, hMath.CLIP(faceH / 1.5, maxTurn)) else: if relX == 0 and relY == 0: relTheta = 0 else: relTheta = hMath.normalizeAngle_180( hMath.RAD2DEG(math.atan2(relY, relX)) - selfH) forward = hMath.CLIP(relD, maxSpeed) * math.cos( hMath.DEG2RAD(relTheta)) left = hMath.CLIP(relD, maxSpeed) * math.sin(hMath.DEG2RAD(relTheta)) turnCCW = hMath.CLIP(relTheta, maxTurn) Action.walk(forward, left, turnCCW)
def walk(target, selfPos, selfH): relX = target[0] - selfPos[0] relY = target[1] - selfPos[1] relD = hMath.getLength((relX, relY)) if relX == 0 and relY == 0: relThetaRad = 0 else: relThetaRad = hMath.DEG2RAD(hMath.normalizeAngle_180(hMath.RAD2DEG(math.atan2(relY, relX)) - selfH)) sinTheta = math.sin(relThetaRad) cosTheta = math.cos(relThetaRad) leftSign = relD * sinTheta leftAbs1 = abs(leftSign) #leftAbs1: the distance left component if cosTheta != 0: #leftAbs2: the max capping due to max forward leftAbs2 = abs(maxFwd * (sinTheta / cosTheta)) else: leftAbs2 = 100000 # huge number #print "left1:", leftAbs1, "left2:", leftAbs2 leftAbs = min(leftAbs1, leftAbs2, maxLeft) left = hMath.getSign(leftAbs, leftSign) if sinTheta != 0: forward = left * (cosTheta / sinTheta) else: fwdSign = relD fwdAbs = min(maxFwd, abs(relD)) forward = hMath.getSign(fwdAbs, fwdSign) if turningLeft: turnCCW = maxTurn else: turnCCW = -maxTurn print __name__, forward, left, turnCCW Action.walk(forward, left, turnCCW,walkType=Action.SkellipticalWalkWT)
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 perform(tx, ty, ta=45): global gIsBirdOfPreyTriggering, targetX, targetY, targetDSquared, targetH global gLastCalledFrame global boxPointOffset,\ cornerCut,\ maxTurn,\ insideBound,\ outsideBound,\ lastDBH,\ currentBopMode,\ targetDesiredAngle,\ lockDefender,\ minAngle,\ breakOutMinAngle,\ breakOutIncAngle targetX, targetY = tx, ty targetH = hMath.normalizeAngle_180(hMath.getHeadingBetween\ (Global.selfLoc.getX(), Global.selfLoc.getY(), targetX, targetY)\ - Global.selfLoc.getHeading()) targetDSquared = hMath.getDistSquaredBetween(targetX, targetY, \ Global.selfLoc.getX(), Global.selfLoc.getY()) targetDesiredAngle = ta if shouldIEnd(): resetPerform() return Constant.STATE_SUCCESS Indicator.showFacePattern([1, 2, 1, 2, 1]) gLastCalledFrame = Global.frame gIsBirdOfPreyTriggering = True # Checking preconditions. desiredTargetAngle = targetDesiredAngle goalx = (Constant.FIELD_WIDTH / 2 + targetX) / 2 dX = targetX - goalx dY = targetY # Determine if we are around the right side of the target. usToGoal = math.sqrt(hMath.SQUARE(Global.selfLoc.getPos()[0] - goalx) + \ hMath.SQUARE(Global.selfLoc.getPos()[1])) targetToGoal = math.sqrt(hMath.SQUARE(dX) + hMath.SQUARE(dY)) if abs(dX) < 0.1: if Debug.defenderDebug: print "pa" if Global.selfLoc.getPos()[0] < goalx: if Debug.defenderDebug: print "pb" desiredBH = desiredTargetAngle else: if Debug.defenderDebug: print "pc" desiredBH = -desiredTargetAngle else: if Debug.defenderDebug: print "pd" m = dY / dX b = targetY - m * targetX if abs(m) < 0.01: desiredBH = 0 elif dY < 0: # This is an error that appeared; target must be close to back line. if targetX > Constant.FIELD_WIDTH / 2: desiredBH = desiredTargetAngle else: desiredBH = -desiredTargetAngle elif lastDBH > 0: if Global.selfLoc.getPos()[0] < ( (Global.selfLoc.getPos()[1] - b) / m) + 5: desiredBH = desiredTargetAngle else: desiredBH = -desiredTargetAngle else: if Global.selfLoc.getPos()[0] < ( (Global.selfLoc.getPos()[1] - b) / m) - 5: desiredBH = desiredTargetAngle else: desiredBH = -desiredTargetAngle if usToGoal < targetToGoal: dp = (Global.selfLoc.getPos()[0] - goalx) * dX \ + Global.selfLoc.getPos()[1] * dY # use dot product to calculate angle between # robot vector and target vector (both from defend pt) cosAng = dp / (usToGoal * targetToGoal) ang = math.acos(cosAng) dist = usToGoal * math.sin(ang) if math.degrees(ang) < minAngle: minAngle = math.degrees(ang) if targetDSquared < hMath.SQUARE(insideBound * 2)\ or dist < insideBound\ or (currentBopMode == Constant.CIRCLE_BOP and dist < outsideBound): if currentBopMode == Constant.CIRCLE_BOP: if lastDBH > 0: turnCCW = maxTurn else: turnCCW = -maxTurn else: if desiredBH > 0: turnCCW = maxTurn else: turnCCW = -maxTurn currentBopMode = Constant.CIRCLE_BOP Action.walk(Action.MAX_FORWARD, 0, turnCCW, minorWalkType=Action.SkeFastForwardMWT) return Constant.STATE_EXECUTING continue_(desiredBH) # function too long for python - so split it in two return Constant.STATE_EXECUTING
def perform(dkd = 90, side = None, bx = None, by = None): # This implementation is very similar to sGetBehindBall (based on 2003) # but the ball is on the bottom edge of the circle, not the centre. global onCircle if side != None: print "Warning: sGetBesideBall.perform: side is not yet implemented" if bx == None or by == None: (bx, by) = Global.gpsGlobalBall.getPos() (myx, myy) = Global.selfLoc.getPos() myh = Global.selfLoc.getHeading() # Determine the centre of the circle, which is CIRCLE_RADIUS towards # dkd from the ball. Global coords. cx = bx + math.cos(hMath.DEG2RAD(dkd)) * CIRCLE_RADIUS cy = by + math.sin(hMath.DEG2RAD(dkd)) * CIRCLE_RADIUS # If we are backward of the ball or really close just run at it ballRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy) ballDSquared = hMath.getDistSquaredBetween(myx, myy, bx, by) if (abs(ballRobotH) > 90 or ballDSquared < hMath.SQUARE(20)): Indicator.showHeadColor(Indicator.RGB_PURPLE) ballH = hMath.getHeadingBetween(myx, myy, bx, by) hTrack.saGoToTargetFacingHeading(bx, by, ballH) return # Work out if we are left or right of the centre (relative to DKD as 0) robotH = hMath.getHeadingToMe(cx, cy, dkd, myx, myy) # FIXME: allow choice of direction if (robotH > 0): # robot to the left #print "robot to left of ball", direction = Constant.dANTICLOCKWISE else: # robot to the right #print "robot to right of ball", direction = Constant.dCLOCKWISE # The circling point can be calculated as looking from the centre # towards the robot at CIRCLE_DEGREES to the left/right, and distance # CIRCLE_RADIUS. CircleAng is from the centre facing the robot with # positive x at zero degrees. # There are two modes here. In the first we are well outside the and # running to make a tangent with the circle. In the second we are close # to or inside the circle and tracing the circumference centreDSquared = hMath.getDistSquaredBetween(myx, myy, cx, cy) if (centreDSquared > hMath.SQUARE(CIRCLE_RADIUS + 20)): #print "Outside circle, running to tangent" onCircle = False Indicator.showHeadColor(Indicator.RGB_GREEN) if direction == Constant.dANTICLOCKWISE: circleAng = 90 + CIRCLE_DEGREES else: circleAng = 90 - CIRCLE_DEGREES circleAng = hMath.normalizeAngle_180(circleAng) else: #print "On circle, tracing circumference" onCircle = True Indicator.showHeadColor(Indicator.RGB_YELLOW) if direction == Constant.dANTICLOCKWISE: circleAng = 110 else: circleAng = 70 # print "me", int(myx), int(myy), "ball", int(bx), int(by), \ # "centre", int(cx), int(cy), "robotH", int(robotH), # relative to centre facing robot circleRelX = math.cos(hMath.DEG2RAD(circleAng)) * CIRCLE_RADIUS circleRelY = math.sin(hMath.DEG2RAD(circleAng)) * CIRCLE_RADIUS #print "circleAng", circleAng, "rel circle pos", circleRelX, circleRelY robotH = hMath.normalizeAngle_180(robotH + dkd) # now global (circleX, circleY) = hMath.getGlobalCoordinate(cx, cy, robotH, \ circleRelX, circleRelY) # print "gRobotH", robotH, "circle pos", int(circleX), int(circleY) # circleX/Y now is the global coords of the circle point, so walk there. # ballH = hMath.getHeadingBetween(myx, myy, bx, by) # global if onCircle: # Calls the walk directly to ensure smoothness: no stopping to turn relX = circleX - myx relY = circleY - myy relD = hMath.getLength((relX, relY)) relTheta = hMath.RAD2DEG(hMath.getHeadingToRelative(relX, relY)) # Don't turn outwards much, even if we are inside the circle. Walking # forward will put us back on it. Nobu can you fix this? # print "relTheta", relTheta, "=>", # if direction == Constant.dANTICLOCKWISE and relTheta < 0: # #relTheta = hMath.CLIP(relTheta, 15) # relTheta = 0 # elif direction == Constant.dCLOCKWISE and relTheta > 0: # #relTheta = hMath.CLIP(relTheta, 15) # relTheta = 0 # print relTheta left = 0 if abs(relTheta) < 30: Action.walk(Action.MAX_FORWARD, left, relTheta) else: Action.walk(Action.MAX_FORWARD, left, relTheta) else: hTrack.saGoToTarget(circleX, circleY)
def performDKD(dkd, isClockwise): selfH = Global.selfLoc.getHeading() direction = hMath.normalizeAngle_180(dkd - selfH) return performDirection(direction, isClockwise)
def performToDKD(dkd, minTimeToDribble=1500, dontKick=False): global gLastActionFrame global gForceToTargetGoal global gForceTimeElapse #print "performToDKD elapsed =",\ # hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) if not sGrab.isGrabbed: print "Grab failed" resetPerform() return Constant.STATE_FAILED Indicator.showFacePattern([1, 0, 0, 0, 1]) gLastActionFrame = Global.frame # If we need to not kick at the end of this dribble, make sure here if dontKick and hMath.getTimeElapsed( sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE: #print "Time elapsed performing SEL_RELEASE" return performTimeElapsedAction(SEL_RELEASE) # If we can see the goal and we are in offensive half, # then let performToTargetGoal take over. # It may only take over if gForceTimeElapse is false. if not dontKick\ and (gForceToTargetGoal or (not gForceTimeElapse and Global.vTGoal.isVisible() and Global.selfLoc.getY() > Constant.FIELD_LENGTH * 0.6) or hWhere.ballInTargetGoalBox()): gForceToTargetGoal = True return performToTargetGoal() # If we are already doing edge behaviour, then continue. if gEdgeCounter > 0 and performEdge() == Constant.STATE_EXECUTING: return Constant.STATE_EXECUTING # If we are already doing dodging behaviour, then continue. if gDodgeCounter > 0 and performDodge() == Constant.STATE_EXECUTING: return Constant.STATE_EXECUTING # If the grab time has elapsed, then release if gForceTimeElapse\ or hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE: return performTimeElapsedAction(SEL_OFFENSIVE) if performEdge() == Constant.STATE_EXECUTING: return Constant.STATE_EXECUTING turnCCW = hMath.normalizeAngle_180(dkd - Global.selfLoc.getHeading()) # If we are lined up to the dkd, do something smart. if abs(turnCCW) <= 5 and not dontKick: # If we see our own goal and we are facing the right way (it's gotta be # our gps is stuffed up!), then do avoid goal move. if (not Global.lightingChallenge) and Global.vOGoal.isVisible(): return performTimeElapsedAction(SEL_DEFENSIVE) # Dodge, if we need to. if performDodge() == Constant.STATE_EXECUTING: return Constant.STATE_EXECUTING # If we exceed the minTimeToDribble, then release the ball. if Global.frame - gLastTurnFrame >= TURN_DURATION\ and hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > minTimeToDribble: # If we are in defensive area, we are lined up, # then force time elapsed with defensive selection. selfY = Global.selfLoc.getY() if selfY < Constant.FIELD_LENGTH * 0.4: return performTimeElapsedAction(SEL_DEFENSIVE) # Otherwise use midfield selection. else: return performTimeElapsedAction(SEL_MIDFIELD) # If we are in outside third, use diagonal grab dribbling. selfX = Global.selfLoc.getX() if selfX < Constant.LEFT_GOAL_POST or selfX > Constant.RIGHT_GOAL_POST: left = hMath.CLIP(selfX - Constant.FIELD_WIDTH * 0.5, Action.MAX_SKE_LEFT_STP * 0.5) else: left = 0 return performDribble(Action.MAX_FORWARD, left, turnCCW)
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 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 slotStepPlayer(self, player=None): if player == None: player = self.player # Set the robot and ball positions VisionLink.me = Global.WMObj(self.robot.x, self.robot.y,\ 0, self.robot.h, 0) VisionLink.ball = Global.WMObj(self.ballView.x,\ self.ballView.y, 0, 0, 0) # FIXME: catch exceptions to reset killThread # Call the offline process frame function to set up frame info obsTuples = map((lambda o: (o.x, o.y)), self.obstacles) VisionLink.clearObstacles() for obs in obsTuples: VisionLink.setObstacle(*obs) VisionLink.processFrame(self.robot.x, self.robot.y, self.robot.h, self.ballView.x, self.ballView.y, 0, 0) # Run the behaviour for one frame We skip past Behaviou and Player # modules to allow us to switch players easily so here we mimic # Behaviou.processFrame hFrameReset.framePreset() hFrameReset.frameReset() hFrameReset.offlineReset() player.DecideNextAction() hFrameReset.framePostset() # Read the last action and update state forward = Action.finalValues[Action.Forward] left = Action.finalValues[Action.Left] turn = Action.finalValues[Action.TurnCCW] forward *= abs(float(Action.finalValues[Action.ForwardStep]) \ / Action.MAX_SKE_FWD_STP) left *= abs(float(Action.finalValues[Action.LeftStep]) \ / Action.MAX_SKE_LEFT_STP) turn *= abs(float(Action.finalValues[Action.TurnCCWStep]) \ / Action.MAX_SKE_TURNCCW_STP) sum = abs(forward) + abs(left) if (sum > 35): # clip total to estimate real life forward = float(forward) / (sum) * 35 left = float(left) / (sum) * 35 print "mvmt:", int(forward), int(left), int(turn) self.txtMvmtSpeed.setText("%d, %d, %d" % \ (Action.finalValues[Action.Forward], \ Action.finalValues[Action.Left], \ Action.finalValues[Action.TurnCCW])) self.txtMvmtStep.setText("%d, %d, %d" % \ (Action.finalValues[Action.ForwardStep], \ Action.finalValues[Action.LeftStep], \ Action.finalValues[Action.TurnCCWStep])) self.txtWalkType.setText(str(Action.finalValues[Action.WalkType])) self.txtKick.setText(str(sSelKick.perform()[0])) forward /= 30.0 left /= 30.0 turn /= 30.0 #print "clipped to:", "%.2f"%forward, "%.2f"%left, "%.2f"%turn heading = self.robot.h * math.pi / 180.0 newy = self.robot.y + forward * math.sin(heading) \ + left * math.cos(heading) newx = self.robot.x + forward * math.cos(heading) \ - left * math.sin(heading) self.robot.move(newx, newy) #self.robot.y += forward * math.cos(heading - math.pi/2) #self.robot.y += left * math.sin(heading - math.pi/2) #self.robot.x += forward * math.sin(heading - math.pi/2) #self.robot.x += left * math.cos(heading - math.pi/2) self.robot.setHeading(hMath.normalizeAngle_180(self.robot.h + turn)) self.app.lock() self.fieldCanvas.update() self.app.unlock()
def SmartSetBeacon(panLimit=90, distLimit=550): global localiseBeacon, lastBeacon # Get shared variables from elsewhere. selfLoc = Global.selfLoc cov = VisionLink.getGPSSelfCovariance() # Calculate the direction of the larger axis of the self covariance b = -(cov[0][0] + cov[1][1]) c = cov[0][0] * cov[1][1] - cov[1][0] * cov[1][0] det = b * b - 4 * c if (det < 0.00001): sol = -b / 2 else: sol = (-b + math.sqrt(det)) / 2 # chooseCov = False # Are we choosing a beacon to reduce covariance? if abs(cov[0][0] - sol) < 0.01 and cov[0][1] < 0.01: # If self position variance is circular, all beacons are equally good # to localise off, and so choose the one closest to current heading. # dHead is global heading we desire to stay close to if abs(cov[1][1] - sol) < 0.01: dHead = selfLoc.getHeading() else: dHead = hMath.RAD2DEG(math.atan2(-cov[1][0], cov[1][1] - sol)) else: #chooseCov = True dHead = hMath.RAD2DEG(math.atan2(sol - cov[0][0], cov[0][1])) # Find the beacon that is closest to the variance direction. beaconPos = getBeaconCoords() bestHead = nextBestHead = 360 localiseBeacon = None # a tuple (x, y) nextBestBeacon = None #print beaconPos for beacon in beaconPos: relPos = map(lambda b, s: b - s, beacon, selfLoc.getPos()) beaconHead = hMath.RAD2DEG(math.atan2(relPos[1], relPos[0])) beaconDist = hMath.getLength(relPos) localHead = hMath.normalizeAngle_180(beaconHead - selfLoc.getHeading()) if abs(localHead) < panLimit and beaconDist < distLimit: #print beacon, localHead, beaconHead, dHead compHead = abs(hMath.normalizeAngle_180(dHead - beaconHead)) if compHead > 90: compHead = 180 - compHead if compHead < bestHead: nextBestBeacon = localiseBeacon nextBestHead = bestHead localiseBeacon = beacon bestHead = compHead elif compHead < nextBestHead: nextBestBeacon = beacon nextBestHead = compHead #print "sActiveLocalise best =", localiseBeacon, "next =", nextBestBeacon # Don't choose the same beacon twice to reduce covariance. If we chose it # last time based on covariance and the covariance is still large that way # we likely didn't see it, so choose the next best if localiseBeacon == lastBeacon and nextBestBeacon != None: # print "Choosing a different beacon to last time" localiseBeacon = nextBestBeacon nextBestBeacon = None # Prefer not to look at a really close beacon - we don't get good # information if nextBestBeacon != None and \ hMath.getDistanceBetween(selfLoc.getX(), selfLoc.getY(),\ localiseBeacon[0], localiseBeacon[1]) < MIN_BEACON_DIST: # print "Choosing not to use close beacon" localiseBeacon = nextBestBeacon #print "Choosing beacon at", localiseBeacon, "frame", Global.frame lastBeacon = localiseBeacon if localiseBeacon == None: return False else: return True