def getOutOfTheCircle(centerX, centerY, targetH, doForce=False, radius=40):
selfX, selfY = Global.selfLoc.getPos()
dist = hMath.getDistanceBetween(centerX, centerY, selfX, selfY)
heading = hMath.getHeadingBetween(centerX, centerY, selfX, selfY)
if amIInTheCircle(centerX,centerY,radius)\
or doForce:
if dist > radius:
relX = dist * 2 * math.cos(hMath.DEG2RAD(heading))
relY = dist * 2 * math.sin(hMath.DEG2RAD(heading))
else:
relX = radius * 2 * math.cos(hMath.DEG2RAD(heading))
relY = radius * 2 * math.sin(hMath.DEG2RAD(heading))
destX = centerX + relX
destY = centerY + relY
hTrack.saGoToTargetFacingHeading(destX, destY, targetH)
Indicator.showFacePattern([0, 2, 0, 2, 0])
return Constant.STATE_EXECUTING
return Constant.STATE_SUCCESS
def trackVisualBallByProj(adjX=0, adjY=0, adjZ=0, isTurning=False, adjPos=1.0):
ballPos = VisionLink.getProjectedBall()
# If projection returns infinity, then use gps.
if abs(ballPos[0]) >= Constant.LARGE_VAL\
or abs(ballPos[1]) >= Constant.LARGE_VAL:
# We should divide the velocity adjustment by half.
trackVisualBallByTrig(adjX, adjY, adjZ, adjPos=adjPos)
return
ballPosX = ballPos[0] * adjPos
ballPosY = ballPos[1] * adjPos
#print " track by proj ",
# Note that xyz in getPointProjection is different from xyz to headMotion
if isTurning and abs(Global.ballH) > 80:
d = hMath.getDistanceBetween(0, 0, ballPosX, ballPosY)
x = d * math.cos(hMath.DEG2RAD(10))
y = d * math.sin(hMath.DEG2RAD(10))
if Global.ballH < 0:
x = -x
Action.setHeadParams(x + adjX, adjY, y + adjZ, Action.HTAbs_xyz)
else:
Action.setHeadParams(ballPosX + adjX, adjY, ballPosY + adjZ,
Action.HTAbs_xyz)
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 isHeadRightOk():
if not Global.vBall.isVisible():
return False
ballx = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD
bally = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD
return -2.0 < ballx < 3.2\
and bally < 3.4*Constant.BallRadius\
and Global.ballD < 12\
and abs(Global.pan) < 30
def isGoalieUPleftOk():
if hTrack.canSeeBall():
ballx = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD
bally = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD
if (ballx < 3.0 and ballx > -5.0 and bally < 4.4 * Constant.BallRadius
and Global.ballD < 18 and Global.tilt < -9
and abs(Global.pan) < 30):
return True
return False
def isUpennRightOk():
if not Global.vBall.isVisible():
return False
ballx = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD
bally = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD
maxBallRadiiAway = 4.5
return -5.0 < ballx < 3.0\
and bally < maxBallRadiiAway * Constant.BallRadius\
and abs(Global.ballH) < 30
def findByWeightedVisBall():
d = Global.weightedVisBallDist
h = Global.weightedVisBallHead
if abs(h) < 90:
x = d * math.sin(hMath.DEG2RAD(h))
y = d * math.cos(hMath.DEG2RAD(h))
Action.setHeadParams(x,Constant.BallDiameter,y,Action.HTAbs_xyz)
else:
Action.setHeadToLastPoint()
if not gHeadOnly:
# Make sure we use gps's information, not wireless
# FIXME: if we are going to dodgy dog then don't get behind
walkToBall(d,h,getBehind = gGetBehind)
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 clipFireballPanHack():
global gLastClipFireballFrame
if Global.vBall.isVisible():
ballx = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD
bally = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD
##~ print "local x %.2f, y%.2f ballH %.2f" % (ballx, bally,Global.ballH)
if abs(ballx) <= 2.5 and 0 <= bally <= 5.0 * Constant.BallRadius:
Action.finalValues[Action.Panx] = 0
gLastClipFireballFrame = Global.frame
elif False and gLastClipFireballFrame >= Global.frame - 1 \
and abs(ballx) <= 3.2 and 0 <= bally <= 5.0 * Constant.BallRadius:
Action.finalValues[Action.Panx] = 0
gLastClipFireballFrame = Global.frame
def areWeAboveTheLine(offset, angle, includeYou, tx, ty):
global targetX, targetY
# Draw the line.
ang = 90 - angle / 2.0
m = math.tan(hMath.DEG2RAD(ang))
bpos = (ty + offset) - m * tx
bneg = (ty + offset) + m * tx
selfX, selfY = Global.selfLoc.getX(), Global.selfLoc.getY()
# If you are far back then you are ok (hysterisis).
if (includeYou and selfY < Constant.GOALBOX_DEPTH)\
or selfY < 40:
if Debug.defenderTriggerDebug:
print "No problem because enough back!"
return False
# If I am behind the threshold line, no need to trigger.
if includeYou and (selfY < m * selfX + bpos) and (selfY <
-m * selfX + bneg):
if Debug.defenderTriggerDebug:
print "False because of the line!"
return False
return True
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 trackVisualBallByTrig(adjX=0, adjY=0, adjZ=0, isTurning=False, adjPos=1.0):
#print " track by trig ",
ballD, ballH = Global.ballD, Global.ballH
ballX = math.sin(hMath.DEG2RAD(ballH)) * ballD
ballY = math.cos(hMath.DEG2RAD(ballH)) * ballD
ballX = ballX * adjPos
ballY = ballY * adjPos
if isTurning and abs(Global.ballH) > 80:
d = Global.ballD
x = d * math.cos(hMath.DEG2RAD(10))
y = d * math.sin(hMath.DEG2RAD(10))
if Global.ballH < 0:
x = -x
Action.setHeadParams(x + adjX, Constant.BallDiameter + adjY, y + adjZ,
Action.HTAbs_xyz)
else:
Action.setHeadParams(ballX + adjX, Constant.BallDiameter + adjY,
ballY + adjZ, Action.HTAbs_xyz)
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 canDoGrab(safe):
global firedType, fired1, fired2
global gLastGrabBallH
global gLastGrabWeightedBallH
id(safe)
# Ball must be visible
if Global.vBall.isVisible():
ballD = Global.vBall.getDistance()
ballH = Global.vBall.getHeading()
else:
return False
can = True
# you are close to the ball
can = can and 0 <= ballD <= GRAB_DIST
# you are not turning right now!
can = can and Global.frame - gLastTurnFrame > 6 # strictly speaking it's 11, because 23 * 8 * 2 / 1000 * 30
# Check if the ball is really close (i.e. under the robot's chin.
# Copied from hTrack.clipFireballPan()
ballX = math.sin(hMath.DEG2RAD(ballH)) * ballD
ballY = math.cos(hMath.DEG2RAD(ballH)) * ballD
ballX1 = math.sin(hMath.DEG2RAD(
Global.weightedVisBallHead)) * Global.weightedVisBallDist
#ballY1 = math.cos(hMath.DEG2RAD(Global.weightedVisBallHead)) * Global.weightedVisBallDist
#can = can and abs(ballX) < 2.5
can = can and ballY < 15
#can = can and abs(ballX1) < 2.5
#can = can and ballY1 < 15
if abs(ballX) < 0.5 and ballY < 8:
gLastGrabBallH = 0
gLastGrabWeightedBallH = 0
if can:
firedType = 1
fired1 += 1
else:
can = can and abs(Global.weightedVisBallHead) < 8
can = can and abs(Global.ballH) < 8
can = can and abs(Global.weightedVisBallHead - Global.ballH) < 6
#can = can and abs(Global.fstVisBallHead - Global.thdVisBallHead) < 6
gLastGrabBallH = Global.ballH
gLastGrabWeightedBallH = Global.weightedVisBallHead
if can:
firedType = 2
fired2 += 1
# Force step complete, if all our grabbing criteria satisfies.
# But, does this work?
if can:
Action.forceStepComplete()
if False and can:
vel = VisionLink.getGPSBallVInfo(Constant.CTLocal)
velx, vely = vel[0], vel[1]
print ""
print "Camera Frame : ", Global.cameraFrame
print "ballX : ", ballX, " ballY : ", math.cos(
hMath.DEG2RAD(ballH)) * ballD
print "gps ball d : ", Global.gpsLocalBall.getDistance()
print "fst vis ball d : ", Global.fstVisBallDist, ", h : ", Global.fstVisBallHead
print "snd vis ball d : ", Global.sndVisBallDist, ", h : ", Global.sndVisBallHead
print "thd vis ball d : ", Global.thdVisBallDist, ", h : ", Global.thdVisBallHead
print "Global.weightedVisBallHead : ", Global.weightedVisBallHead
print "Global.ballH : ", Global.ballH
print "vel : ", velx, ",", vely
print "Since turned : ", Global.frame - gLastTurnFrame
print "lostball : ", Global.lostBall
print "Grab decision true : ", can
print "fired Type : ", firedType
print ""
return can
def approachBall(sGrabDoGetBehind=None):
global gLastApproachFrame
global gLastReallyCloseFrame
global gLastTurnFrame
resetPerform()
if sGrabDoGetBehind == None:
sGrabDoGetBehind = sFindBall.GET_BEHIND_DEFAULT
# Setting default action values with sFindBall.
# Action values are overwritten by the approach ball code below.
sFindBall.perform(doGetBehind=sGrabDoGetBehind)
if Global.ballD < TIME_CRITICAL_DIST and Global.lostBall <= 3:
VisionLink.setTimeCritical()
else:
VisionLink.clearTimeCritical()
if Global.lostBall < Constant.LOST_BALL_GPS and Global.ballD < CLOSE_DIST:
gLastApproachFrame = Global.frame
if Global.frame - gLastApproachFrame < 5:
VisionLink.setGrabbing()
# Try to step just the right amount, but limit the minimum step
# length. Reduce max to shorten step time and improve reaction time
#vel = VisionLink.getGPSBallVInfo(Constant.CTLocal)
#velX, velY = vel[0], vel[1]
rate = Action.SKE_FF_PG * 2.0 * 8.0 / 1000.0
fwd = Global.ballD - BALL_AGAINST_CHEST_DIST
fwd = hMath.EXTEND(fwd, Action.MAX_SKE_FF_FWD_STP * 0.7)
# don't walk backward
if fwd < 0:
fwd = 0
turnCCW = Global.ballH
left = Global.ballD * math.sin(hMath.DEG2RAD(Global.ballH))
#if left > 0:
# left = hMath.CLIP(left,Action.MAX_SKE_FF_LEFT_STP * 0.8)
#else:
# left = hMath.CLIP(left,Action.MAX_SKE_FF_RIGHT_STP * 0.8)
if Global.ballD < REALLY_CLOSE_DIST:
gLastReallyCloseFrame = Global.frame
mwt = Action.SkeNeckTurnWalkMWT
if Global.frame - gLastReallyCloseFrame < 5:
turnCCW = hMath.CLIP(turnCCW, 30)
if abs(left) < 2:
#print "branch 1"
fwd = fwd / rate
left = left / rate
#turnCCW = hMath.CLIP(turnCCW * 0.4,30) / rate
Action.walk(fwd, left, turnCCW, "sss", minorWalkType=mwt)
elif abs(left) < 8:
#print "branch 2"
left = left * 1.2 / rate
#turnCCW = hMath.CLIP(turnCCW * 0.4,30) / rate
Action.walk(0, left, 0, "sss", minorWalkType=mwt)
else:
#print "branch 3"
#turnCCW = hMath.CLIP(turnCCW * 0.5,30) / rate
turnCCW = turnCCW * 0.7
Action.walk(0, 0, turnCCW, minorWalkType=mwt)
gLastTurnFrame = Global.frame
else:
if abs(turnCCW) < 30:
#print "branch 5"
#left = left * 0.6
turnCCW = turnCCW * 1.1
Action.walk(fwd, 0, turnCCW, "ddd", minorWalkType=mwt)
else:
if Global.ballD < 30:
#print "branch 6"
turnCCW = turnCCW * 0.8
Action.walk(0, 0, turnCCW, minorWalkType=mwt)
else:
Action.walk(fwd, 0, turnCCW, minorWalkType=mwt)
gLastTurnFrame = Global.frame
# Use the hacked version of HTAbs_xyz
if Action.finalValues[Action.HeadType] == Action.HTAbs_xyz:
Action.finalValues[Action.HeadType] = Action.HTAbs_xyz_hack
# Force high gain!!
Global.forceHighGain = True
# setting hTrack.panLow variable to true.
hTrack.panLow = True
def grabBall():
global grabbingCount
global isGrabbed
global gLastGrabSelfLoc
global gLastGrabTime
debug("Grabbing!")
# Hack lost ball here.
Global.lostBall = 0
# Force high gain!!
Global.forceHighGain = True
turnCCW = gLastGrabBallH * 1.2
Action.walk(Action.MAX_FORWARD,
0,
turnCCW,
minorWalkType=Action.SkeNeckTurnWalkMWT)
grabbingCount += 1
if grabbingCount < GRAB_FORWARD_TIME:
# If we can see the ball still and is lined up, delay the grabbingCount.
# Experimenting!
if Global.vBall.isVisible():
ballX = math.sin(hMath.DEG2RAD(Global.ballH)) * Global.ballD
ballY = math.cos(hMath.DEG2RAD(Global.ballH)) * Global.ballD
if abs(ballX) < 0.5 and ballY < 8:
grabbingCount = hMath.EXTEND(grabbingCount - 1, 1)
Action.setHeadParams(0, -25, 30, Action.HTAbs_h)
Action.closeMouth()
debug("Going for the ball")
elif grabbingCount < GRAB_NO_CHECK_TIME:
moveHeadForward()
Action.closeMouth()
elif grabbingCount < GRAB_COMPLETE_TIME:
moveHeadForwardTight()
#Action.closeMouth()
Action.openMouth()
if Global.vBall.isVisible():
#print "wait i can see the ball still.."
VisionLink.clearTimeCritical()
resetPerform()
sFindBall.setHint(25, 90)
return Constant.STATE_FAILED
else:
moveHeadForward()
Action.openMouth()
isGrabbed = True
grabbingCount = 0
gLastGrabSelfLoc = Global.selfLoc.getCopy()
gLastGrabTime = VisionLink.getCurrentTime()
frameReset()
VisionLink.clearTimeCritical()
return Constant.STATE_EXECUTING
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 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 getDKDRange():
nearEdgeDist = 40
debugDKD = False
length = Constant.FIELD_LENGTH
width = Constant.FIELD_WIDTH
# Make sure determine ballSource is called
if Global.ballSource is None:
print "Warning: in getDKDRange() havent determined the ball source before getDKD()"
return None
X_ = Global.ballX
Y_ = Global.ballY
if debugDKD:
print "Ball Source: ", Global.ballSource, " X_ ", X_, " Y_ ", Y_
# If you don't know where the ball is determine dkd based on where you are.
if Global.ballSource == Constant.GPS_BALL\
and VisionLink.getGPSBallMaxVar() >= hMath.get95var(35):
X_, Y_ = Global.selfLoc.getPos()
if debugDKD:
print "Warning: in getDKDRange(): I dont trust GPS ball right now: use my own pos is X_ ", X_, " Y_ ", Y_
Y_ = hMath.CLIPTO(Y_, 0, length)
X_ = hMath.CLIPTO(X_, 0, width)
# If the ball is near one of the side edges and not in corners then the direction becomes up the field.
if Y_ > nearEdgeDist:
if X_ < nearEdgeDist: # on the left edge
if debugDKD:
print "On the left edge"
return (90, 70, 100, 10, 110)
elif X_ > (width - nearEdgeDist): #on the right edge
if debugDKD:
print "On the right edge"
return (90, 80, 110, 70, 170)
# upper 60%, can I use voak?
if Y_ > 0.6 * length:
# If target goal seen and is close and not many obstacles, it has a good heading, get the best heading like VOAK
if (Global.vTGoal.getConfidence() > 3
and Global.ballSource == Constant.VISION_BALL):
toY = length - Y_
toX = width / 2.0 - X_
xRange = Constant.GOAL_WIDTH / 2.0 + (toY *
math.tan(hMath.DEG2RAD(30)))
inVOAKcal = abs(toX) < xRange
if inVOAKcal:
(lmin, lmax, rmin, rmax) = VisionLink.getHeadingToBestGap()
lmin = hMath.normalizeAngle_0_360(lmin +
Global.selfLoc.getHeading())
lmax = hMath.normalizeAngle_0_360(lmax +
Global.selfLoc.getHeading())
rmin = hMath.normalizeAngle_0_360(rmin +
Global.selfLoc.getHeading())
rmax = hMath.normalizeAngle_0_360(rmax +
Global.selfLoc.getHeading())
if abs(lmax - lmin) > abs(rmax - rmin):
tdkdmin, tdkdmax = lmin, lmax
dkd = (lmin + lmax) / 2.0
else:
tdkdmin, tdkdmax = rmin, rmax
dkd = (rmin + rmax) / 2.0
if (dkd < 150 and dkd > 30):
return (dkd, tdkdmin, tdkdmax, 10, 170)
# Find the dkd fron the center of the target and own goals
dkdup = hMath.getHeadingBetween(X_, Y_, (width / 2.0), length)
dkdbottom = hMath.getHeadingBetween((width / 2.0), 0, X_, Y_)
# if i cant use voak, then the heading to the center of target goal is used as dkd
if Y_ > 0.6 * length:
tdkdmin = hMath.getHeadingBetween(X_, Y_, Constant.RIGHT_GOAL_POST,
length)
tdkdmax = hMath.getHeadingBetween(X_, Y_, Constant.LEFT_GOAL_POST,
length)
if debugDKD:
print "close to target goal, dkd directed to the target goal", Global.DKD[
0]
return (dkdup, tdkdmin, tdkdmax, 10, 170)
# if close to own goal, direted out of own goal
elif Y_ < 0.4 * length:
if debugDKD:
print "close to own goal, dkd directed out of the own goal"
return (dkdbottom, 20, 160, 10, 170)
# Now, the middle 20% of the field should have a continous change of direction.
else:
myy = Y_ - 0.4 * length
ratio = myy / (0.2 * length)
dkd = dkdup * ratio + dkdbottom * (1 - ratio)
if debugDKD:
print "middle 20%, linear interpolation"
return (dkd, 40, 140, 20, 160)
def walkToMovingBall(ballD,ballH,velX,velY):
#Indicator.showFacePattern([3,3,3,3,3])
id(ballD)
#print ""
#print "velocity X : ", velX
#print "velocity Y : ", velY
ballX = ballD * math.sin(hMath.DEG2RAD(ballH))
ballY = ballD * math.cos(hMath.DEG2RAD(ballH))
tx = ballX
ty = ballY
dx = velX
dy = velY
#print "ball : (", ballX, ",", ballY, ") ", ballH
# closest target point so far
minTarget = (Constant.LARGE_VAL,0,0)
# try out next 150 frames which is 5 seconds
for i in range(150):
tty = ty / 1.5 # assuming we walking in 2 cm / frame (over-estimating)
ttx = tx / 1.0
tt = max(tty,ttx)
# If the robot can reach to the target point comfortably,
# then go to the target point.
if tt + FRAME_TO_PREPARE < i:
#print "success"
#print "target : (", tx, ",", ty, ") "
fwd = ty
left = tx
if left > 0:
left = hMath.EXTEND(left,Action.MAX_SKE_FF_LEFT_STP*0.7)
elif left < 0:
left = hMath.EXTEND(left,Action.MAX_SKE_FF_RIGHT_STP*0.7)
th = hMath.getHeadingToRelative(tx,ty)
turnCCW = hMath.normalizeAngle_180((th + ballH) / 2)
Action.walk(fwd,left,turnCCW,"ddd",minorWalkType=Action.SkeFastForwardMWT)
return True
# If this target point has the smallest difference of time for
# ball and time for robot to reach the point, then consider
# this target point.
elif minTarget[0] > tt - i:
minTarget = (tt-i,tx,ty)
# updating next possible target point
tx += dx
ty += dy
# updating velocity by decaying
dx *= 0.98
dy *= 0.98
if minTarget[0] != Constant.LARGE_VAL:
fwd = minTarget[2]
left = minTarget[1]
if left > 0:
left = hMath.EXTEND(left,Action.MAX_SKE_FF_LEFT_STP*0.7)
elif left < 0:
left = hMath.EXTEND(left,Action.MAX_SKE_FF_RIGHT_STP*0.7)
th = hMath.getHeadingToRelative(tx,ty)
turnCCW = hMath.normalizeAngle_180((th + ballH) / 2)
Action.walk(fwd,left,turnCCW,"ddd",minorWalkType=Action.SkeFastForwardMWT)
return True
return False
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])
def walkToBall(ballD, ballH, getBehind=False, useVelocity=False):
vel = VisionLink.getGPSBallVInfo(Constant.CTLocal)
velX, velY = vel[0], vel[1]
if useVelocity\
and (abs(velX) > 2 or abs(velY) > 2):
#Indicator.showFacePattern([3,3,3,3,3])
velXY = hMath.getDistanceBetween(0, 0, velX, velY)
if velXY == 0:
velXY = 0.01
ballX, ballY = Global.ballX, Global.ballY
tx, ty = ballX, ballY
offsetX = DISTANCE_CONSTANT * velX / velXY
offsetY = DISTANCE_CONSTANT * velY / velXY
for i in range(30):
# time that the ball needs to get to the target point
timeBall = (
(DISTANCE_CONSTANT * i) / velXY) * (1 / 30.0) # seconds
# time that the robot needs to get to the target point
tx += offsetX
ty += offsetY
td = hMath.getDistanceBetween(0, 0, tx, ty)
#
timeRobot = (td / 30.0) # seconds
# If the robot can reach to the target point comfortably,
# then go to the target point.
if timeRobot + TIME_TO_PREPARE < timeBall:
th = hMath.getHeadingToRelative(tx, ty)
relh = hMath.normalizeAngle_180(th - ballH)
fComp = math.cos(hMath.DEG2RAD(relh))
lComp = math.sin(hMath.DEG2RAD(relh))
if fComp > 0:
maxF = Action.MAX_FORWARD_SKE
else:
maxF = Action.MAX_BACKWARD_SKE
if lComp > 0:
maxL = Action.MAX_LEFT_SKE
else:
maxL = Action.MAX_RIGHT_SKE
if maxF * abs(fComp) >= maxL * abs(lComp):
scale = 1.0 / abs(fComp)
else:
scale = 1.0 / abs(lComp)
fComp = scale * fComp
lComp = scale * lComp
fwd = maxF * fComp
left = maxL * lComp
Action.walk(fwd, left, ballH / 2, "ddd")
return
walkToStillBall(ballD, ballH, getBehind)
