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.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 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 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 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 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 amIInTheCircle(targetX, targetY, radius): selfX, selfY = Global.selfLoc.getPos() distSquared = hMath.getDistSquaredBetween(targetX, targetY, selfX, selfY) return distSquared < hMath.SQUARE(radius)