def selectKickInLeftEdge():
selfH2BallH = hMath.normalizeAngle_0_360(selfH + ballH)
ballH2TGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y)
selfH2TGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2TGoalH)
ballH2CenterH = hMath.getHeadingBetween(ballX,ballY,\
Constant.FIELD_WIDTH/2.0,ballY+200)
selfH2CenterH = hMath.normalizeAngle_180(selfH2BallH - ballH2CenterH)
# We are safe to do something...
if 0 <= selfH2BallH <= 135:
#
if 45 <= abs(selfH2TGoalH) <= 60:
if selfH2TGoalH > 0:
return (KT_UPENN_LEFT, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_UPENN_RIGHT, ballH2TGoalH, Constant.dAUTO)
#
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# Otherwise, if we just approach, the ball might go out.
# Hence, get-around-the-ball action.
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
def shouldIWirelessBackOff():
global gBackOffType
BALL_DISTANCE = 50
## If you receive wireless information of mate being closer to the
## ball than you and you are behind him
selfX, selfY = Global.selfLoc.getX(), Global.selfLoc.getY()
for i in range(Constant.NUM_TEAM_MEMBER):
mate = Global.teamPlayers[i]
if i != Global.myPlayerNum - 1\
and mate.isValid()\
and not mate.isGoalie()\
and mate.hasSeenBall()\
and Global.ballD > mate.getBallDist()\
and mate.getBallDist() < BALL_DISTANCE:
#and Global.ballD < BALL_DISTANCE + 5
mateX, mateY = Global.teammatesLoc[i].getX(
), Global.teammatesLoc[i].getY()
## Compare headings of yourself and your mate from the
## ball to tell whether you two are near by.
selfH2ball = hMath.normalizeAngle_0_360(\
hMath.getHeadingBetween(Global.ballX,Global.ballY,selfX,selfY))
mateH2ball = hMath.normalizeAngle_0_360(\
hMath.getHeadingBetween(Global.ballX,Global.ballY,mateX,mateY))
selfD2mate = hMath.getDistanceBetween(selfX, selfY, mateX, mateY)
if abs(selfH2ball - mateH2ball) < 50:
if selfD2mate < 5\
and i > Global.myPlayerNum - 1:
return True
elif selfD2mate < 20:
gBackOffType = SIDE_BACKOFF
if Debug.backOffTriggerDebug:
print "shouldIWirelessBackOff(): True, Side backoff"
return True
if abs(selfH2ball - mateH2ball) < 70:
if selfD2mate < 5\
and i > Global.myPlayerNum - 1:
return True
elif selfD2mate < 30:
gBackOffType = FACE_BACKOFF
if Debug.backOffTriggerDebug:
print "shouldIWirelessBackOff(): True, Face backoff"
return True
return False
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 perform():
global gLastFrameCalled
global gDirection
global gNoGetBehindCounter
if gLastFrameCalled != Global.frame - 1:
resetPerform()
gLastFrameCalled = Global.frame
gNoGetBehindCounter = max(gNoGetBehindCounter - 1, 0)
selfLoc = Global.selfLoc
selfX, selfY = selfLoc.getPos()
selfH = selfLoc.getHeading()
ballX, ballY = Global.ballX, Global.ballY
ballD = Global.ballD
ballH = Global.ballH
selfH2BallH = selfH + ballH
ballH2OGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y)
selfH2OGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2OGoalH)
selfH2SafeH = hMath.normalizeAngle_180(selfH2OGoalH + 180)
if abs(selfH2OGoalH) > 110:
resetPerform()
return Constant.STATE_SUCCESS
# If we are far away from the ball, then just use our implicit get behind.
#if Global.ballD > 60:
# sFindBall.perform(doGetBehind = sFindBall.GET_BEHIND_LOTS)
# # Reset the direction.
# gDirection = None
# return Constant.STATE_EXECUTING
# First time to enter the function, so choose the direction.
if gDirection == None:
# go left
if selfH2OGoalH < 0:
gDirection = Constant.dANTICLOCKWISE
# go right
else:
gDirection = Constant.dCLOCKWISE
if gNoGetBehindCounter > 0:
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY)
else:
sFindBall.perform(True)
r = sGetBehindBall.performBall(dist=25,
direction=gDirection,
accuracy=10)
if r == Constant.STATE_SUCCESS:
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY)
gNoGetBehindCounter = NO_GET_BEHIND_DURATION
return Constant.STATE_EXECUTING
def performToPosition(targetPos=(0, 0), minTimeToDribble=1500):
global gForceToTargetGoal
global gForceTimeElapse
global targetPosition
pos = Global.selfLoc.getPos()
targetPosition = targetPos
dkd = hMath.getHeadingBetween(pos[0], pos[1], targetPos[0], targetPos[1])
performToDKD(dkd, minTimeToDribble)
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 perform():
global gObsToBall
global selfX, selfY, selfH, ballX, ballY, ballD, ballH
global gPerform, gPerformFrame
if gPerformFrame == Global.frame:
return gPerform
selfX, selfY = Global.selfLoc.getPos()
selfH = Global.selfLoc.getHeading()
ballX = Global.ballX
ballY = Global.ballY
ballH = Global.ballH
ballD = Global.ballD
ballH2TGoalH = hMath.getHeadingBetween(ballX, ballY,\
Constant.TARGET_GOAL_X,
Constant.TARGET_GOAL_Y)
# Special variable lighting challenge kick selection
if Global.lightingChallenge:
# In the rear of the field just kick the ball out - it will get replaced
# at the centre circle
if ballY < Constant.FIELD_LENGTH * 0.35:
rtn = selectKickOut()
elif ballY < Constant.FIELD_LENGTH * 0.55:
rtn = (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
rtn = (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
# Special penalty shooter kick selection
elif Global.penaltyShot:
if ballY < Constant.FIELD_LENGTH * 0.55:
rtn = (KT_GRAB_DRIBBLE_STOP, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
elif not sGrab.isGrabbed and Global.frame - gLastAvoidOwnGoalFrame < 5:
rtn = (KT_AVOID_OWN_GOAL, ballH2TGoalH, Constant.dAUTO)
# Usual attacker kick selection
elif ballY < Constant.FIELD_LENGTH * 0.4:
rtn = selectKickInDefensiveThird()
elif ballY < Constant.FIELD_LENGTH * 0.6:
rtn = selectKickInMiddleThird()
else:
rtn = selectKickInOffensiveThird()
gPerform = rtn
gPerformFrame = Global.frame
return rtn
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 selectKickInOffensiveThird():
tGoalY = Constant.TARGET_GOAL_Y + 20
# If the ball's y is somehow futher away than Constant.TARGET_GOAL_Y,
# then hack the target goal y.
if ballY > tGoalY:
tGoalY = ballY + 50
selfH2BallH = hMath.normalizeAngle_0_360(selfH + ballH)
ballH2TGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.TARGET_GOAL_X,tGoalY)
selfH2TGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2TGoalH)
ballH2CrossingH = hMath.getHeadingBetween(ballX,ballY,\
Constant.TARGET_GOAL_X,tGoalY-40)
selfH2CrossingH = hMath.normalizeAngle_180(selfH2BallH - ballH2CrossingH)
ballH2CenterH = hMath.getHeadingBetween(ballX,ballY,\
Constant.FIELD_WIDTH/2.0,ballY+200)
selfH2CenterH = hMath.normalizeAngle_180(selfH2BallH - ballH2CenterH)
if sGrab.isGrabbed:
if hWhere.ballInTriTLCorner()\
or hWhere.ballInTriTRCorner():
return (KT_GRAB_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
elif hWhere.ballOnLEdge(100)\
or hWhere.ballOnREdge(100):
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
# If we are stuck in offensive area, do something!!!
# FIXME : smart strategy
elif hStuck.gUseContestDetect and hStuck.isBallContested():
dkd = ballH2TGoalH
if hWhere.ballInTriTLCorner() or hWhere.ballInTriTRCorner():
dkd = ballH2CrossingH
selfH2dkd = hMath.normalizeAngle_180(selfH2BallH - dkd)
if 40 <= abs(selfH2dkd) < 75:
if selfH2dkd < 0:
return (KT_UPENN_RIGHT, dkd, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, dkd, Constant.dAUTO)
elif (ballD < sGrab.CLOSE_DIST or Global.frame - sGrab.gLastApproachFrame < 5)\
and (hWhere.isOnLEdge(EDGE_DIST)
or hWhere.isOnREdge(EDGE_DIST)
or hWhere.ballInTriTLCorner()
or hWhere.ballInTriTRCorner()
or selfH >= 180):
return (KT_GRAB_DRIBBLE, dkd, Constant.dAUTO)
else:
return (KT_DRIBBLE, dkd, Constant.dAUTO)
# If the ball is in the corner, kick it across the field.
elif hWhere.ballInTriTLCorner():
if abs(selfH2CrossingH) < 10:
return (KT_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
elif 45 <= abs(selfH2CrossingH) < 60:
if selfH2CrossingH < 0:
return (KT_UPENN_RIGHT, ballH2CrossingH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2CrossingH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
# If there's already a supporter waiting, then TURN KICK????
# If the ball is in the left edge.
elif hWhere.ballOnLEdge(100):
if hStuck.isBallContested():
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the ball is in the corner, kick it across the field.
elif hWhere.ballInTriTRCorner():
if abs(selfH2CrossingH) < 10:
return (KT_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
elif 45 <= abs(selfH2CrossingH) < 60:
if selfH2CrossingH < 0:
return (KT_UPENN_RIGHT, ballH2CrossingH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2CrossingH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2CrossingH, Constant.dAUTO)
# If there's already a supporter waiting, then TURN KICK????
# If the ball is in the right edge.
elif hWhere.ballOnREdge(100):
if hStuck.isBallContested():
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the ball is really close to the goal, dribble
elif not sGrab.isGrabbed\
and ballY > (Constant.FIELD_LENGTH - 30)\
and ballX > (Constant.FIELD_WIDTH/2 - Constant.GOAL_WIDTH/2 + 15)\
and ballX < (Constant.FIELD_WIDTH/2 + Constant.GOAL_WIDTH/2 - 15)\
and abs(selfH2TGoalH) < 45:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the ball is really close to the goal, grab dribble into goal
elif ballY > (Constant.FIELD_LENGTH - Constant.GOALBOX_DEPTH)\
and ballX > (Constant.FIELD_WIDTH/2 - Constant.GOAL_WIDTH/2 - 50)\
and ballX < (Constant.FIELD_WIDTH/2 + Constant.GOAL_WIDTH/2 + 50):
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
elif hStuck.isBallContested():
if sGrab.isGrabbed:
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
elif abs(selfH2TGoalH) < 40:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the robot is a little far, then dribble
elif ballY < Constant.FIELD_LENGTH * 0.65 and abs(selfH2TGoalH) < 15:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
return (KT_GRAB_DRIBBLE_GOAL, ballH2TGoalH, Constant.dAUTO)
def 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 doAvoidGoalBox(ownGoalBox=True):
headToBall = hMath.normalizeAngle_0_360(Global.ballH + \
Global.selfLoc.getHeading())
ballH2OGoalH = hMath.getHeadingBetween(Global.ballX,Global.ballY,\
Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y)
selfH2OGoalH = hMath.normalizeAngle_180(headToBall - ballH2OGoalH)
xOffset = 50
if (Global.selfLoc.getX() > Constant.FIELD_WIDTH/2 \
and Global.ballX < Constant.FIELD_WIDTH/2) \
or (Global.selfLoc.getX() < Constant.FIELD_WIDTH/2 \
and Global.ballX > Constant.FIELD_WIDTH/2):
xOffset += abs(Global.ballX - Constant.FIELD_WIDTH / 2) / 2
# Far away? Just go as normal
if not hWhere.inGoalBox(Global.selfLoc.getX(), Global.selfLoc.getY(),
ownGoalBox, Constant.GOALBOX_WIDTH):
doTrackBall()
# If next to the goal box walk to the corner of it
elif ownGoalBox and Global.selfLoc.getY() < Constant.GOALBOX_DEPTH - 10:
if Global.selfLoc.getX() < Constant.FIELD_WIDTH / 2:
hTrack.saGoToTargetFacingHeading(Constant.MIN_GOALBOX_EDGE_X,
Constant.OWN_GOALBOX_EDGE_Y + 25,
headToBall)
else:
hTrack.saGoToTargetFacingHeading(Constant.MAX_GOALBOX_EDGE_X,
Constant.OWN_GOALBOX_EDGE_Y + 25,
headToBall)
elif (not ownGoalBox
) and Global.selfLoc.getY() > Constant.TOP_GOALBOX_EDGE_Y + 10:
if Global.selfLoc.getX() < Constant.FIELD_WIDTH / 2:
hTrack.saGoToTargetFacingHeading(Constant.MIN_GOALBOX_EDGE_X,
Constant.TOP_GOALBOX_EDGE_Y - 25,
headToBall)
else:
hTrack.saGoToTargetFacingHeading(Constant.MAX_GOALBOX_EDGE_X,
Constant.TOP_GOALBOX_EDGE_Y - 25,
headToBall)
# Else if we are in front of goalbox walk to line up ouselves with
# the ball and goal
elif ownGoalBox:
# Move to the side of the ball, so we can localise as well.
# May need hysterisis here... not to switch sides frequently.
if selfH2OGoalH < 0:
adjX = xOffset
else:
adjX = -xOffset
hTrack.saGoToTargetFacingHeading(Global.ballX + adjX,
Constant.OWN_GOALBOX_EDGE_Y + 20,
headToBall)
elif not ownGoalBox:
# Move to the side of the ball, so we can localise as well.
# May need hysterisis here... not to switch sides frequently.
if selfH2OGoalH < 0:
adjX = xOffset
else:
adjX = -xOffset
hTrack.saGoToTargetFacingHeading(Global.ballX + adjX,
Constant.TOP_GOALBOX_EDGE_Y - 20,
headToBall)
def perform(params=None):
global gLastFrameCalled
global gLastDecisionFunction
global pos, friendPos, isPlayerSet, friend, friendloc, centerPos
hTeam.sendWirelessInfo()
id(params) # ignore
if not isPlayerSet:
if Global.myPlayerNum == 3:
friend = Global.teamPlayers[1]
friendloc = Global.teammatesLoc[1]
centerPos = (Constant.FIELD_WIDTH / 2, Constant.GOALBOX_DEPTH)
else:
friend = Global.teamPlayers[2]
friendloc = Global.teammatesLoc[2]
centerPos = (Constant.FIELD_WIDTH / 2,
Constant.FIELD_LENGTH - Constant.GOALBOX_DEPTH)
isPlayerSet = True
print "Player no:", Global.myPlayerNum
friendPos = friendloc.getPos()
pos = Global.selfLoc.getPos()
#if friend grab, go to center position, while looking at obstacle, get facing ball.
#if see ball far away, get behind ball (don't pass half the field) while scanning for ball and obstacle.
#if see ball < 1/2 field, chase and grab, turn no obstacle, until facing friend or gap upfield. and kick if obstacle near, if not stop realease, and scan look for obstacle and ball.
#if got wirelessball get behind wireless ball on keep a distance with obstacle, scan.
#if doesn't see and doesn't have wireless ball, find ball and goto base spot to spin again.
calculateObstacleBox()
shouldIBeDodgy = gUseDodgyDog
if gLastFrameCalled != Global.frame - 1:
resetPerform()
gLastFrameCalled = Global.frame
# elif shouldIKick():
# #shouldIBeDodgy = False #Really?
# doKick()
if (sGrab.isGrabbed):
print "friend is in :", friendPos[0], " ", friendPos[1]
r = scGrabDribble.performToPosition((friendPos[0], friendPos[1]))
# if r == Constant.STATE_EXECUTING:
# return
# else:
# scGrabDribble.resetPerform()
#align to friend gps, while avoiding the obstacle.
#and the pass to him.
elif (sGrab.grabbingCount > 0):
#print "grabbing"
r = sGrab.perform(doGetBehindv=sFindBall.GET_BEHIND_NONE)
if r == Constant.STATE_EXECUTING:
return
elif r == Constant.STATE_FAILED:
sGrab.resetPerform()
elif shouldIFindSpace():
#print "Find space"
doFindSpace()
elif not shouldIEndActivelyLocalise()\
or shouldIActivelyLocalise():
#print "active loc"
doActivelyLocalise()
elif (Global.vBall.isVisible() or Global.lostBall < Constant.LOST_BALL_GPS
or friend.hasSeenBall()):
#print "see a ball"
#if (friend.getTimeToReachBall() < selfobj.getTimeToReachBall() + 1000):
if (centerPos[1] < Constant.FIELD_LENGTH / 2):
isOtherHalf = Global.ballY > Constant.FIELD_LENGTH / 2
else:
isOtherHalf = Global.ballY < Constant.FIELD_LENGTH / 2
if (isOtherHalf): #And no near obstacle and Global.ballD > 30):
#print "ball the other end"
#if hMath.getDistSquaredBetween(pos[0],pos[1],Global.ballX,Global.ballY) > YDISTANCE_TO_BALL_SQR:
if (abs(Global.ballX - pos[0]) > YDISTANCE_TO_BALL):
sFindBall.perform(True)
#time to ball ? to get the ball if the other one is slow
if (centerPos[1] < Constant.FIELD_LENGTH / 2):
hTrack.saGoToTarget(
Global.ballX,
max(pos[1], Global.ballY - YDISTANCE_TO_BALL)
) #and ywhere the same or more if y diff less than 2 meter)
else:
hTrack.saGoToTarget(
Global.ballX,
max(pos[1], Global.ballY + YDISTANCE_TO_BALL)
) #and ywhere the same or more if y diff less than 2 meter)
else:
sFindBall.perform(True)
if (centerPos[1] < Constant.FIELD_LENGTH / 2):
movePoint = (max(
LOOK_OFFSETX,
min(Global.ballX,
Constant.FIELD_WIDTH - LOOK_OFFSETX)),
Global.ballY - YDISTANCE_TO_BALL)
else:
movePoint = (max(
LOOK_OFFSETX,
min(Global.ballX,
Constant.FIELD_WIDTH - LOOK_OFFSETX)),
Global.ballY + YDISTANCE_TO_BALL)
hTrack.saGoToTargetFacingHeading(
movePoint[0], movePoint[1],
hMath.getHeadingBetween(pos[0], pos[1], Global.ballX,
Global.ballY))
else:
#print "Grab"
r = sGrab.perform(doGetBehindv=sFindBall.GET_BEHIND_NONE)
if r == Constant.STATE_EXECUTING:
return
elif r == Constant.STATE_FAILED:
sGrab.resetPerform()
else: #if shouldIGoToCenter():
#print "goto center"
doGotoCenter()
# Use dodgy dog if required
fwdCmd = Action.finalValues[Action.Forward]
shouldIBeDodgy = shouldIBeDodgy and (not sGrab.isGrabbed) \
and sGrab.grabbingCount == 0 \
and Global.ballD > sGrab.CLOSE_DIST \
and fwdCmd > Action.MAX_SKE_FWD_SPD/2 \
and fwdCmd > Action.finalValues[Action.Left] \
and sDodgyDog.shouldIBeDodgyToBall()
if shouldIBeDodgy:
sDodgyDog.dodgyDogToBall()
def doFindSpace():
hTrack.saGoToTargetFacingHeading(
centerPos[0], centerPos[1],
hMath.getHeadingBetween(pos[0], pos[1], Global.ballX, Global.ballY))
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 selectKickInMiddleThird():
global gLastAvoidOwnGoalFrame
selfH2BallH = hMath.normalizeAngle_0_360(selfH + ballH)
ballH2OGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y)
selfH2OGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2OGoalH)
ballH2TGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.TARGET_GOAL_X,Constant.TARGET_GOAL_Y-50)
selfH2TGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2TGoalH)
ballH2CenterH = hMath.getHeadingBetween(ballX,ballY,\
Constant.FIELD_WIDTH/2.0,ballY+200)
selfH2CenterH = hMath.normalizeAngle_180(selfH2BallH - ballH2CenterH)
if sGrab.isGrabbed:
if hWhere.ballOnLEdge() or hWhere.ballOnREdge():
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
elif abs(selfH2OGoalH) < 40 or Global.vOGoal.isVisible():
gLastAvoidOwnGoalFrame = Global.frame
return (KT_AVOID_OWN_GOAL, ballH2TGoalH, Constant.dAUTO)
# If we are stuck in midfield area, do something!!!
# FIXME : smart strategy
elif hStuck.gUseContestDetect and hStuck.isBallContested():
if 45 <= abs(selfH2TGoalH) < 60:
if selfH2TGoalH < 0:
return (KT_UPENN_RIGHT, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2TGoalH, Constant.dAUTO)
elif (ballD < sGrab.CLOSE_DIST or Global.frame - sGrab.gLastApproachFrame < 5)\
and ((hWhere.isOnLEdge(EDGE_DIST) and abs(selfH2TGoalH) > 10)
or (hWhere.isOnREdge(EDGE_DIST) and abs(selfH2TGoalH) > 10)
or selfH >= 180):
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
# If the ball is in the left edge.
elif hWhere.ballOnLEdge():
return selectKickInLeftEdge()
# If the ball is in the right edge.
elif hWhere.ballOnREdge():
return selectKickInRightEdge()
# If the ball is not in the edges of the field
else:
if 45 <= abs(selfH2TGoalH) < 60:
if selfH2TGoalH < 0:
return (KT_UPENN_RIGHT, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2TGoalH, Constant.dAUTO)
elif abs(selfH2TGoalH) < 90:
if abs(selfH2TGoalH) < 10:
return (KT_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2TGoalH, Constant.dAUTO)
def selectKickInDefensiveThird():
global gLastAvoidOwnGoalFrame
selfH2BallH = hMath.normalizeAngle_0_360(selfH + ballH)
ballH2OGoalH = hMath.getHeadingBetween(ballX,ballY,\
Constant.OWN_GOAL_X,Constant.OWN_GOAL_Y)
ballH2MinOGoalBoxH = hMath.getHeadingBetween(ballX,ballY,\
Constant.MIN_GOALBOX_EDGE_X,Constant.OWN_GOAL_Y)
ballH2MaxOGoalBoxH = hMath.getHeadingBetween(ballX,ballY,\
Constant.MAX_GOALBOX_EDGE_X,Constant.OWN_GOAL_Y)
selfH2OGoalH = hMath.normalizeAngle_180(selfH2BallH - ballH2OGoalH)
ballH2CenterH = hMath.getHeadingBetween(ballX,ballY,\
Constant.FIELD_WIDTH/2.0,ballY+200)
selfH2CenterH = hMath.normalizeAngle_180(selfH2BallH - ballH2CenterH)
if sGrab.isGrabbed:
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
elif abs(selfH2OGoalH) < 60\
or ballH2MinOGoalBoxH < selfH2BallH < ballH2MaxOGoalBoxH\
or Global.vOGoal.isVisible():
gLastAvoidOwnGoalFrame = Global.frame
return (KT_AVOID_OWN_GOAL, ballH2CenterH, Constant.dAUTO)
# If we are stuck in defensive area, do something!!!
elif hStuck.gUseContestDetect and hStuck.isBallContested():
if 45 <= abs(selfH2CenterH) < 60:
if selfH2CenterH < 0:
return (KT_UPENN_RIGHT, ballH2CenterH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2CenterH, Constant.dAUTO)
elif (ballD < sGrab.CLOSE_DIST or Global.frame - sGrab.gLastApproachFrame < 5)\
and ((hWhere.isOnLEdge(EDGE_DIST) and abs(selfH2CenterH) > 10)
or (hWhere.isOnREdge(EDGE_DIST) and abs(selfH2CenterH) > 10)
or selfH >= 180):
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
else:
return (KT_DRIBBLE, ballH2CenterH, Constant.dAUTO)
# If the ball is in the left edge.
elif hWhere.ballOnLEdge():
return selectKickInLeftEdge()
# If the ball is in the right edge.
elif hWhere.ballOnREdge():
return selectKickInRightEdge()
# If the ball is not in the edge of the field
else:
if 45 <= abs(selfH2CenterH) < 60:
if selfH2CenterH < 0:
return (KT_UPENN_RIGHT, ballH2CenterH, Constant.dAUTO)
else:
return (KT_UPENN_LEFT, ballH2CenterH, Constant.dAUTO)
elif abs(selfH2CenterH) < 90:
if abs(selfH2CenterH) < 20:
return (KT_DRIBBLE, ballH2CenterH, Constant.dAUTO)
else:
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
# Otherwise we should do get behind??
# turn kick
else:
return (KT_GRAB_DRIBBLE, ballH2CenterH, Constant.dAUTO)
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])
