def perform(dist=0, head=0, distSquared=False): if dist != 0: if not (distSquared): ballDSquared = hMath.SQUARE(dist) else: ballDSquared = dist ballH = head else: ballDSquared = hMath.SQUARE(Global.ballD) ballH = Global.ballH if abs(ballH) > 40: Action.walk(0, 0, ballH, minorWalkType=Action.SkeFastForwardMWT) return Action.stopLegs() slowDownDistance = 30 # If (global) heading is forward then slowdown distance is smaller if hWhere.isInRange(5, 175): slowDownDistance = 20 slowDown = ballDSquared < hMath.SQUARE(slowDownDistance) headingLarge = abs(ballH) > 15 # Unless the robot want to grab the ball or control it directly. # sHoverToBall shouldn't slow down the robot at all. if slowDown: if headingLarge: Action.walk(Action.MAX_FORWARD, 0, hMath.CLIP(ballH / 1.6, 30.0), minorWalkType=Action.SkeFastForwardMWT) else: Action.walk(Action.MAX_FORWARD, 0, hMath.CLIP(ballH / 2.0, 30.0), minorWalkType=Action.SkeFastForwardMWT) else: if headingLarge: Action.walk(Action.MAX_FORWARD, 0, hMath.CLIP(ballH / 1.6, 30.0), minorWalkType=Action.SkeFastForwardMWT) else: Action.walk(Action.MAX_FORWARD, 0, hMath.CLIP(ballH / 2.0, 30.0), minorWalkType=Action.SkeFastForwardMWT)
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 perform(paw=AUTO_PAW_KICK): global pawKickType if Global.lostBall > Constant.LOST_BALL_GPS: return Constant.STATE_FAILED # Head ball tracking movement sFindBall.perform(True) PAW_KICK_Y_OFFSET = Constant.BallRadius PAW_KICK_X_OFFSET = 7.0 ballX = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD ballY = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD # The 2003 offset is 6 and it is not too bad. if paw == AUTO_PAW_KICK: if pawKickType == LEFT_PAW_KICK and ballX < -PAW_KICK_X_OFFSET / 2: pawKickType = RIGHT_PAW_KICK elif pawKickType == RIGHT_PAW_KICK and ballX > PAW_KICK_X_OFFSET / 2: pawKickType = LEFT_PAW_KICK else: pawKickType = paw if pawKickType == RIGHT_PAW_KICK: targetX = ballX + PAW_KICK_X_OFFSET else: targetX = ballX - PAW_KICK_X_OFFSET targetY = ballY - PAW_KICK_Y_OFFSET targetD = math.sqrt(hMath.SQUARE(targetX) + hMath.SQUARE(targetY)) targetH = hMath.RAD2DEG(math.asin(targetX / targetD)) # Once the ball is lined up and well within range for the forward, the # robot should commit to charge at the ball at full speed. if abs(targetX) <= 1.0 and targetD < Constant.BallRadius * 1.5: turnccw = hMath.CLIP(targetH, 10) Action.walk(Action.MAX_FORWARD, 0, turnccw, minorWalkType=Action.SkeFastForwardMWT) else: sFindBall.walkToBall(targetD, targetH, getBehind=sFindBall.GET_BEHIND_LOTS) return Constant.STATE_EXECUTING
def DecideNextAction(): global STRIKER_ACC_SMALL_CIRCLE, STRIKER_ACC_LARGE_CIRCLE, strikerPointReqAccuracy global strikerPtX, strikerPtY Global.myRole = Constant.WINGER selfX, selfY = Global.selfLoc.getPos() if (strikerPtX is None or strikerPtY is None): strikerPtX, strikerPtY = getStrikerPointXPositioning() ##~ print "ball position : myx: %.2f myy: %.2f" % (Global.ballX,Global.ballY) ##~ print "striker point : strikeX: %.2f strikeY: %.2f" % (strikerPtX,strikerPtY) ##~ print "" h = HelpMath.normalizeAngle_0_360(HelpMath.RAD2DEG \ (math.atan2(Global.ballY - selfY, Global.ballX - selfX))) ##~ print "===========" ##~ print "heading is %.2f" % (h) ##~ print "myloc %.2f, %.2f" % Global.selfLoc.getPos() ##~ print "ballLoc %.2f, %.2f" % (Global.ballX,Global.ballY) # If facing away the ball, then dont. Use the slower saGoToTargetFacingHeading angleBetweenStrikerPtBall = HelpMath.absAngleBetweenTwoPointsFromPivotPoint \ (Global.ballX,Global.ballY,strikerPtX,strikerPtY,selfX,selfY) if (angleBetweenStrikerPtBall > 90): HelpTrack.saGoToTargetFacingHeading(strikerPtX, strikerPtY, h) # Otherwsie if you can see the ball constantly, use the fast way else: HoverToBall.saGoToTargetFacingHeading(strikerPtX, strikerPtY, h) # Hysterisis for whether or not your at the striker point. dist = math.sqrt(HelpMath.SQUARE(strikerPtX - selfX) \ + HelpMath.SQUARE(strikerPtY - selfY)) ##~ print "StrikeX: %f" % strikerPtX ##~ print "StrikeY: %f" % strikerPtY ##~ print "Distance: %f" % dist ##~ print "StrikerPointReqAccuracy: %f" % strikerPointReqAccuracy if dist <= strikerPointReqAccuracy: strikerPointReqAccuracy = STRIKER_ACC_LARGE_CIRCLE Action.walk(0, 0, None, None) else: strikerPointReqAccuracy = STRIKER_ACC_SMALL_CIRCLE
def perform(bShowLED=True): global localiseBeacon, isFocusing # print "Active-localising!!" # Set debugging information. if bShowLED: pass #Indicator.showRedEye(True) if localiseBeacon == None: print "sActiveLocalise.perform: localiseBeacon None" Action.setHeadToLastPoint() return Constant.STATE_FAILED #print "sActiveLocalise.DecideNextAction() : Actively Localising at beacon: ", localiseBeacon relPos = map(lambda b, s: b - s, localiseBeacon, Global.selfLoc.getPos()) relPos = hMath.rotate(relPos, 90 - Global.selfLoc.getHeading()) targetX = -relPos[0] targetY = relPos[1] # We don't need to put the centre of the camera on the beacon since that # wastes at least half the image. Instead we look at the base of the beacon # if it is far enough away for the whole thing still to be in frame when we # do this. (The distance was calibrated from measurement) if hMath.getDistSquaredBetween( 0, 0, targetX, targetY) <= hMath.SQUARE(BEACON_LOOK_UP_DIST): Action.setHeadParams(targetX, Constant.BEACON_HEIGHT, targetY,\ Action.HTAbs_xyz) else: Action.setHeadParams(targetX, 0, targetY, Action.HTAbs_xyz) return Constant.STATE_EXECUTING
def supportBall(): global gTargetPointReqAccuracy targetX, targetY, rangeX = hWhere.getStrikerPos() targetX, targetY = getAdjustedTarget(targetX, targetY, rangeX) selfX, selfY = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() ballH = Global.ballH h = hMath.normalizeAngle_0_360(selfH + ballH) distSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) # From outside a metre walk fast, else turn to face ball #if dist > 100: # hTrack.saGoToTarget(targetX, targetY) #else: # hTrack.saGoToTargetFacingHeading(targetX, targetY, h) hTrack.saGoToTargetFacingHeading(targetX, targetY, h) # Hysterisis for whether or not you are at the striker point. if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy): gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE if abs(ballH) < 5: Action.stopLegs() else: gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE return targetX, targetY
def supportBall(): global gTargetPointReqAccuracy targetX, targetY, rangeX = hWhere.getSupporterPos(False) targetX, targetY = getAdjustedTarget(targetX, targetY, rangeX) selfX, selfY = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() ballH = Global.ballH h = hMath.normalizeAngle_0_360(selfH + ballH) distSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) # From outside a metre walk fast, else turn to face ball #if dist > 100: # hTrack.saGoToTarget(targetX, targetY) #else: # hTrack.saGoToTargetFacingHeading(targetX, targetY, h) hTrack.saGoToTargetFacingHeading(targetX, targetY, h) # Hysterisis for whether or not you are at the supporter point. if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy): gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE if abs(ballH) < 5: Action.stopLegs() if 0 <= Global.selfLoc.getHeading( ) <= 180 and not Global.vOGoal.isVisible(): # only block if my heading is right and I cannot see my own goal sBlock.checkThenBlock(minBallSpeed=2, bothSides=True) else: gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE return targetX, targetY
def doGetToTargetPoint(targetX, targetY): global gTargetPointReqAccuracy selfX, selfY = Global.selfLoc.getX(), Global.selfLoc.getY() ballX, ballY, ballH = Global.ballX, Global.ballY, Global.ballH h = hMath.normalizeAngle_0_360(hMath.RAD2DEG(math.atan2(\ ballY - selfY, ballX - selfX))) #angle = hMath.absAngleBetweenTwoPointsFromPivotPoint(ballX, ballY, \ # targetX, targetY, \ # selfX, selfY) distSquared = hMath.getDistSquaredBetween(targetX,targetY,selfX,selfY) ## if dist > 100 and angle < 80: ## hTrack.saGoToTarget(targetX,targetY) ## else: ## hTrack.saGoToTargetFacingHeading(targetX,targetY,h) hTrack.saGoToTargetFacingHeading(targetX,targetY,h) # Hysterisis for whether or not your at the defender point. if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy): gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE if abs(ballH) < 15: Action.stopLegs() else: gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE if abs(targetX - selfX) < 30 and abs(targetY - selfY) < 100: checkThenBlock()
def findByWireless(): hTrack.scan(highSpeed=10, minPan=-60, maxPan=60) if not gHeadOnly: selfX, selfY = Global.selfLoc.getPos() # Make sure we get a ball distance from wireless info. ballDSquared = hMath.getDistSquaredBetween(selfX, selfY, Global.sharedBallX, Global.sharedBallY) if ballDSquared < hMath.SQUARE(WIRELESS_BALL_CLOSE_DIST): findBySpin() else: Action.walk(0, 0, Global.ballH, minorWalkType=Action.SkeFastForwardMWT)
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 findBall(tx, ty): global gSpinCounter global gTargetPointReqAccuracy selfX, selfY = Global.selfLoc.getPos() distSquared = hMath.getDistSquaredBetween(selfX, selfY, tx, ty) if gSpinCounter > 0: sFindBall.findBySpin() gSpinCounter -= 1 elif distSquared <= hMath.SQUARE(gTargetPointReqAccuracy): gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE sFindBall.findBySpin() gSpinCounter = 100 else: gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE hTrack.scan() hTrack.saGoToTarget(tx, ty)
def timeToReachBall(): global gTimeToReachBall, gTimeToReachBallFrame if gTimeToReachBallFrame == Global.frame: return gTimeToReachBall # 1. Calculate time to reach ball # 2. Add time if the ball is unsure # 3. Reduce time for current attacker for hysterisis # 4. Reduce time a lot if we are grabbing # 5. timeToReachPoint adds time for turning to face and turning to dkd bonus = 0 # larger numbers are better # Penalty for lost ball - quadratic if Global.lostBall > LOST_BALL_FRAME: bonus -= int( hMath.SQUARE(Global.lostBall - LOST_BALL_FRAME) * LOST_BALL_TIME_PENALTY) # Bonus for role if Global.myLastRole == Constant.ATTACKER: bonus += ATTACKER_TIME_BONUS # attacker bonus if sGrab.isGrabbed: bonus += GRABBER_TIME_BONUS # big bonus for grabbing #dkd = 90 # DKD is initialised in Forward so for ready/set use upfield #if Global.DKD != None: # dkd = Global.DKD[0] dkd = sSelKick.perform()[1] t = timeToReachPoint(Global.ballX, Global.ballY, dkd) - bonus if gCapTime: rtn = hMath.CLIP(t, Constant.MAX_TIME_TO_REACH) else: rtn = t gTimeToReachBall = rtn gTimeToReachBallFrame = Global.frame return rtn
def doGetToTargetPoint(): global gTargetPointReqAccuracy targetX, targetY = hWhere.getWingerPos() selfX, selfY = Global.selfLoc.getPos() selfH = Global.selfLoc.getHeading() ballH = Global.ballH h = hMath.normalizeAngle_0_360(selfH + ballH) hTrack.saGoToTargetFacingHeading(targetX, targetY, h) # Hysterisis for whether or not your at the striker point. distSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy): gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE if abs(ballH) < 5: Action.stopLegs() else: gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE
def 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 shouldIEnd(): global targetX, targetY, targetDSquared, targetH global boxPointOffset,\ cornerCut,\ maxTurn,\ insideBound,\ outsideBound,\ lastDBH,\ currentBopMode,\ targetDesiredAngle,\ lockDefender,\ minAngle,\ breakOutMinAngle,\ breakOutIncAngle DEFENCE_OFFSET = 0 DEFENCE_ANGLE = 120.0 if not areWeAboveTheLine(DEFENCE_OFFSET, DEFENCE_ANGLE, False, \ targetX, targetY): return True # If in place, don't become the bird anymore. if currentBopMode == Constant.CIRCLE_BOP and abs(targetH) < 20: currentBopMode = Constant.NORMAL_BOP return True # If we've reached the goal box don't bird anymore if Global.selfLoc.getY() < Constant.GOALBOX_DEPTH + 25: return True tx, ty = getWalkToPosition(targetX, targetY) selfD2TargetSquared = hMath.getDistSquaredBetween(Global.selfLoc.getX(),\ Global.selfLoc.getY(),\ tx,\ ty) if selfD2TargetSquared < hMath.SQUARE(TARGET_PT_ACC_SMALL_CIRCLE): currentBopMode = Constant.NORMAL_BOP return True goalX = (Constant.FIELD_WIDTH / 2 + targetX) / 2 selfX, selfY = Global.selfLoc.getX(), Global.selfLoc.getY() ## Determine if we are around the right side of the ball. selfD2Goal = hMath.getDistanceBetween(selfX, selfY, goalX, 0) targetD2Goal = hMath.getDistanceBetween(targetX, targetY, goalX, 0) if selfD2Goal < targetD2Goal: dX = targetX - goalX dY = targetY dp = (Global.selfLoc.getPos()[0] - goalX) * dX + Global.selfLoc.getPos()[1] * dY ## Quit due to something being to close to goal (should never happen) if abs(selfD2Goal) < 0.1 or abs(targetD2Goal) < 0.1: return True ## use dot product to calculate angle between ## robot vector and target vector (both from defend pt) cosAng = dp / (selfD2Goal * targetD2Goal) ang = math.acos(cosAng) if math.degrees(ang) < minAngle: minAngle = math.degrees(ang) ## Quit due to crossing the line from target to goal if minAngle < breakOutMinAngle: return True ## Quit due to angle of line from target to goal increasing if ang - minAngle > breakOutIncAngle: return True else: if currentBopMode == Constant.CIRCLE_BOP: currentBopMode = Constant.NORMAL_BOP return True else: if currentBopMode == Constant.CIRCLE_BOP: currentBopMode = Constant.NORMAL_BOP return True return False
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 getOutOfEveryonesWay(): global gGetOutOfTheShotCounter global gGetOutOfTheAttackerCounter # global gGetAwayFromTheBallCounter global gGetAwayFromTheAttackerCounter # --------------------------------------------------------------- # Work out who is in what role. We cater for at most one attacker attacker = None for i in Global.otherValidForwards: mate = Global.teamPlayers[i] mateLoc = Global.teammatesLoc[i] if mate.isAttacker(): attacker = (mate, mateLoc) # --------------------------------------------------------------- # Continue any already-running get-out-of-the-ways targetH = hMath.normalizeAngle_0_360(Global.selfLoc.getHeading()\ + Global.ballH) # Away from the goal-shoot line if gGetOutOfTheShotCounter > 0: r = sGetOutOfTheWay.perform(Global.ballX,Global.ballY,\ Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y,\ targetH,True) if r == Constant.STATE_EXECUTING: gGetOutOfTheShotCounter -= 1 return True gGetOutOfTheShotCounter = 0 # Away from the attacker->ball line if attacker != None and gGetOutOfTheAttackerCounter > 0: r = sGetOutOfTheWay.perform(Global.ballX, Global.ballY,\ attacker[1].getX(), attacker[1].getY(),\ targetH,True) if r == Constant.STATE_EXECUTING: gGetOutOfTheAttackerCounter -= 1 return True gGetOutOfTheAttackerCounter = 0 else: gGetOutOfTheAttackerCounter = 0 # if gGetAwayFromTheBallCounter > 0: # Away from the ball # r = sGetOutOfTheWay.getOutOfTheCircle(Global.ballX, Global.ballY, \ # targetH, True) # if r == Constant.STATE_EXECUTING: # gGetAwayFromTheBallCounter -= 1 # return True # gGetAwayFromTheBallCounter = 0 # Away from the attacker robot if attacker != None and gGetAwayFromTheAttackerCounter > 0: r = sGetOutOfTheWay.getOutOfTheCircle(attacker[1].getX(),\ attacker[1].getY(), targetH, True) if r == Constant.STATE_EXECUTING: gGetAwayFromTheAttackerCounter -= 1 return True gGetAwayFromTheAttackerCounter = 0 else: gGetAwayFromTheAttackerCounter = 0 #------- Check for any get-out-of-the-way I should start ---------------- # Get away from between ball and goal if attacker close to ball or the line # is short if attacker != None\ and (attacker[0].getTimeToReachBall() < 1500\ or hMath.getDistSquaredBetween(Global.ballX, Global.ballY,\ Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y) < hMath.SQUARE(150))\ and not (Global.ballSource == Constant.GPS_BALL and Global.lostBall >= Constant.LOST_BALL_GPS): r = sGetOutOfTheWay.perform(Global.ballX,Global.ballY,\ Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y,\ targetH) if r == Constant.STATE_EXECUTING: gGetOutOfTheShotCounter = GET_OUT_DURATION return True # Get out from between the attacker and the ball if attacker != None and attacker[0].getTimeToReachBall() < 3000\ and not (Global.ballSource == Constant.GPS_BALL and Global.lostBall >= Constant.LOST_BALL_GPS): r = sGetOutOfTheWay.perform(Global.ballX, Global.ballY,\ attacker[1].getX(), attacker[1].getY(),\ targetH) if r == Constant.STATE_EXECUTING: gGetOutOfTheAttackerCounter = GET_OUT_DURATION return True # Stay clear of the attacker robot if attacker != None: r = sGetOutOfTheWay.getOutOfTheCircle(attacker[1].getX(), \ attacker[1].getY(),\ targetH, radius = 30) if r == Constant.STATE_EXECUTING: gGetAwayFromTheAttackerCounter = GET_OUT_DURATION return True # Get away from the ball # if not (Global.ballSource == Constant.GPS_BALL and Global.lostBall >= Constant.LOST_BALL_GPS): # r = sGetOutOfTheWay.getOutOfTheCircle(Global.ballX, Global.ballY, targetH) # if r == Constant.STATE_EXECUTING: # gGetAwayFromTheBallCounter = GET_OUT_DURATION # return True # All clear return False
def walkToStillBall(ballD, ballH, getBehind = GET_BEHIND_NONE): global gLastTurnFrame global gGoalie if getBehind == GET_BEHIND_GOALIE: getBehind = GET_BEHIND_PRIORITY gGoalie = True fwd = Action.MAX_FORWARD # Lighting challenge and penalty shooter don't get behind. It makes the # grab a bit harder and takes time if Global.lightingChallenge or Global.penaltyShot: getBehind = GET_BEHIND_NONE # Don't get behind when close to our goal box since # this moves us towards it (unless we are the goalie) if not gGoalie and hWhere.selfInOwnGoalBox(20, sideoffset = 10): getBehind = GET_BEHIND_NONE # Calculate getBehind adjustment leftAdj = 0 if getBehind > GET_BEHIND_NONE: myx, myy = Global.selfLoc.getPos() myh = Global.selfLoc.getHeading() ballx, bally = hMath.getGlobalCoordinate(myx, myy, myh, *hMath.polarToCart(ballD, ballH+90)) # In defense we get between our goal and the ball. In attack we line # up the ball and target goal. In the (small) midfield we line up with # upfield reversePoints = False if bally < Constant.FIELD_LENGTH/2 - 20 \ or hTeam.amIFurthestBack(ignoreRoles = []): lineX, lineY = Constant.OWN_GOAL_X, Constant.OWN_GOAL_Y elif bally > Constant.FIELD_LENGTH/2 + 20: lineX, lineY = Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y reversePoints = True else: lineX, lineY = ballx, 0 # Draw a line between ball and goal. Find self offset from the line dist = ((ballx - lineX)*(lineY - myy)-(lineX - myx)*(bally - lineY)) /\ math.sqrt(hMath.SQUARE(ballx - lineX) \ + hMath.SQUARE(bally - lineY)) # If dist > 0 we are to the right of the line, so adjustment left # These adjustments will only be applied assuming we are facing # the ball if dist > 15: leftAdj = 14 elif dist < -15: leftAdj = -12 elif myy > bally: # Always sidestep from in front if dist >= 0: leftAdj = 14 elif dist < 0: leftAdj = -12 else: leftAdj = 0 if leftAdj != 0: # At this point check that sidestepping in this direction is not # going to put obstacles between us and the ball. If so it would be # better to go direct (and let dodgy dog do its thing) (lBallX, lBallY) = Global.gpsLocalBall.getPos() nobs = VisionLink.getNoObstacleBetween(leftAdj, 0, int(lBallX) + leftAdj, int(lBallY) - 30, abs(leftAdj), 0, Constant.MIN_GPS_OBSTACLE_BOX, Constant.OBS_USE_GPS) if nobs > 2 * Constant.MIN_GPS_OBSTACLE_BOX: #Indicator.showFacePattern([3, 2, 0, 2, 3]) leftAdj = 0 # Multiply getBehind in some cases. Only one of these can apply at # at time so put them in order of importance. leftAdjMult = 1 if getBehind == GET_BEHIND_DEFAULT: # Last man attacker must be behind if hTeam.amIFurthestBack(ignoreRoles = []): if myy < bally: getBehind = GET_BEHIND_PRIORITY else: getBehind = GET_BEHIND_LOTS # In defense do lots of getBehind elif bally < Constant.FIELD_LENGTH * 0.25: getBehind = GET_BEHIND_LOTS # From in front of the ball do a little more to get behind the # ball elif myy > bally: getBehind = GET_BEHIND_MORE # Lots of getBehind if requested if getBehind >= GET_BEHIND_ONLY: fwd = 10 # Walk in a circular arc leftAdjMult = 2 elif getBehind >= GET_BEHIND_PRIORITY: fwd = 30 # Slow down so getBehind has more effect leftAdjMult = 2 elif getBehind >= GET_BEHIND_LOTS: leftAdjMult = 2 elif getBehind >= GET_BEHIND_MORE: leftAdjMult = 1.5 leftAdj *= leftAdjMult # If the line was to a target to line up, reverse direction if reversePoints: leftAdj = -leftAdj #Indicator.showFacePattern([0]*5) # if leftAdj > 0: # Indicator.showFacePattern([0, 0, 0, 2, 3]) # elif leftAdj < 0: # Indicator.showFacePattern([3, 2, 0, 0, 0]) # if gGoalie: # Indicator.setDefault() # # DANGER CASE: Goalie Facing Backwards # if gGoalie and (225 <= myh < 315): # Indicator.showHeadColor(Indicator.RGB_ORANGE) # #ball to goalie's right # if ballx <= myx: # print "Ball to my RIGHT" # Action.walk(-5,Action.MAX_LEFT,-Action.MAX_TURN, # "ddd",minorWalkType=Action.SkeFastForwardMWT) # #ball to goalie's left # else: # print "Ball to my LEFT" # Action.walk(-5,-Action.MAX_LEFT,Action.MAX_TURN, # "ddd",minorWalkType=Action.SkeFastForwardMWT) # Walk direct - with getBehind adjustment if abs(ballH) < 30: # if gGoalie: # Indicator.showHeadColor(Indicator.RGB_GREEN) Action.walk(fwd, leftAdj, ballH, minorWalkType=Action.SkeFastForwardMWT) # Walk and turn to face elif abs(ballH) < 80: gLastTurnFrame = Global.frame fwd = max(ballD * math.cos(ballH),0) left = ballD * math.sin(ballH) # if gGoalie: # left = leftAdj # Indicator.showHeadColor(Indicator.RGB_BLUE) Action.walk(fwd,left,ballH,"ddd",minorWalkType=Action.SkeFastForwardMWT) # Turn on the spot else: gLastTurnFrame = Global.frame fwd = 0 left = 0 turnccw = hMath.CLIP(ballH,60) # if gGoalie: # fwd = -Action.MAX_FORWARD # # if abs(ballx - myx) > Constant.DOG_WIDTH * 2: # left = (myx - ballx) * 0.7 # turnccw = 0 # Indicator.showHeadColor(Indicator.RGB_PURPLE) Action.walk(fwd,left,turnccw,"ddd",minorWalkType=Action.SkeFastForwardMWT)
def amIInTheCircle(targetX, targetY, radius): selfX, selfY = Global.selfLoc.getPos() distSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) return distSquared < hMath.SQUARE(radius)
def perform(fromX, fromY, toX, toY, targetH, doForce=False, getOutOfTheWayDist=60): selfX, selfY = Global.selfLoc.getPos() # If you are really further away from the "from" cords, then don't do anything. if hMath.getDistSquaredBetween(selfX, selfY, fromX, fromY) > hMath.SQUARE(250): return Constant.STATE_SUCCESS dist, head = amIInTheWay(fromX, fromY, toX, toY) if dist != None\ and (doForce or abs(dist) < getOutOfTheWayDist): rise = fromY - toY run = fromX - toX if rise == 0: rise = 0.01 if run == 0: run = 0.01 # gradient of the line between from and to m1 = rise / (run + 0.0) b1 = fromY - m1 * fromX # perpendicular gradient to the line between from and to m2 = -run / (rise + 0.0) b2 = selfY - m2 * selfX # on the right side if dist > 0: newX = selfX + 50 newY = m2 * newX + b2 # on the left side else: newX = selfX - 50 newY = m2 * newX + b2 # Adjust newY, so that the robot would move downwards. temp = m1 * newX + b1 if newY > temp: newY = selfY else: newY -= (50.0 / abs(getOutOfTheWayDist)) * abs(dist) newX = max(newX, 0) newX = min(newX, Constant.FIELD_WIDTH) newY = max(newY, 0) newY = min(newY, Constant.FIELD_LENGTH) hTrack.saGoToTargetFacingHeading(newX, newY, targetH) Indicator.showFacePattern([0, 0, 2, 0, 0]) return Constant.STATE_EXECUTING else: return Constant.STATE_SUCCESS
def walkToStillBall(ballD, ballH, getBehind=False): # Calculate getBehind adjustment leftAdj = 0 if getBehind: myx, myy = Global.selfLoc.getPos() myh = Global.selfLoc.getHeading() ballx, bally = hMath.getGlobalCoordinate( myx, myy, myh, *hMath.polarToCart(ballD, ballH + 90)) # In defense we get between our goal and the ball. In attack we line # up the ball and target goal. In the (small) midfield we line up with # upfield reversePoints = False if bally < Constant.FIELD_LENGTH / 2 - 20: lineX, lineY = Constant.OWN_GOAL_X, Constant.OWN_GOAL_Y elif bally > Constant.FIELD_LENGTH / 2 + 20: lineX, lineY = Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y reversePoints = True else: lineX, lineY = ballx, 0 # Draw a line between ball and own goal. Find self offset from the line dist = ((ballx - lineX)*(lineY - myy)-(lineX - myx)*(bally - lineY)) /\ math.sqrt(hMath.SQUARE(ballx - lineX) \ + hMath.SQUARE(bally - lineY)) # If dist > 0 we are to the right of the line, so adjustment left # These adjustments will only be applied assuming we are facing # the ball if dist > 15: leftAdj = 14 elif dist < -15: leftAdj = -12 else: leftAdj = 0 # From in front of the ball adjust even more to get more behind if myy > bally: leftAdj *= 1.5 # If the line was to a target to line up, reverse direction if reversePoints: leftAdj = -leftAdj Indicator.showFacePattern([0] * 5) if ballD < MID_BALL_DIST and abs(ballH) > 30: # Turn on the spot ballH = hMath.CLIP(ballH, 70) Action.walk(0, 0, ballH, minorWalkType=Action.SkeFastForwardMWT) else: # Walk direct - with getBehind adjustment if leftAdj > 0: Indicator.showFacePattern([0, 0, 0, 2, 3]) elif leftAdj < 0: Indicator.showFacePattern([3, 2, 0, 0, 0]) if abs(ballH) < 30: Action.walk(Action.MAX_FORWARD, leftAdj, ballH, minorWalkType=Action.SkeFastForwardMWT) else: ballH = hMath.CLIP(ballH, 70) Action.walk(0, 0, ballH, minorWalkType=Action.SkeFastForwardMWT) """