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