def getObstaclesToGoal():
(lGoalX, lGoalY) = hMath.getLocalCoordinate(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(),
Constant.TARGET_GOAL_X,
Constant.TARGET_GOAL_Y - 30)
(lFromX, lFromY) = hMath.getLocalCoordinate(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(),
Constant.FIELD_WIDTH / 2,
Constant.FIELD_LENGTH / 2 + 20)
return VisionLink.getNoObstacleBetween(lFromX, lFromY, lGoalX, lGoalY,
Constant.GOAL_WIDTH, 0, 5,
Constant.OBS_USE_SHARED)
def getGapTo(targetX, targetY):
(lTargetX,
lTargetY) = hMath.getLocalCoordinate(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), targetX,
targetY)
gap = VisionLink.getBestGap(lTargetX, lTargetY, GAP_MAX_DIST, GAP_MIN_DIST,
GAP_MIN, GAP_MIN_INTENSITY,
Constant.OBS_USE_SHARED)
# getBestGap currently has some wierdness, so try to filter it here
if gap[2] > gap[0] and gap[2] < gap[1]:
return gap
return None
def shouldIBeDodgy(destX, destY, fixedDest = False,\
fixedDestVar = FIXED_DEST_VAR):
global gUseMinDist, gUseMinIntensity
#return False #HACK: Make dodgydog off all the time
#return True #HACK: Make dodgydog on all the time
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
dist = hMath.getDistanceBetween(selfX, selfY, destX, destY)
localDest = hMath.getLocalCoordinate(selfX, selfY, selfH, destX, destY)
localHead = hMath.cartToPolar(*localDest) # zero right
if dist <= MIN_DODGY_TARGET_DIST:
return False
if fixedDest and dist < fixedDestVar:
return False
# clip distance we count obstacles to MAX_DODGY_OBSTACLE_DIST or
# (dist - MIN_DODGY_TARGET_DIST) so we don't dodge obstacles that are
# miles away, and don't dodge obstacles that are right next to the
# target.
dist -= MIN_DODGY_TARGET_DIST
if dist >= MAX_DODGY_OBSTACLE_DIST:
dist = MAX_DODGY_OBSTACLE_DIST
localDest = hMath.polarToCart(dist, localHead)
tmp = VisionLink.getNoObstacleBetween(0, 0,
int(localDest[0]), int(localDest[1]),
CORRIDOR_WIDTH, gUseMinDist,
gUseMinIntensity, Constant.OBS_USE_NONE)
if Debug.dodgyDebug:
print "shouldIBeDodgy: obs in corr lhead, dist (", localHead, \
", ", dist, ") = ", tmp,
if tmp > MIN_OBS_IN_CORRIDOR:
#when get a better sidestep, take out mindist
#requirement and when close add left component?
if Debug.dodgyDebug:
print "-> True"
return True
if Debug.dodgyDebug:
print "-> False"
return False
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 perform(dkd=90, dist=20, direction=None, bx=None, by=None, accuracy=20):
global gDirection
#if side != None:
# print "Warning: sGetBehindBall.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()
# Work out if we are to the left or right of the ball (relative to DKD as 0)
lRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
if direction == None and gDirection == None:
if lRobotH < 0: # robot to the left
#print "robot to left of ball",
gDirection = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
gDirection = Constant.dCLOCKWISE
elif direction != None:
gDirection = direction
# The circling point can be calculated as looking from the ball
# towards the robot at CIRCLE_DEGREES to the left/right, and distance
# dist. CircleAng is from the ball facing the robot with positive x
# at zero degrees
# if robotH > 180 - CIRCLE_DEGREES:
# # If we are within CIRCLE_DEGREES of the target point don't overshoot
# circleAng = 90 + (180 - robotH)
# elif robotH < -180 + CIRCLE_DEGREES:
# circleAng = 90 - (-180 + robotH)
# else:
if gDirection == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
#print ""
#print "local robot H ", lRobotH
# relative to target facing robot
# This factor is used to adjust the dodgyness of the sidewards and
# backwards ability of fast skelliptical walk.
factor = 1 #max(min(abs(180-lRobotH)/60.0,2),1)
lcx = math.cos(hMath.DEG2RAD(circleAng)) * dist * factor
lcy = math.sin(hMath.DEG2RAD(circleAng)) * dist * factor
#print "circleAng", circleAng, "rel circle pos", circleRelX, circleRelY
robotH = hMath.normalizeAngle_0_360(lRobotH + dkd) # now global
cx, cy = hMath.getGlobalCoordinate(bx, by, robotH, lcx, lcy)
#print " local circle pos : (", lcx ,",",lcy,")"
#print "my pos : (", myx, ",", myy, ",",myh,")"
#print "global robotH ", robotH
#print "global ball :(", bx, ",", by, ") global circle pos : (", cx ,",",cy,")"
# circleX/Y now is the global coords of the circle point, so walk there.
bh = hMath.getHeadingBetween(myx, myy, bx, by) # global
lbh = hMath.normalizeAngle_180(bh - myh)
lcx, lcy = hMath.getLocalCoordinate(myx, myy, myh, cx, cy)
#lcdSquared = hMath.getDistSquaredBetween(0,0,lcx,lcy)
Action.walk(lcy, lcx, lbh, "ddd", minorWalkType=Action.SkeFastForwardMWT)
if abs(hMath.normalizeAngle_180(bh -
dkd)) < accuracy and abs(lbh) < accuracy:
resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def performBall(dkd=90, dist=20, direction=None, accuracy=20):
global gLastCalledFrame
global gDirection
global gLastCalledFrame
bx, by = Global.ballX, Global.ballY
myx, myy = Global.selfLoc.getPos()
myh = Global.selfLoc.getHeading()
# If this function wasn't called in previous frame, then reset the direction.
if Global.frame == gLastCalledFrame:
gDirection = None
gLastCalledFrame = Global.frame
# Work out if we are to the left or right of the ball (relative to DKD as 0)
lRobotH = hMath.getHeadingToMe(bx, by, dkd, myx, myy)
if abs(lRobotH) > 70:
gDirection = None
if direction == None and gDirection == None:
if lRobotH < 0: # robot to the left
#print "robot to left of ball",
gDirection = Constant.dANTICLOCKWISE
else: # robot to the right
#print "robot to right of ball",
gDirection = Constant.dCLOCKWISE
elif direction != None:
gDirection = direction
if gDirection == Constant.dANTICLOCKWISE:
circleAng = 90 + CIRCLE_DEGREES
else:
circleAng = 90 - CIRCLE_DEGREES
circleAng = hMath.normalizeAngle_180(circleAng)
# This factor is used to adjust the dodgyness of the sidewards and
factor = max(min(abs(180 - lRobotH) / 90.0, 1), 0)
lcx = math.cos(hMath.DEG2RAD(circleAng)) * dist * factor
lcy = math.sin(hMath.DEG2RAD(circleAng)) * dist * factor
robotH = hMath.normalizeAngle_0_360(lRobotH + dkd) # now global
cx, cy = hMath.getGlobalCoordinate(bx, by, robotH, lcx, lcy)
bh = Global.ballH
lcx, lcy = hMath.getLocalCoordinate(myx, myy, myh, cx, cy)
if abs(bh) > 30 and Global.ballD > dist:
Action.walk(0, 0, bh, minorWalkType=Action.SkeFastForwardMWT)
else:
Action.walk(lcy,
lcx,
bh * 0.75,
"ddd",
minorWalkType=Action.SkeFastForwardMWT)
if abs(hMath.normalizeAngle_180(myh + bh -
dkd)) < accuracy and abs(bh) < accuracy:
resetPerform()
return Constant.STATE_SUCCESS
gLastCalledFrame = Global.frame
return Constant.STATE_EXECUTING
def kickOffKickToGap():
global playCounter, whenGrabbed
global bestHeading, gBestHeading
(myX, myY) = Global.selfLoc.getPos()
myH = Global.selfLoc.getHeading()
if pReady.chosenFormation == 0:
print "chosenFormation =", pReady.chosenFormation, "- going right"
(kickX, kickY) = (Constant.MAX_GOALBOX_EDGE_X,
Constant.TOP_GOALBOX_EDGE_Y)
(suppX, suppY) = (kickX + 40, kickY - 40)
(lKickX, lKickY) = hMath.getLocalCoordinate(myX, myY, myH, kickX,
kickY)
else:
print "chosenFormation =", pReady.chosenFormation, "- going left"
(kickX, kickY) = (Constant.MIN_GOALBOX_EDGE_X,
Constant.TOP_GOALBOX_EDGE_Y)
(suppX, suppY) = (kickX - 40, kickY - 40)
(lKickX, lKickY) = hMath.getLocalCoordinate(myX, myY, myH, kickX,
kickY)
if pReady.isCentre(Global.kickOffPos):
if playCounter > 120: # 6 sec, just in case
return False
if whenGrabbed == 0:
#print "grabbing"
if sGrab.grabbingCount == 0:
# Find the obstacle gap just before we drop the head to grab
(gapLeft, gapRight, bestHeading, angle) = \
VisionLink.getBestGap(lKickX, lKickY,
GAP_MAX_DIST,GAP_MIN_DIST,
GAP_MIN,GAP_MIN_INTENSITY,
Constant.OBS_USE_SHARED)
gBestHeading = hMath.local2GlobalHeading(
Global.selfLoc, bestHeading)
#gBestHeading = hMath.getHeadingBetween(Constant.FIELD_WIDTH/2, Constant.FIELD_LENGTH/2, kickX, kickY)
print "local target", lKickX, lKickY
print "gap", bestHeading, gBestHeading
if sGrab.perform() == Constant.STATE_FAILED:
return False
if sGrab.isGrabbed:
print "grabbed, best gap at local", bestHeading, \
"global", gBestHeading
whenGrabbed = playCounter
return True
# Don't prevent other forward from being attacker
Global.myRole = Global.myLastRole = Constant.SUPPORTER
relH = gBestHeading - Global.selfLoc.getHeading()
if abs(relH) >= 3:
if sGrabDribble.perform(0, 0, relH) == Constant.STATE_FAILED:
return False
return True
Action.kick(Action.SoftTapWT)
if Action.shouldIContinueKick():
Action.continueKick()
return True
sGrab.resetPerform()
return False
elif pReady.isForward(Global.kickOffPos):
if playCounter < 80:
sDodgyDog.dodgyDogTo(suppX, suppY)
hTrack.stationaryLocalise(6, 30, -30)
return True
elif playCounter < 120:
if hTrack.canSeeBall and Global.ballD < 30:
Global.myRole = Global.myLastRole = Constant.ATTACKER
return False
h = hMath.normalizeAngle_0_360(myH + Global.ballH)
hTrack.saGoToTargetFacingHeading(suppX, suppY, h)
hFWHead.DecideNextAction()
return True
else:
Global.myRole = Global.myLastRole = Constant.ATTACKER
return False
else:
return False
return False
def dodgyDogTo(destX, destY, fixedDest = False, fixedDestVar = FIXED_DEST_VAR):
global gLastBestHeading, gNewHeading
global gBestHeadingInfluence, gLastBestHeadingInfluence
global gUseMinGap, gUseMinIntensity, gUseMinDist
global gSidestepCounter, gLeft
myPos = Global.selfLoc.getPos()
myHeading = Global.selfLoc.getHeading()
localDest = hMath.getLocalCoordinate(myPos[0], myPos[1], myHeading,
destX, destY)
localHead = hMath.cartToPolar(*localDest) - 90 # zero forward
distToDest = hMath.getDistanceBetween(myPos[0],myPos[1],destX,destY)
if fixedDest and distToDest < fixedDestVar:
return Constant.STATE_SUCCESS
# bestGap = (hLeft, hRight, hBest, gapsize) degrees
bestGap = VisionLink.getBestGap(localDest[0], localDest[1],
GAP_MAX_DIST, gUseMinDist, gUseMinGap,
gUseMinIntensity, Constant.OBS_USE_NONE)
if bestGap != None:
if Debug.dodgyDebug:
print "Dodgy: localxy_dest", localDest, "dist", distToDest
print "bestgap (left, right, best, size) =", bestGap
thisBestHeading = bestGap[2]
# Don't try to head more than x away from direct heading
if abs(localHead - thisBestHeading) > MAX_DODGY_DODGE:
if thisBestHeading > localHead:
thisBestHeading = localHead + MAX_DODGY_DODGE
elif thisBestHeading < localHead:
thisBestHeading = localHead - MAX_DODGY_DODGE
if gNewHeading:
gLastBestHeading = thisBestHeading
gNewHeading = False
bestHeading = thisBestHeading * gBestHeadingInfluence
bestHeading += gLastBestHeading * gLastBestHeadingInfluence
bestHeading /= gBestHeadingInfluence + gLastBestHeadingInfluence
gLastBestHeading = bestHeading
# Don't try to turn more than 30 in one go
bestHeading = hMath.CLIP(bestHeading, 30)
if Debug.dodgyDebug:
if VisionLink.ObstacleGPSValid():
print "dodgyDog: heading ", thisBestHeading, "->", \
bestHeading, " (GPS)"
else:
print "dodgyDog: heading ", thisBestHeading, "->", \
bestHeading, " (Local)"
if abs(bestHeading - Global.ballH) < 10:
Indicator.showFacePattern([0,2,2,2,0])
elif bestHeading < Global.ballH - 60:
Indicator.showFacePattern([3,0,0,0,0])
elif bestHeading < Global.ballH:
Indicator.showFacePattern([3,2,0,0,0])
elif bestHeading > Global.ballH + 60:
Indicator.showFacePattern([0,0,0,0,3])
elif bestHeading > Global.ballH:
Indicator.showFacePattern([0,0,0,2,3])
# Wag tail in direction of dodge.
tailH = Indicator.TAIL_H_CENTRED
if bestHeading > localHead + 10:
tailH = Indicator.TAIL_LEFT * 3/4
elif bestHeading < localHead - 10:
tailH = Indicator.TAIL_RIGHT * 3/4
Indicator.finalValues[Indicator.TailH] = tailH
#Indicator.finalValues[Indicator.TailV] = tailV
fwd = Action.MAX_FORWARD
turnccw = bestHeading
closeDist = CLOSE_DODGY_OBSTACLE_DIST
if closeDist > distToDest - MIN_DODGY_TARGET_DIST:
closeDist = distToDest - MIN_DODGY_TARGET_DIST
closePoint = hMath.polarToCart(closeDist, localHead)
closeObsIntensity = VisionLink.getNoObstacleBetween(0, 0,
int(closePoint[0]), int(closePoint[1]),
CLOSE_CORRIDOR_WIDTH, gUseMinDist,
gUseMinIntensity, Constant.OBS_USE_NONE)
if closeObsIntensity > MIN_OBS_IN_CORRIDOR:
if bestHeading > localHead:
gLeft = Action.MAX_LEFT
else:
gLeft = -Action.MAX_LEFT
#turnccw /= 2.0
gSidestepCounter = SIDESTEP_FOR
else:
if gSidestepCounter > 0:
gSidestepCounter -= 1
else:
gLeft = 0
if Debug.dodgyDebug and gLeft != 0:
if bestHeading > localHead:
print "sDodgyDog: Sidestepping LEFT (", gSidestepCounter, ")"
else:
print "sDodgyDog: Sidestepping RIGHT (", gSidestepCounter, ")"
Action.walk(fwd, gLeft, turnccw, minorWalkType=Action.SkeFastForwardMWT)
return Constant.STATE_EXECUTING
else:
if Debug.dodgyDebug:
print "getBestGap failed - can't be dodgy"
return Constant.STATE_FAILED