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 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 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 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 walk(fwd=0,
left=0,
turnCCW=0,
cmdType="ssd",
walkType=SkellipticalWalkWT,
minorWalkType=DefaultMWT):
global finalValues
# Use learning parameters if it is set.
#if minorWalkType == LearningMWT:
# walkLearning(walkType)
if walkType == None:
walkType = SkellipticalWalkWT
finalValues[WalkType] = walkType
finalValues[MinorWalkType] = minorWalkType
# Clip requested speeds to maximum
if walkType == NormalWalkWT:
finalValues[ForwardStep] = hMath.CLIP(fwd, MAX_FORWARD_NORMAL)
finalValues[LeftStep] = hMath.CLIP(left, MAX_LEFT_NORMAL)
finalValues[TurnCCWStep] = hMath.CLIP(turnCCW, MAX_TURN_NORMAL)
elif walkType == SkellipticalWalkWT:
if minorWalkType == SkePG22MWT:
clipWalk(fwd,left,turnCCW,cmdType,SKE_PG22_PG,\
MAX_SKE_PG22_FWD_STP,MAX_SKE_PG22_BACK_STP,\
MAX_SKE_PG22_LEFT_STP,MAX_SKE_PG22_RIGHT_STP,\
MAX_SKE_PG22_TURNCCW_STP,MAX_SKE_PG22_TURNCW_STP)
elif minorWalkType == SkeGrabDribbleMWT:
clipWalk(fwd, left, turnCCW, cmdType, SKE_GD_PG,
MAX_SKE_GD_FWD_STP, MAX_SKE_GD_BACK_STP,
MAX_SKE_GD_LEFT_STP, MAX_SKE_GD_RIGHT_STP,
MAX_SKE_GD_TURNCCW_STP, MAX_SKE_GD_TURNCW_STP)
elif minorWalkType == SkePG31MWT:
clipWalk(fwd,left,turnCCW,cmdType,SKE_PG31_PG,\
MAX_SKE_PG31_FWD_STP,MAX_SKE_PG31_BACK_STP,\
MAX_SKE_PG31_LEFT_STP,MAX_SKE_PG31_RIGHT_STP,\
MAX_SKE_PG31_TURNCCW_STP,MAX_SKE_PG31_TURNCW_STP)
else: #minorWalkType == (SkeFastForwardMWT, DefaultMWT, SkeTurnWalkMWT, SkeNeckTurnWalkMWT)
if abs(fwd) > MAX_SKE_FWD_SPD / 2 or abs(
left) > MAX_SKE_LEFT_SPD / 2:
clipWalk(fwd, left, turnCCW, cmdType, SKE_FF_PG,
MAX_SKE_FF_FWD_STP, MAX_SKE_FF_BACK_STP,
MAX_SKE_FF_LEFT_STP, MAX_SKE_FF_RIGHT_STP,
MAX_SKE_FF_COMBINED_TURNCCW_STP,
MAX_SKE_FF_COMBINED_TURNCW_STP)
else:
# Use turn only max step size, if we are not moving forward and left much
clipWalk(fwd, left, turnCCW, cmdType, SKE_FF_PG,
MAX_SKE_FF_FWD_STP, MAX_SKE_FF_BACK_STP,
MAX_SKE_FF_LEFT_STP, MAX_SKE_FF_RIGHT_STP,
MAX_SKE_FF_TURNCCW_STP, MAX_SKE_FF_TURNCW_STP)
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 findBySpin(turnDir=None):
pan = Global.desiredPan
turnRate = Action.MAX_TURN
if Global.lightingChallenge:
turnRate = 60
# if turnDir is specified, then force to spin that direction.
if turnDir != None:
if turnDir == Constant.dANTICLOCKWISE:
turnCCW = turnRate
pan += 10
else:
pan -= 10
turnCCW = -turnRate
# Decide which direction to spin.
elif gIsClockwise:
turnCCW = -turnRate
pan -= 10
else:
turnCCW = turnRate
pan += 10
pan = hMath.CLIP(pan,90)
Action.setHeadParams(pan,-10,-8,Action.HTAbs_h)
Action.walk(0,0,turnCCW,minorWalkType=Action.SkeFastForwardMWT)
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 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 performDodge():
global gLastDiagFrame, gDodgeCounter, gLeft
obsFront = VisionLink.getNoObstacleInBox(-25, 100, 25, 20,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
if gDodgeCounter > 0:
gLastDiagFrame = Global.frame
gDodgeCounter -= 1
Indicator.showFacePattern([1, 1, 1, 1, 1])
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading()
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading()
turnCCW = hMath.CLIP(turnCCW, 30)
return perform(5, gLeft, turnCCW)
elif obsFront > MIN_OBS_TO_DODGE:
gLastDiagFrame = Global.frame
Indicator.showFacePattern([1, 1, 1, 1, 1])
gDodgeCounter = DODGE_DURATION
left = 0
# Decide which way to dodge.
# If you are in offensive half,
# then always dodge towards the center of the field.
selfX, selfY = Global.selfLoc.getPos()
if selfY > Constant.FIELD_LENGTH / 2.0:
if selfX > Constant.FIELD_WIDTH / 2.0:
left = Action.MAX_LEFT
else:
left = -Action.MAX_LEFT
# Otherwise, dodge towards outside?
else:
if selfX > Constant.FIELD_WIDTH / 2.0:
left = hMath.getSign(Action.MAX_LEFT,
selfX - Constant.FIELD_WIDTH / 3.0)
else:
left = hMath.getSign(Action.MAX_LEFT,
selfX - 2.0 * Constant.FIELD_WIDTH / 3.0)
gLeft = left
return perform(5, left, 0)
else:
return Constant.STATE_SUCCESS
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 walk(self):
heading = self.target().getRobustHeading() - self.panOffset()
# act according to heading
if abs(heading) < 30:
self.setWalkingToTarget(True)
if abs(heading) < 5:
walk(forward, left, 0, walkType)
else:
walk(forward, left,
hMath.CLIP(heading * turnAdjustment, walkToTargetMaxTurn),
walkType)
else:
self.searchOnSpot(heading)
def WalkToBall(x, y, maxSpeed=12):
global lastAction
angle = -hMath.RAD2DEG(hMath.getHeadingToRelative(x, y))
dist = math.sqrt(x * x + y * y)
if math.fabs(angle) > 30:
if dist < 30 or y < 0: # if close or ball is hehind the camera
Action.walk(0, 0, angle / 2) # rotate on spot
else:
forward = hMath.CLIP(y, maxSpeed)
Action.walk(forward, 0, angle / 2)
else:
Action.walk(FORWARD_SPEED, 0, angle)
lastAction = WALK_FORWARD
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 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 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 moveToMyPositionDirectly():
# Firstly get my kickoff position and return a fatal warning if there is none.
if Global.kickOffPos == None:
print "Warning: pReady.py - directPositionToMyposition() no kick off position?"
Debug.goAndMakeTraceBack()
return False
selfX = Global.selfLoc.getPos()[0]
selfY = Global.selfLoc.getPos()[1]
kickX = Global.kickOffPos[0]
kickY = Global.kickOffPos[1]
# Now I am sure I have the kickoff position, then I should run to it directly.
# Calculate the distance, heading to the kickoff position.
distance = hMath.getDistanceBetween(selfX, selfY, kickX, kickY)
heading = hMath.getHeadingToFaceAt(Global.selfLoc, kickX, kickY)
if Debug.directPosDebug:
print "Distance: %f" % distance
print "Heading: %f" % heading
# If the distance is small, do saGoToTargetFacingHeading(). With such small
# distance, better go and turn toward the target goal.
if distance <= 50:
if Debug.readyDebug:
print "Distance too small case!", Global.selfLoc.getPos(), \
Global.kickOffPos
hTrack.saGoToTargetFacingHeading(kickX, kickY, 90, 12, 50)
return False
# If the heading is too large, it would be better to stationary and turn.
if abs(heading) >= 30:
if Debug.readyDebug:
print "Heading too large case!", heading
Action.walk(0, 0, hMath.CLIP(heading, 60))
return False
# Should this or / and sign?
if not anyVisionRobotBlockingMyPath() and not anyGPSRobotBlockingMyPath():
if Debug.readyDebug:
print "No robot block my path, lets go as quickly as possible!"
Action.walk(Action.MAX_FORWARD, 0, 0)
return False
# Now the heading is not too large, draw a vector directly to the kickoff
# position.
Action.walk(Action.MAX_FORWARD, 0, 0)
return True
def findBallDefender():
time = Global.lostBall % TIMER_3
if time < TIMER_1:
turnccw = hMath.CLIP(Global.ballH,80)
Action.walk(0,0,turnccw, minorWalkType = Action.SkeFastForwardMWT)
hTrack.scan()
elif time < TIMER_2:
hFWHead.compulsoryAction = hFWHead.doNothing
sFindBall.findBySpin()
else:
targetH = Global.selfLoc.getHeading() + Global.ballH
hTrack.saGoToTargetFacingHeading(Global.selfLoc.getX(), \
Constant.FIELD_LENGTH * 0.25, targetH)
def findBallStriker():
time = Global.lostBall % TIMER_3
if time < TIMER_1:
turnccw = hMath.CLIP(Global.ballH, 80)
Action.walk(0, 0, turnccw, minorWalkType=Action.SkeFastForwardMWT)
hTrack.scan()
elif time < TIMER_2:
hFWHead.compulsoryAction = hFWHead.doNothing
sFindBall.findBySpin()
else:
targetH = Global.selfLoc.getHeading() + Global.ballH
targetX, targetY, _rangeX = hWhere.getStrikerPos(True)
hTrack.saGoToTargetFacingHeading(targetX, targetY, targetH)
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 performTimeElapsedAction(selType):
global gForceTimeElapse
global gKickType
global gLastKickFrame
global gLastBreakFrame
global gLastLocaliseFrame
global gKickCounter
global gBreakCounter
global gLocaliseCounter
gForceTimeElapse = True
if Action.shouldIContinueKick():
#print "Waiting for Kick to activate ...", gKickCounter
Action.continueKick()
moveHeadForwardTight()
elif gKickCounter > 0:
#print "Kicking ...", gKickCounter
if gKickType == AVOID_OWN_GOAL:
if gKickCounter > 10:
perform(0, 0, Action.MAX_TURN)
elif gKickCounter > 8:
perform(0, 0, 0)
else:
perform(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeGrabDribbleMWT)
elif gKickType == GAP_KICK:
selfH = Global.selfLoc.getHeading()
if abs(gKickHead - selfH) > 10:
turnCCW = gKickHead - selfH
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.SoftTapWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == SIDE_STEP_LEFT_SHOOT\
or gKickType == SIDE_STEP_RIGHT_SHOOT\
or gKickType == FWD_STEP_SHOOT:
# Do side step shoot...
if gKickCounter > FWD_STEP_SHOOT_DURATION:
left = Action.MAX_LEFT
if gKickType == SIDE_STEP_RIGHT_SHOOT:
left = -Action.MAX_LEFT
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 30)
perform(5, left, turnCCW)
# Do forward step shoot...
elif gKickCounter > NU_FWD_DURATION:
fwd = Action.MAX_FORWARD
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 15)
perform(fwd,
0,
turnCCW,
minorWalkType=Action.SkeFastForwardMWT)
# Do kick!
else:
gKickType = Action.NUFwdKickWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == JUST_SHOOT:
selfH = Global.selfLoc.getHeading()
if gKickHead - selfH > 5:
turnCCW = (gKickHead - selfH) / 2
# if (turnCCW > 0):
# left = Action.MAX_LEFT
# else:
# left = -Action.MAX_LEFT
#
# perform(5,left,hMath.CLIP(turnCCW,30))
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.HandKickLeftWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
else:
# Reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 160)
VisionLink.resetBall(x, y, 0, 0)
Action.continueKick()
Action.openMouth()
hTrack.panLow = False
gKickCounter -= 1
gLastKickFrame = Global.frame
elif gBreakCounter > 0:
# If we cannot see the ball, then reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
if not Global.vBall.isVisible():
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 50)
VisionLink.resetBall(x, y, 0, 0)
if gBreakCounter > 10:
Action.setHeadParams(0, 0, 0, Action.HTAbs_h)
Action.walk(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeFastForwardMWT)
elif gBreakCounter > 5:
Global.lostBall = Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(True)
rate = Action.SKE_FF_PG * 8.0 * 2.0 / 1000.0
fwd = Action.MAX_SKE_FF_FWD_STP * 0.8 / rate
Action.walk(fwd, 0, 0, minorWalkType=Action.SkeFastForwardMWT)
Action.openMouth()
else:
Global.lostBall = 8 - gBreakCounter + Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform()
Action.openMouth()
hTrack.panLow = True
gBreakCounter -= 1
gLastBreakFrame = Global.frame
elif Global.frame - gLastKickFrame > 1\
and Global.frame - gLastBreakFrame > 1:
#print "Kick!!!!"
selectKick(selType)
else:
resetPerform()
sGrab.resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def performToTargetGoal():
global gTurningDir
global gTurningCnt
global gLastVisTGoalFrame
global gLastVisTGoal
global gLastSelfLoc
global gTrustGpsGoal
global gTrustGpsGoalCounter
global gForceTimeElapse
Indicator.showFacePattern([0, 3, 3, 3, 0])
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 dodge behaviour, then continue.
if gDodgeCounter > 0 and performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
if gForceTimeElapse\
or (hMath.getTimeElapsed(sGrab.gLastGrabTime,VisionLink.getCurrentTime()) > MIN_TIME_TO_DRIBBLE
and Global.frame - gLastTurnFrame >= TURN_DURATION):
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 2500"
return performTimeElapsedAction(SEL_OFFENSIVE)
if hMath.getTimeElapsed(sGrab.gLastGrabTime,
VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE:
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 3000"
return performTimeElapsedAction(SEL_BREAK)
if Global.vTGoal.isVisible():
gLastVisTGoalFrame = Global.frame
gLastVisTGoal = Global.vTGoal.getCopy()
gLastSelfLoc = Global.selfLoc.getCopy()
gTurningDir = None
gTurningCnt = 0
minH, maxH = selectGap()
padding = abs(maxH - minH) / 5.0
# Our heading is within the gap for sure.
if minH < -padding and padding < maxH\
and Global.pWalkInfo.getCurrentMinorWalkType() != Action.GrabTurnOnlyMWT\
and Global.frame - gLastTurnFrame >= TURN_DURATION:
gForceTimeElapse = True
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 3000"
return performTimeElapsedAction(SEL_OFFENSIVE)
# Perform dodge, when there's something in front of you.
# May be dodge a goalie.
elif performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# Our heading is not within the gap and can detect the gap.
elif minH != 0 and maxH != 0:
turnCCW = (minH + maxH) / 2.0
# Our heading is not within the gap and can't detect the gap.
else:
turnCCW = Global.vTGoal.getHeading()
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
if performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
turnCCW = gLastVisTGoal.getHeading()
else:
if gTrustGpsGoal and performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
#print "turning",
h = hMath.getHeadingToFaceAt(Global.selfLoc,\
Constant.TARGET_GOAL_X,\
Constant.TARGET_GOAL_Y)
if abs(h) < 15:
#print ", turning with gps",
# If we see many obstacle in front of us, it may explain why we are not seeing any goals.
# Thus, do some dodging.
if gTrustGpsGoal\
and performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# Gps thinks, we are in front of the goal, but we can't see it.
# Thus, Gps is possibly wrong.
gTrustGpsGoalCounter += 1
if gTrustGpsGoalCounter > TRUST_GPS_GOAL_DURATION:
gTrustGpsGoal = False
# We still trust gps.
if gTrustGpsGoal:
turnCCW = h
#print ", trust gps goal",
# Otherwise, do something smart.
else:
# We tried both directions, we are completely lost...
# Maybe, do localise?
# At the moment, we just keep spinning...
if gTurningCnt >= 2:
if gTurningDir == Constant.dANTICLOCKWISE:
turnCCW = -Action.MAX_TURN
else:
turnCCW = Action.MAX_TURN
# Otherwise, turn both directions to find the target goal.
else:
if gTurningDir == None:
h1 = hMath.getHeadingToFaceAt(sGrab.gLastGrabSelfLoc,\
Constant.TARGET_GOAL_X,\
Constant.TARGET_GOAL_Y)
if h1 > 0:
gTurningDir = Constant.dANTICLOCKWISE
else:
gTurningDir = Constant.dCLOCKWISE
if gTurningDir == Constant.dANTICLOCKWISE\
and h > MAX_OVER_TURN:
gTurningDir = Constant.dCLOCKWISE
gTurningCnt += 1
elif gTurningDir == Constant.dCLOCKWISE\
and h < -MAX_OVER_TURN:
gTurningDir = Constant.dANTICLOCKWISE
gTurningCnt += 1
if gTurningDir == Constant.dANTICLOCKWISE:
turnCCW = Action.MAX_TURN
else:
turnCCW = -Action.MAX_TURN
# 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 frameReset():
VisionLink.startProfile("GlobalFrameReset")
global lastFrameReset, forceHighGain, isTimeCritical
global myRole, myLastRole
global pan, tilt, crane
global selfLoc, teammatesLoc, gpsLocalBall, gpsGlobalBall
global sharedBall, sharedBallX, sharedBallY, sharedBallVar, sharedBallSourceRobot
global pWalkInfo, desiredHead, desiredPan, desiredTilt, desiredCrane, isHighGain
global myPlayerNum, otherTeammates, otherValidTeammates, otherValidForwards, teamColor
global lastVisBall
global fstVisBallDist, fstVisBallHead, sndVisBallDist, sndVisBallHead, thdVisBallDist, thdVisBallHead
global weightedVisBallDist, weightedVisBallHead
global attackerCount, attackerWithBallCount
global vBall, vTGoal, vOGoal
global vBluePink, vPinkBlue, vYellowPink, vPinkYellow, seenBeacon
if frame == lastFrameReset:
return #already reset in this frame, quit
else:
lastFrameReset = frame
# Need to be reset, otherwise it over written by dynamic_gain,
# which is very dangerous
forceHighGain = None
#Kick.frameReset()
myLastRole = myRole
isTimeCritical = VisionLink.isTimeCritical()
#-----------------------------------
# Get the head motion info
pan = VisionLink.getJointSensor(Constant.jHeadPan)
tilt = VisionLink.getJointSensor(Constant.jHeadTilt)
crane = VisionLink.getJointSensor(Constant.jHeadCrane)
#-----------------------------------
# Get the GPS info
gpsGlobalBall.setVals(*VisionLink.getGPSBallInfo(Constant.CTGlobal))
gpsLocalBall.setVals(*VisionLink.getGPSBallInfo(Constant.CTLocal))
selfLoc.setVals(*VisionLink.getGPSSelfInfo())
#print "frameReset setting selfLoc at", ("%x" % id(selfLoc)) ,"=", selfLoc
for i in range(Constant.NUM_TEAM_MEMBER):
teammatesLoc[i].setVals( \
*VisionLink.getGPSTeammateInfo(Constant.CTGlobal, i+1))
#--------------------------------------
# Get the Shared ball
sharedBall = VisionLink.getSharedBallInfo()
sharedBallX, sharedBallY, sharedBallVar, sharedBallSourceRobot = sharedBall
#-----------------------------------
# Get pWalkInfo
pWalkInfo.setVals(*VisionLink.getPWalkInfo())
desiredHead = pWalkInfo.getDesiredHead()
desiredPan, desiredTilt, desiredCrane = desiredHead
isHighGain = pWalkInfo.isUsingHighGain()
#-----------------------------------
# Get team related info
myPlayerNum = VisionLink.getMyPlayerNum()
# VisionLink.setGPSGoal(Constant.BLUE_TEAM)
# VisionLink.setGPSGoal(Constant.RED_TEAM)
teamColor = VisionLink.getTeamColor()
# if penaltyShot: # Force penalty shooter colour
# teamColor = Constant.RED_TEAM
# elif lightingChallenge: # Force challenge colour
# teamColor = Constant.BLUE_TEAM
if teamColor == Constant.RED_TEAM: # RED
ownGoal = Constant.vobYellowGoal
targetGoal = Constant.vobBlueGoal
elif teamColor == Constant.BLUE_TEAM: # BLUE
ownGoal = Constant.vobBlueGoal
targetGoal = Constant.vobYellowGoal
else:
print "Warning: teamColor is None, the dog's coordinate is wrong"
#-----------------------------------
# Get the VisualObjects
vBluePink.setVals(
*VisionLink.getVisualObject(Constant.vobBlueOnPinkBeacon))
vPinkBlue.setVals(
*VisionLink.getVisualObject(Constant.vobPinkOnBlueBeacon))
vYellowPink.setVals(
*VisionLink.getVisualObject(Constant.vobYellowOnPinkBeacon))
vPinkYellow.setVals(
*VisionLink.getVisualObject(Constant.vobPinkOnYellowBeacon))
if vBluePink.getConfidence() > 0\
or vPinkBlue.getConfidence() > 0\
or vYellowPink.getConfidence() > 0\
or vPinkYellow.getConfidence() > 0:
seenBeacon = True
else:
seenBeacon = False
if vBall.isVisible():
lastVisBall.setVals(*vBall.getTuple())
vBall.setVals(*VisionLink.getVisualObject(Constant.vobBall))
# visual ball smoothing for ball grabbing
if vBall.isVisible():
thdVisBallDist = sndVisBallDist
thdVisBallHead = sndVisBallHead
sndVisBallDist = fstVisBallDist
sndVisBallHead = fstVisBallHead
fstVisBallDist = vBall.getDistance()
fstVisBallHead = vBall.getHeading()
weightedVisBallDist = fstVisBallDist*0.5\
+ sndVisBallDist*0.3\
+ thdVisBallDist*0.2
weightedVisBallHead = fstVisBallHead*0.5\
+ sndVisBallHead*0.3\
+ thdVisBallHead*0.2
#weightedVisBallHead = vBall.getHeading() * 0.2\
# + weightedVisBallHead * 0.8
vOGoal.setVals(*VisionLink.getVisualObject(ownGoal))
vTGoal.setVals(*VisionLink.getVisualObject(targetGoal))
#-----------------------------------
# Goals doesnt change their global positions
vOGoal.setPos(Constant.OWN_GOAL_X, Constant.OWN_GOAL_Y)
vTGoal.setPos(Constant.TARGET_GOAL_X, Constant.TARGET_GOAL_Y)
#-----------------------------------
# Set default final actions/indicators/wireless info.
global finalWirelessInfo
finalWirelessInfo = [-1, False, False, False, False, False, False]
#-----------------------------------
# Other variable to be reset
global lostBall, haveBall
global lostGoal, haveGoal
# Increase the lostBall counter if can't see a ball.
if vBall.isVisible():
haveBall += 1
lostBall = 0
else:
lostBall += 1
if True or lostBall >= 3:
haveBall = 0
# Cap lostBall if ball was just out so GPS ball is used. GPS ball is set
# to the drop-in points. This also allows role switching on the replaced
# ball
# (NOT doing this to the goalie, as it may cause unexpected behaviours.)
if VisionLink.gameDataIsValid() and \
0 <= VisionLink.getDropInTime() < Constant.DROP_IN_REACT_TIME:
#print "lostBall hack: dropInTime =", VisionLink.getDropInTime()
lostBall = hMath.CLIP(lostBall, Constant.LOST_BALL_LAST_VISUAL)
if vTGoal.isVisible():
lostGoal = 0
haveGoal += 1
else:
lostGoal += 1
haveGoal = 0
determineBallSource()
#-----------------------------------
# Set the wireless teammate communication variables.
global teamPlayers, forwardAlive, framesSince3ForwardsPresent, framesSince2ForwardsPresent
wirelessInfo = VisionLink.getWirelessTeammateInfo()
for i in range(Constant.NUM_TEAM_MEMBER):
teamPlayers[i].setVals(\
[i,\
wirelessInfo[i],\
wirelessInfo[i+4],\
wirelessInfo[i+8],\
wirelessInfo[i+12],\
wirelessInfo[i+16],\
wirelessInfo[i+20],\
wirelessInfo[i+24],\
wirelessInfo[i+28],\
wirelessInfo[i+32],\
wirelessInfo[i+36]])
otherTeammates = []
otherValidTeammates = []
otherValidForwards = []
attackerCount = 0
attackerWithBallCount = 0
for i in range(Constant.NUM_TEAM_MEMBER):
if i != myPlayerNum - 1:
otherTeammates.append(i)
if teamPlayers[i].isValid():
otherValidTeammates.append(i)
if not teamPlayers[i].isGoalie():
otherValidForwards.append(i)
if teamPlayers[i].isAttacker():
attackerCount = attackerCount + 1
if not teamPlayers[i].hasLostBall():
attackerWithBallCount = attackerWithBallCount + 1
#------------------------------------------------
# This is the RoboCup Game state, see the rule book
global state, lastMaxPWMValues, penalised
state = VisionLink.getTheCurrentMode()
penalised = VisionLink.getPenalised()
lastMaxPWMValues.pop(0)
lastMaxPWMValues.append(getMaxPWMDuty())
VisionLink.stopProfile("GlobalFrameReset")
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 performTimeElapsedAction(selType):
global gLastActionFrame
global gForceTimeElapse
global gKickType
global gLastKickFrame
global gLastBreakFrame
global gLastLocaliseFrame
global gKickCounter
global gBreakCounter
global gLocaliseCounter
gLastActionFrame = Global.frame
gForceTimeElapse = True
#print "performTimeElapsed(", selType, ")"
#if selType == SEL_OFFENSIVE:
# import traceback
# traceback.print_stack()
if Action.shouldIContinueKick():
#print "Waiting for kick to activate ...", gKickCounter
Action.continueKick()
moveHeadForwardTight()
elif gKickCounter > 0:
#print "Kicking ...", gKickCounter
if gKickType == AVOID_OWN_GOAL:
if gKickCounter > 10:
perform(0, 0, Action.MAX_TURN)
elif gKickCounter > 8:
perform(0, 0, 0)
else:
perform(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeGrabDribbleMWT)
elif gKickType == GAP_KICK:
selfH = Global.selfLoc.getHeading()
if abs(gKickHead - selfH) > 10:
turnCCW = gKickHead - selfH
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.SoftTapWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == SIDE_STEP_LEFT_SHOOT\
or gKickType == SIDE_STEP_RIGHT_SHOOT\
or gKickType == FWD_STEP_SHOOT:
#print "gKickType = SIDESTEP_X_SHOOT"
# Do side step shoot...
if gKickCounter > FWD_STEP_SHOOT_DURATION:
left = Action.MAX_LEFT
if gKickType == SIDE_STEP_RIGHT_SHOOT:
left = -Action.MAX_LEFT
turnCCW = 0
# FIXME: Sidestep to Gap
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 30)
perform(5, left, turnCCW)
# Do forward step shoot...
elif gKickCounter > NU_FWD_DURATION:
fwd = Action.MAX_FORWARD
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 15)
perform(fwd,
0,
turnCCW,
minorWalkType=Action.SkeFastForwardMWT)
# Do kick!
else:
gKickType = gNextKickType
print "after sidestep setting kick type to", gNextKickType
#perform(0,0,0)
Action.kick(gKickType)
moveHeadForwardTight()
elif gKickType == RELEASE:
#print "gKickType = RELEASE", gKickCounter
Action.walk(0, 0, 0, minorWalkType=Action.SkeGrabDribbleHoldMWT)
if gKickCounter > RELEASE_DURATION - 15:
#print "stopping..."
Action.openMouth()
sGrab.moveHeadForward()
elif gKickCounter > 15:
#print "scanning"
hTrack.stationaryLocalise(speed=6)
Action.closeMouth()
else:
#print "regrabbing"
sGrab.moveHeadForward()
Action.openMouth()
# Reset grab timers
sGrab.gLastGrabTime = VisionLink.getCurrentTime()
sGrab.gGrabCount = 0
gForceTimeElapse = False
else:
#print "gKickCounter =", gKickCounter, "- continuing kick"
# Reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk
# straight.
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 160)
VisionLink.resetBall(x, y, 0, 0)
Action.continueKick()
Action.openMouth()
hTrack.panLow = True
gKickCounter -= 1
gLastKickFrame = Global.frame
if gKickType != RELEASE:
sFindBall.setForce(sFindBall.FORCE_FOLLOW)
elif gBreakCounter > 0:
#print "gBreakCounter > 0"
#if Global.penaltyShot or Global.lightingChallenge:
# # Stop and release
# gBreakCounter = 0
# gKickType = RELEASE
# gKickCounter = RELEASE_DURATION
# If we cannot see the ball, then reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
if not Global.vBall.isVisible():
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 50)
VisionLink.resetBall(x, y, 0, 0)
if gBreakCounter > 10:
Action.setHeadParams(0, 0, 0, Action.HTAbs_h)
Action.walk(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeFastForwardMWT)
elif gBreakCounter > 6:
Global.lostBall = Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(True)
rate = Action.SKE_FF_PG * 8.0 * 2.0 / 1000.0
fwd = Action.MAX_SKE_FF_FWD_STP * 0.7 / rate
Action.walk(fwd, 0, 0, minorWalkType=Action.SkeFastForwardMWT)
Action.openMouth()
else:
Global.lostBall = 8 - gBreakCounter + Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY)
Action.openMouth()
hTrack.panLow = True
gBreakCounter -= 1
gLastBreakFrame = Global.frame
elif Global.frame - gLastKickFrame > 1\
and Global.frame - gLastBreakFrame > 1:
#print "gKickCounter = 0, Selecting kick", selType
selectKick(selType)
else:
resetPerform()
sGrab.resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def performDodge():
global gLastActionFrame, gLastDiagFrame, gDodgeCounter
global gForward, gLeft
gLastActionFrame = Global.frame
obsFront = VisionLink.getNoObstacleInBox(-25, 100, 25, 20,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
if gDodgeCounter > 0:
gLastDiagFrame = Global.frame
gDodgeCounter -= 1
Indicator.showFacePattern([1, 1, 1, 1, 1])
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading()
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading()
turnCCW = hMath.CLIP(turnCCW, 30)
return perform(gForward, gLeft, turnCCW)
elif hStuck.amIStuckForward(dodgeTime=0):
gLastDiagFrame = Global.frame
Indicator.showFacePattern([2, 2, 2, 2, 2])
forward = 0
left = 0
if hStuck.gStuckType == hStuck.STUCK_FWD_LEFT:
left = -Action.MAX_LEFT
elif hStuck.gStuckType == hStuck.STUCK_FWD_RIGHT:
left = Action.MAX_LEFT
# otherwise, stuck both.
else:
forward = -Action.MAX_FORWARD
gForward = forward
gLeft = left
return perform(gForward, gLeft, 0)
elif obsFront > MIN_OBS_TO_DODGE:
gLastDiagFrame = Global.frame
Indicator.showFacePattern([1, 1, 1, 1, 1])
gDodgeCounter = DODGE_DURATION
left = 0
selfX, selfY = Global.selfLoc.getPos()
# Decide which way to dodge.
# If we can see best gap, dodge towards the best gap.
if Global.vTGoal.isVisible():
minH, maxH = selectGap(gGapAimCount >= 2)
if Global.vTGoal.isVisible() and minH != 0 and maxH != 0:
# If we are within the best gap, don't dodge.
padding = abs(maxH - minH) / 5.0
if minH < -padding and padding < maxH:
gDodgeCounter = 0
return Constant.STATE_SUCCESS
# Dodge left.
if abs(minH) > abs(maxH):
left = Action.MAX_LEFT
# Otherwise, dodge right.
else:
left = -Action.MAX_LEFT
# If you are in offensive half,
# then always dodge towards the center of the field.
elif selfY > Constant.FIELD_LENGTH / 2.0:
if selfX > Constant.FIELD_WIDTH / 2.0:
left = Action.MAX_LEFT
else:
left = -Action.MAX_LEFT
# Otherwise, dodge towards outside?
else:
if selfX > Constant.FIELD_WIDTH / 2.0:
left = hMath.getSign(Action.MAX_LEFT,
selfX - Constant.FIELD_WIDTH / 3.0)
else:
left = hMath.getSign(Action.MAX_LEFT,
selfX - 2.0 * Constant.FIELD_WIDTH / 3.0)
gForward = 5
gLeft = left
return perform(gForward, left, 0)
else:
return Constant.STATE_SUCCESS
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 performToTargetGoal():
global gLastActionFrame
global gTurningDir
global gTurningCnt
global gGapAimCount
global gLastVisTGoalFrame
global gLastVisTGoal
global gLastSelfLoc
global gTrustGpsGoal
global gTrustGpsGoalCounter
global gForceTimeElapse
global gLastSelectedGap
global gTurnCCWSpd
Indicator.showFacePattern([0, 3, 3, 3, 0])
if not sGrab.isGrabbed:
print "Grab failed"
resetPerform()
return Constant.STATE_FAILED
gLastActionFrame = Global.frame
#print "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 dodge behaviour, then continue.
if gDodgeCounter > 0 and performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
if gForceTimeElapse\
or (hMath.getTimeElapsed(sGrab.gLastGrabTime,VisionLink.getCurrentTime()) > MIN_TIME_TO_DRIBBLE
and Global.frame - gLastTurnFrame >= TURN_DURATION):
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 2500"
return performTimeElapsedAction(SEL_OFFENSIVE)
if hMath.getTimeElapsed(sGrab.gLastGrabTime,
VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE:
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 3000"
return performTimeElapsedAction(SEL_BREAK)
if performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
left = None
if Global.vTGoal.isVisible():
gLastVisTGoalFrame = Global.frame
gLastVisTGoal = Global.vTGoal.getCopy()
gLastSelfLoc = Global.selfLoc.getCopy()
gTurningDir = None
gTurningCnt = 0
left = 0
# Choose gap
#minH, maxH = selectGap(gGapAimCount >= 2)
gLastSelectedGap = selectGap(gGapAimCount >= 2)
minH, maxH = gLastSelectedGap
padding = abs(maxH - minH) / 5.0
if minH < -padding and padding < maxH:
#VisionLink.sendYUVPlane()
gGapAimCount += 1
# Our heading is within the gap for sure.
if minH < -padding and padding < maxH\
and gGapAimCount >= MIN_GAP_AIM\
and hPWalk.isStepComplete():
gForceTimeElapse = True
return performTimeElapsedAction(SEL_OFFENSIVE)
# Perform dodge, when there's something in front of you.
# May be dodge a goalie.
elif performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# Our heading is not within the gap and can detect the gap.
else:
turnCCW = (minH + maxH) / 2.0
if abs(turnCCW) < 20:
turnCCW = turnCCW * 0.8
gTurnCCWSpd = hMath.getSign(1, turnCCW) * 120
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
if performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
left = 0
turnCCW = 0
else:
turnCCW = turnToGPSGoal()
# If we are in outside third, use diagonal grab dribbling.
if left == None:
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)
