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)