def doGetToTargetPoint(targetX, targetY):
global gTargetPointReqAccuracy
selfX, selfY = Global.selfLoc.getX(), Global.selfLoc.getY()
ballX, ballY, ballH = Global.ballX, Global.ballY, Global.ballH
h = hMath.normalizeAngle_0_360(hMath.RAD2DEG(math.atan2(\
ballY - selfY, ballX - selfX)))
#angle = hMath.absAngleBetweenTwoPointsFromPivotPoint(ballX, ballY, \
# targetX, targetY, \
# selfX, selfY)
distSquared = hMath.getDistSquaredBetween(targetX,targetY,selfX,selfY)
## if dist > 100 and angle < 80:
## hTrack.saGoToTarget(targetX,targetY)
## else:
## hTrack.saGoToTargetFacingHeading(targetX,targetY,h)
hTrack.saGoToTargetFacingHeading(targetX,targetY,h)
# Hysterisis for whether or not your at the defender point.
if distSquared <= hMath.SQUARE(gTargetPointReqAccuracy):
gTargetPointReqAccuracy = TARGET_PT_ACC_LARGE_CIRCLE
if abs(ballH) < 15:
Action.stopLegs()
else:
gTargetPointReqAccuracy = TARGET_PT_ACC_SMALL_CIRCLE
if abs(targetX - selfX) < 30 and abs(targetY - selfY) < 100:
checkThenBlock()
def 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 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 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 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 amIInTheWay(x, y, targetX, targetY):
myX, myY = Global.selfLoc.getPos()
if x == None or y == None or targetX == None or targetY == None:
return (None, None)
# if Global.frame % 30 == 0:
# print "amIInTheWay", x, y, targetX, targetY, "me", myX, myY
# We have a line in two point form, find offset of selfLoc from the line
dist = ((targetX - x)*(y - myY) - (x - myX)*(targetY - y)) / \
math.sqrt(hMath.SQUARE(targetX - x) + hMath.SQUARE(targetY - y))
# if Global.frame % 30 == 0:
# print "I'm", dist, "away from in the way",
# If we're not between the points return None
dx = x - targetX
dy = y - targetY
if abs(dx) > abs(dy) and \
((myX > x and myX > targetX) or (myX < x and myX < targetX)):
# if Global.frame % 30 == 0:
# print "but not between the points (X)"
return (None, None)
elif abs(dy) > abs(dx) and \
((myY > y and myY > targetY) or (myY < y and myY < targetY)):
# if Global.frame % 30 == 0:
# print "but not between the points (Y)"
return (None, None)
# The heading away is perpendicular to the target line
if dy == 0:
dy += 0.1 # prevent division by zero
head = -dx / dy
head = hMath.RAD2DEG(math.atan(head))
# Which way along the line is away? If dist is positive we are to the left
# of the line and want heading in the left half
if (dist > 0):
head += 180
head = hMath.normalizeAngle_180(head)
if Global.frame % 30 == 0:
print "dx =", dx, "dy =", dy, "head =", head
# if Global.frame % 30 == 0:
# print "and between the points. Head", head, "to escape"
return (dist, head)
def perform(paw=AUTO_PAW_KICK):
global pawKickType
if Global.lostBall > Constant.LOST_BALL_GPS:
return Constant.STATE_FAILED
# Head ball tracking movement
sFindBall.perform(True)
PAW_KICK_Y_OFFSET = Constant.BallRadius
PAW_KICK_X_OFFSET = 7.0
ballX = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD
ballY = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD
# The 2003 offset is 6 and it is not too bad.
if paw == AUTO_PAW_KICK:
if pawKickType == LEFT_PAW_KICK and ballX < -PAW_KICK_X_OFFSET / 2:
pawKickType = RIGHT_PAW_KICK
elif pawKickType == RIGHT_PAW_KICK and ballX > PAW_KICK_X_OFFSET / 2:
pawKickType = LEFT_PAW_KICK
else:
pawKickType = paw
if pawKickType == RIGHT_PAW_KICK:
targetX = ballX + PAW_KICK_X_OFFSET
else:
targetX = ballX - PAW_KICK_X_OFFSET
targetY = ballY - PAW_KICK_Y_OFFSET
targetD = math.sqrt(hMath.SQUARE(targetX) + hMath.SQUARE(targetY))
targetH = hMath.RAD2DEG(math.asin(targetX / targetD))
# Once the ball is lined up and well within range for the forward, the
# robot should commit to charge at the ball at full speed.
if abs(targetX) <= 1.0 and targetD < Constant.BallRadius * 1.5:
turnccw = hMath.CLIP(targetH, 10)
Action.walk(Action.MAX_FORWARD,
0,
turnccw,
minorWalkType=Action.SkeFastForwardMWT)
else:
sFindBall.walkToBall(targetD,
targetH,
getBehind=sFindBall.GET_BEHIND_LOTS)
return Constant.STATE_EXECUTING
def 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 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 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 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 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 calcTheta(self):
if (self.__x == 0 or self.__y == 0):
self.__theta = 0
else:
self.__theta = hMath.RAD2DEG(math.atan2(self.__y, self.__x))
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(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 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
def DecideNextAction(radius, aa, turndir, taOff, raOff, kp, ki, kd):
global lastBx, lastBy, sum_reqTurn, last_reqTurn, lastFrame, lastDir
skippedFrames = Global.frame - (lastFrame + 1)
# Reset the running sum if the routine is not called.
lastFrame = Global.frame # record the last frame
Indicator.showFacePattern(Constant.FP_DIR_KICK)
# No need to showFacePattern, because rAttacker has called already before entering this.
myx, myy = Global.selfLoc.getPos() # My (the dog) position and its heading
myh = Global.selfLoc.getHeading()
if Global.vBall.getConfidence() > 2:
bx, by = Global.vBall.getPos() # ball position
lastBx, lastBy = Global.vBall.getPos()
else:
bx = lastBx
by = lastBy
# Find the lp, given the ball position, taOff and the attack angle
# aa+180 because u want the tagental offset extended to behind the ball
# the point where the dog leaves the locus (leaving point)
lpx, lpy = hMath.getPointRelative(bx, by, (aa + 180), taOff)
# Find the center of the circle, given the lp, the radius and attack angle, and where u are.
angleFromBall = hMath.getHeadingBetween(bx, by, myx, myy)
angleRelativeToAA = hMath.normalizeAngle_180(angleFromBall - aa)
if angleRelativeToAA >= 0 and angleRelativeToAA <= 180:
turndir = Constant.dANTICLOCKWISE
else:
turndir = Constant.dCLOCKWISE
if turndir == Constant.dCLOCKWISE:
cx, cy = hMath.getPointRelative(lpx, lpy, (aa - 90), (radius + raOff))
else:
cx, cy = hMath.getPointRelative(lpx, lpy, (aa + 90), (radius + raOff))
c2my = hMath.getDistanceBetween(cx, cy, myx,
myy) # distance btw me and center
errL = radius - c2my # Indicates inside or outside of the circle. Used as adjustment in vector field.
# If not outside of the circle, no point to use alpha(Angle between center and cloest tangent point)
if c2my <= radius:
alpha = 0
else:
alpha = hMath.RAD2DEG(math.asin(radius / c2my)) # 0 <= alpha <= 90
beta = hMath.getHeadingBetween(
myx, myy, cx, cy) # Angle between global x-axis and center
# Fing the current vector
if turndir == Constant.dANTICLOCKWISE:
vectorArr = beta - 90 # Find the tangent vector perp to the c2my line first
if errL > 0:
vectorArr = vectorArr - (errL / radius * 45.0
) # linear discrepency
elif errL < 0: # When outside of the circle, the vector is
vectorArr = beta - alpha # the tangent closest to the circle.
else:
vectorArr = beta + 90
if errL > 0:
vectorArr = vectorArr + (errL / radius * 45.0)
elif errL < 0:
vectorArr = beta + alpha
b2my = hMath.getDistanceBetween(bx, by, myx, myy)
angleFromCenToMe = hMath.normalizeAngle_180(
hMath.getHeadingBetween(cx, cy, myx, myy) - aa)
distanceToAALine = b2my * math.sin(hMath.DEG2RAD(angleRelativeToAA))
##~ verticalDistToBall = b2my * math.cos(hMath.DEG2RAD(angleRelativeToAA))
if (raOff > 0 and abs(distanceToAALine) < raOff):
vectorArr = aa
##~ leftAdjust = -hMath.CLIP(distanceToAALine,3)
elif (turndir == Constant.dANTICLOCKWISE and angleFromCenToMe < 0
and angleFromCenToMe > -90 and abs(distanceToAALine) <
(raOff + radius)):
sHoverToBall.DecideNextAction()
Indicator.showFacePattern([1, 2, 1, 2, 0])
##~ reverse(aa,turndir,taOff,verticalDistToBall)
return
elif (turndir == Constant.dCLOCKWISE and angleFromCenToMe < 90
and angleFromCenToMe > 0 and abs(distanceToAALine) <
(raOff + radius)):
sHoverToBall.DecideNextAction()
Indicator.showFacePattern([1, 2, 1, 2, 0])
##~ reverse(aa,turndir,taOff,verticalDistToBall)
return
# After the vector is found, the PID controller will handle the job.
# Note:
# When u tune, first tune the KP to improve the rise time, then the KD to improve
# the overshoot, finally the KI for better steady state.
# better see http:##www.engin.umich.edu/group/ctm/PID/PID.html before u touch them
# thisTurn - The vector that brings the dog heading to the vector arrow
thisTurn = hMath.CLIP(hMath.normalizeAngle_180(vectorArr - myh), 30.0)
# For normal walk , kp = 40 ki = 8 kd = 5
KP = kp / 100.0 #0.5 ## Less Than 1
KD = kd / 100.0 #0.1
KI = ki / 100.0 #0.1 ## Has to be very small
if skippedFrames > 0:
sum_reqTurn = sum_reqTurn * math.pow(0.8, skippedFrames)
last_reqTurn = 0
# clip the running sum to be as most causing a 8 degree change.
if abs(sum_reqTurn) > (3.0 / KI):
sum_reqTurn = hMath.CLIP(sum_reqTurn, 3.0 / KI)
# if far away, there is no I and D effects, only the P.
if c2my > (4.0 * radius):
sum_reqTurn = 0
last_reqTurn = thisTurn
# The real PID calculations
#print thisTurn
#print last_reqTurn
#print sum_reqTurn
##~ print "==========new frame"
##~ print "ball pos %.2f , %.2f and my h %.2f" % (bx,by,myh)
##~ print "lp pos %.2f , %.2f and cx pos %.2f , %.2f" % (lpx,lpy,cx,cy)
##~ print "vectorArr %.2f and thisTurn %.2f " % (vectorArr,thisTurn)
##~ print "last %.2f and sum_reqTurn %.2f) " % (last_reqTurn,sum_reqTurn)
##~ print "P %.5f I %.5f D %.5f " % (KP * thisTurn,KI * sum_reqTurn,\
##~ KD * (thisTurn - last_reqTurn))
##~
reqTurn = (KP * thisTurn) + (KI *
sum_reqTurn) + (KD *
(thisTurn - last_reqTurn))
##~ print "the reqTurn is %.2f" % (reqTurn)
sum_reqTurn = sum_reqTurn + thisTurn # The integral part
last_reqTurn = thisTurn # Used in the derivative part
# Print the debugging info
Action.walk(7, 0, hMath.CLIP(reqTurn, 30), Action.EllipticalWalk)
##~ print "Direction: " , turndir , "vectorArr %.2f" % (vectorArr)
if ((angleRelativeToAA > 160 or angleRelativeToAA < -160)
and abs(hMath.normalizeAngle_180(myh - aa)) < 20):
sPawKick.firesPawKick(sPawKick.FIRE_PAWKICK_AUTO)
Indicator.showFacePattern([1, 1, 1, 1, 0])