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)
"""
