def frameReset():
# Hack for when the robot has grabbed the ball.
if isGrabbed:
# The ball x, y should be a little bit in front from the robot,
# so that kick selection won't stuff up.
Global.ballX, Global.ballY = hMath.getPointRelative(
Global.selfLoc.getX(), Global.selfLoc.getY(),
Global.selfLoc.getHeading(), Constant.BallRadius)
Global.ballH = 0
Global.haveBall = 1
Global.lostBall = 0
VisionLink.resetBall(Global.ballX, Global.ballY, 0, 0)
VisionLink.setIfGPSVisionUpdate(0)
VisionLink.setGrabbed()
else:
VisionLink.setIfGPSVisionUpdate(1)
def isBallContested():
global gObsToBall, gObsBack
global gLastObstacleFrame
global gLastStuckFrame
global gLastPawFrame
global gLastContestedCalledFrame
global gflp, gfrp, gLastflp, gLastfrp
if Global.frame != gLastContestedCalledFrame:
x, y = hMath.getPointRelative(0, 0, Global.ballH, Global.ballD + 30)
# Count obstacles in corridor between us and the ball
gObsToBall = VisionLink.getNoObstacleBetween(0, 0, int(x), int(y), 50,
0, 10,
Constant.OBS_USE_GPS)
# Count obstacles in corridor behind me (these hang around in GPS
# for a while (we think))
gObsBack = VisionLink.getNoObstacleBetween(0, -35, 0, 0, 50, 0, 10,
Constant.OBS_USE_GPS)
gLastContestedCalledFrame = Global.frame
if gUseObstacleDetect and gObsToBall + gObsBack > MIN_OBS_CONTESTED:
gLastObstacleFrame = Global.frame
rlr = VisionLink.getLastMaxStepPWM(Constant.ssRL_ROTATOR)
rrr = VisionLink.getLastMaxStepPWM(Constant.ssRR_ROTATOR)
if gUseStuckDetect\
and Action.finalValues[Action.Forward] > 0\
and Action.finalValues[Action.ForwardStep] > 0\
and (rlr[1] > 800 or rrr[1] > 800):
gLastStuckFrame = Global.frame
if gUsePawDetect and (gflp > 0 or gfrp > 0):
gLastPawFrame = Global.frame
return Global.frame - gLastObstacleFrame < 15\
or Global.frame - gLastStuckFrame < 15\
or Global.frame - gLastPawFrame < 15
def performTimeElapsedAction(selType):
global gForceTimeElapse
global gKickType
global gLastKickFrame
global gLastBreakFrame
global gLastLocaliseFrame
global gKickCounter
global gBreakCounter
global gLocaliseCounter
gForceTimeElapse = True
if Action.shouldIContinueKick():
#print "Waiting for Kick to activate ...", gKickCounter
Action.continueKick()
moveHeadForwardTight()
elif gKickCounter > 0:
#print "Kicking ...", gKickCounter
if gKickType == AVOID_OWN_GOAL:
if gKickCounter > 10:
perform(0, 0, Action.MAX_TURN)
elif gKickCounter > 8:
perform(0, 0, 0)
else:
perform(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeGrabDribbleMWT)
elif gKickType == GAP_KICK:
selfH = Global.selfLoc.getHeading()
if abs(gKickHead - selfH) > 10:
turnCCW = gKickHead - selfH
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.SoftTapWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == SIDE_STEP_LEFT_SHOOT\
or gKickType == SIDE_STEP_RIGHT_SHOOT\
or gKickType == FWD_STEP_SHOOT:
# Do side step shoot...
if gKickCounter > FWD_STEP_SHOOT_DURATION:
left = Action.MAX_LEFT
if gKickType == SIDE_STEP_RIGHT_SHOOT:
left = -Action.MAX_LEFT
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 30)
perform(5, left, turnCCW)
# Do forward step shoot...
elif gKickCounter > NU_FWD_DURATION:
fwd = Action.MAX_FORWARD
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 15)
perform(fwd,
0,
turnCCW,
minorWalkType=Action.SkeFastForwardMWT)
# Do kick!
else:
gKickType = Action.NUFwdKickWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == JUST_SHOOT:
selfH = Global.selfLoc.getHeading()
if gKickHead - selfH > 5:
turnCCW = (gKickHead - selfH) / 2
# if (turnCCW > 0):
# left = Action.MAX_LEFT
# else:
# left = -Action.MAX_LEFT
#
# perform(5,left,hMath.CLIP(turnCCW,30))
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.HandKickLeftWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
else:
# Reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 160)
VisionLink.resetBall(x, y, 0, 0)
Action.continueKick()
Action.openMouth()
hTrack.panLow = False
gKickCounter -= 1
gLastKickFrame = Global.frame
elif gBreakCounter > 0:
# If we cannot see the ball, then reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
if not Global.vBall.isVisible():
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 50)
VisionLink.resetBall(x, y, 0, 0)
if gBreakCounter > 10:
Action.setHeadParams(0, 0, 0, Action.HTAbs_h)
Action.walk(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeFastForwardMWT)
elif gBreakCounter > 5:
Global.lostBall = Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(True)
rate = Action.SKE_FF_PG * 8.0 * 2.0 / 1000.0
fwd = Action.MAX_SKE_FF_FWD_STP * 0.8 / rate
Action.walk(fwd, 0, 0, minorWalkType=Action.SkeFastForwardMWT)
Action.openMouth()
else:
Global.lostBall = 8 - gBreakCounter + Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform()
Action.openMouth()
hTrack.panLow = True
gBreakCounter -= 1
gLastBreakFrame = Global.frame
elif Global.frame - gLastKickFrame > 1\
and Global.frame - gLastBreakFrame > 1:
#print "Kick!!!!"
selectKick(selType)
else:
resetPerform()
sGrab.resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def performTimeElapsedAction(selType):
global gLastActionFrame
global gForceTimeElapse
global gKickType
global gLastKickFrame
global gLastBreakFrame
global gLastLocaliseFrame
global gKickCounter
global gBreakCounter
global gLocaliseCounter
gLastActionFrame = Global.frame
gForceTimeElapse = True
#print "performTimeElapsed(", selType, ")"
#if selType == SEL_OFFENSIVE:
# import traceback
# traceback.print_stack()
if Action.shouldIContinueKick():
#print "Waiting for kick to activate ...", gKickCounter
Action.continueKick()
moveHeadForwardTight()
elif gKickCounter > 0:
#print "Kicking ...", gKickCounter
if gKickType == AVOID_OWN_GOAL:
if gKickCounter > 10:
perform(0, 0, Action.MAX_TURN)
elif gKickCounter > 8:
perform(0, 0, 0)
else:
perform(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeGrabDribbleMWT)
elif gKickType == GAP_KICK:
selfH = Global.selfLoc.getHeading()
if abs(gKickHead - selfH) > 10:
turnCCW = gKickHead - selfH
perform(0, 0, turnCCW)
# resetting until we are lined up to the gap.
gKickCounter = SOFT_TAP_DURATION
else:
gKickType = Action.SoftTapWT
Action.kick(gKickType)
perform(0, 0, 0)
moveHeadForwardTight()
elif gKickType == SIDE_STEP_LEFT_SHOOT\
or gKickType == SIDE_STEP_RIGHT_SHOOT\
or gKickType == FWD_STEP_SHOOT:
#print "gKickType = SIDESTEP_X_SHOOT"
# Do side step shoot...
if gKickCounter > FWD_STEP_SHOOT_DURATION:
left = Action.MAX_LEFT
if gKickType == SIDE_STEP_RIGHT_SHOOT:
left = -Action.MAX_LEFT
turnCCW = 0
# FIXME: Sidestep to Gap
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 30)
perform(5, left, turnCCW)
# Do forward step shoot...
elif gKickCounter > NU_FWD_DURATION:
fwd = Action.MAX_FORWARD
turnCCW = 0
if Global.vTGoal.isVisible():
turnCCW = Global.vTGoal.getHeading() / 2
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
turnCCW = gLastVisTGoal.getHeading() / 2
turnCCW = hMath.CLIP(turnCCW, 15)
perform(fwd,
0,
turnCCW,
minorWalkType=Action.SkeFastForwardMWT)
# Do kick!
else:
gKickType = gNextKickType
print "after sidestep setting kick type to", gNextKickType
#perform(0,0,0)
Action.kick(gKickType)
moveHeadForwardTight()
elif gKickType == RELEASE:
#print "gKickType = RELEASE", gKickCounter
Action.walk(0, 0, 0, minorWalkType=Action.SkeGrabDribbleHoldMWT)
if gKickCounter > RELEASE_DURATION - 15:
#print "stopping..."
Action.openMouth()
sGrab.moveHeadForward()
elif gKickCounter > 15:
#print "scanning"
hTrack.stationaryLocalise(speed=6)
Action.closeMouth()
else:
#print "regrabbing"
sGrab.moveHeadForward()
Action.openMouth()
# Reset grab timers
sGrab.gLastGrabTime = VisionLink.getCurrentTime()
sGrab.gGrabCount = 0
gForceTimeElapse = False
else:
#print "gKickCounter =", gKickCounter, "- continuing kick"
# Reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk
# straight.
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 160)
VisionLink.resetBall(x, y, 0, 0)
Action.continueKick()
Action.openMouth()
hTrack.panLow = True
gKickCounter -= 1
gLastKickFrame = Global.frame
if gKickType != RELEASE:
sFindBall.setForce(sFindBall.FORCE_FOLLOW)
elif gBreakCounter > 0:
#print "gBreakCounter > 0"
#if Global.penaltyShot or Global.lightingChallenge:
# # Stop and release
# gBreakCounter = 0
# gKickType = RELEASE
# gKickCounter = RELEASE_DURATION
# If we cannot see the ball, then reset gps ball coordinates.
# So when it goes back to find ball, it won't turn around and walk straight.
if not Global.vBall.isVisible():
x, y = hMath.getPointRelative(Global.selfLoc.getX(),
Global.selfLoc.getY(),
Global.selfLoc.getHeading(), 50)
VisionLink.resetBall(x, y, 0, 0)
if gBreakCounter > 10:
Action.setHeadParams(0, 0, 0, Action.HTAbs_h)
Action.walk(Action.MAX_FORWARD,
0,
0,
minorWalkType=Action.SkeFastForwardMWT)
elif gBreakCounter > 6:
Global.lostBall = Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(True)
rate = Action.SKE_FF_PG * 8.0 * 2.0 / 1000.0
fwd = Action.MAX_SKE_FF_FWD_STP * 0.7 / rate
Action.walk(fwd, 0, 0, minorWalkType=Action.SkeFastForwardMWT)
Action.openMouth()
else:
Global.lostBall = 8 - gBreakCounter + Constant.LOST_BALL_LAST_VISUAL
sFindBall.perform(doGetBehind=sFindBall.GET_BEHIND_PRIORITY)
Action.openMouth()
hTrack.panLow = True
gBreakCounter -= 1
gLastBreakFrame = Global.frame
elif Global.frame - gLastKickFrame > 1\
and Global.frame - gLastBreakFrame > 1:
#print "gKickCounter = 0, Selecting kick", selType
selectKick(selType)
else:
resetPerform()
sGrab.resetPerform()
return Constant.STATE_SUCCESS
return Constant.STATE_EXECUTING
def doKickWithGrab():
global gSelectedKick
if not sGrab.isGrabbed:
# If the ball is in the edge, do more get behind.
if hWhere.ballOnEdge():
r = sGrab.perform(sFindBall.GET_BEHIND_PRIORITY)
else:
r = sGrab.perform()
if r == Constant.STATE_FAILED:
hTrack.panLow = True
return r
elif r == Constant.STATE_EXECUTING:
return r
# Grab success
else:
# override kick selection
selectedKick = sSelKick.perform()
#print "kick reselected"
# If the selected kick is different, then do kick reset.
if gSelectedKick != None and selectedKick[0] != gSelectedKick[0]:
doKickReset()
gSelectedKick = selectedKick
# If kick selection returns paw kick, change it to
# grab dribble.
# Because we can't paw kick or dribble from grab.
kickType = gSelectedKick[0]
if kickType == sSelKick.KT_DRIBBLE\
or kickType == sSelKick.KT_PAW_KICK\
or kickType == sSelKick.KT_AVOID_OWN_GOAL:
gSelectedKick = (sSelKick.KT_GRAB_DRIBBLE, gSelectedKick[1],
gSelectedKick[2])
r = Constant.STATE_EXECUTING
if sGrab.isGrabbed:
if not sGrab.isBallUnderChin():
return Constant.STATE_FAILED
kickType, dkd, direction = gSelectedKick
# Find a global co-ordinate on the dkd
tx, ty = hMath.getPointRelative(Global.ballX, Global.ballY, dkd, 200)
# Setting isClockwise
isClockwise = None
if direction == Constant.dCLOCKWISE:
isClockwise = True
elif direction == Constant.dANTICLOCKWISE:
isClockwise = False
if kickType == sSelKick.KT_GRAB_TURN_KICK:
r = sGrabTurnKick.perform(False, Action.HeadTapWT, tx, ty)
#r = sGrabDribble.performToDKD(dkd, 0)
elif kickType == sSelKick.KT_GRAB_TURN_SHOOT:
r = sGrabTurnKick.perform()
elif kickType == sSelKick.KT_GRAB_DRIBBLE_GOAL:
r = sGrabDribble.performToTargetGoal()
elif kickType == sSelKick.KT_GRAB_DRIBBLE_STOP:
r = sGrabDribble.performToDKD(dkd,
minTimeToDribble=2500,
dontKick=True)
elif kickType == sSelKick.KT_GRAB_DRIBBLE:
r = sGrabDribble.performToDKD(dkd)
elif kickType == sSelKick.KT_TURN_KICK:
r = sTurnKick.perform(dkd, isClockwise=isClockwise)
elif kickType == sSelKick.KT_UPENN_LEFT:
#print "grab upeen left"
r = sUpennKick.perform(dkd, True)
elif kickType == sSelKick.KT_UPENN_RIGHT:
#print "grab upeen right"
r = sUpennKick.perform(dkd, False)
else:
print "rAttacker.py : No appropriate kick for grab chosen??? ", kickType
r = Constant.STATE_FAILED
return r
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])