def performToDKD(dkd, minTimeToDribble=1500):
global gForceToTargetGoal
global gForceTimeElapse
Indicator.showFacePattern([1, 0, 0, 0, 1])
if not sGrab.isGrabbed:
resetPerform()
return Constant.STATE_FAILED
# If we are already doing edge behaviour, then continue.
if gEdgeCounter > 0 and performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# If we are already doing dodging behaviour, then continue.
if gDodgeCounter > 0 and performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
if gForceTimeElapse\
or hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE:
return performTimeElapsedAction(SEL_GPS)
if performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
turnCCW = hMath.normalizeAngle_180(dkd - Global.selfLoc.getHeading())
# If we are lined up to the dkd, do something smart.
if abs(turnCCW) <= 10:
# Dodge, if we need to.
if performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# If we exceed the minTimeToDribble, then release the ball.
if Global.frame - gLastTurnFrame >= TURN_DURATION\
and hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > minTimeToDribble:
return performTimeElapsedAction(SEL_GPS)
# If we are in outside third, use diagonal grab dribbling.
selfX = Global.selfLoc.getX()
if selfX < Constant.LEFT_GOAL_POST or selfX > Constant.RIGHT_GOAL_POST:
if (0 < dkd < 180):
left = hMath.CLIP(selfX - Constant.FIELD_WIDTH * 0.5,
Action.MAX_SKE_LEFT_STP * 0.5)
else:
left = hMath.CLIP(Constant.FIELD_WIDTH * 0.5 - selfX,
Action.MAX_SKE_LEFT_STP * 0.5)
else:
left = 0
return performDribble(Action.MAX_FORWARD, left, turnCCW)
def performDribble(fwd, left, turnCCW):
global gLastDiagFrame
global gLastTurnFrame, gLastTurnDir
if abs(turnCCW) <= 30:
if left == 0\
or hMath.getTimeElapsed(sGrab.gLastGrabTime,VisionLink.getCurrentTime()) > 2000\
or (left > 0 and VisionLink.getNoObstacleBetween(-30,-20,-30,80) > MIN_OBS_TO_DODGE)\
or (left < 0 and VisionLink.getNoObstacleBetween(30,-20,30,80) > MIN_OBS_TO_DODGE):
if Global.frame - gLastTurnFrame < 8:
r = perform(fwd,
0,
turnCCW,
minorWalkType=Action.SkeGrabDribbleMWT)
else:
r = perform(fwd, 0, turnCCW)
else:
gLastDiagFrame = Global.frame
if Global.frame - gLastTurnFrame < 8:
r = perform(fwd,
left,
turnCCW,
minorWalkType=Action.SkeGrabDribbleMWT)
else:
r = perform(fwd, left, turnCCW)
else:
r = perform(0, 0, turnCCW)
gLastTurnFrame = Global.frame
if turnCCW > 0:
gLastTurnDir = Constant.dANTICLOCKWISE
else:
gLastTurnDir = Constant.dCLOCKWISE
return r
def calWalk():
# 1. initialise
target = currentMode.target()
target.frameReset() # update visual info every frame
# 2. act
currentMode.look()
currentMode.walk()
# 2.1 special case
if currentMode == forwardOnly and forward > 0 and \
target.lostCount() > 4 * 30: # lost the target for more than 4 sec.
currentMode.changeTarget()
global stopLegsCount
if stopLegsCount > 0:
#Action.stopLegs()
#Action.finalValues[Action.WalkType] = Action.NormalWalkWT
stopLegsCount -= 1
if stopLegsCount == 0:
global lastForward
if not (lastForward * forward < 0): #if not diff. signs
currentMode.changeTarget()
lastForward = forward
currentMode.reset()
return
# 3. time
global isTiming, startTimeStamp
if isTiming:
flc = VisionLink.getSwitchSensorCount(Constant.SWITCH_FL_PALM)
frc = VisionLink.getSwitchSensorCount(Constant.SWITCH_FR_PALM)
# if we hit something, or the current mode tell us to stop.
if flc > 0 or frc > 0 or \
currentMode.shouldEndTiming():
endTimeStamp = VisionLink.getCurrentTime()
elapsed = hMath.getTimeElapsed(startTimeStamp, endTimeStamp)
if elapsed > 1300: # 1.3 sec is the minimal time b/t twode detection
isTiming = False
print "--------------------------------"
print currentMode
print "timestamp:", endTimeStamp
print "I'm close to", target
print "time elapsed: ", elapsed, " milli seconds"
print
sendWalkInfo(elapsed)
Action.forceStepComplete()
stopLegsCount = 1
else:
print "time elapsed is too short"
else: # if not isTiming:
if currentMode.shouldStartTiming():
isTiming = True
startTimeStamp = VisionLink.getCurrentTime()
# 4. indicate (note the order, and latter one can overwrite former)
Indicator.showDropFrameIndicator()
target.indicate()
indicateIsTiming()
def performToTargetGoal():
global gTurningDir
global gTurningCnt
global gLastVisTGoalFrame
global gLastVisTGoal
global gLastSelfLoc
global gTrustGpsGoal
global gTrustGpsGoalCounter
global gForceTimeElapse
Indicator.showFacePattern([0, 3, 3, 3, 0])
if not sGrab.isGrabbed:
resetPerform()
return Constant.STATE_FAILED
# If we are already doing edge behaviour, then continue.
if gEdgeCounter > 0 and performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# If we are already doing dodge behaviour, then continue.
if gDodgeCounter > 0 and performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
if gForceTimeElapse\
or (hMath.getTimeElapsed(sGrab.gLastGrabTime,VisionLink.getCurrentTime()) > MIN_TIME_TO_DRIBBLE
and Global.frame - gLastTurnFrame >= TURN_DURATION):
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 2500"
return performTimeElapsedAction(SEL_OFFENSIVE)
if hMath.getTimeElapsed(sGrab.gLastGrabTime,
VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE:
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 3000"
return performTimeElapsedAction(SEL_BREAK)
if Global.vTGoal.isVisible():
gLastVisTGoalFrame = Global.frame
gLastVisTGoal = Global.vTGoal.getCopy()
gLastSelfLoc = Global.selfLoc.getCopy()
gTurningDir = None
gTurningCnt = 0
minH, maxH = selectGap()
padding = abs(maxH - minH) / 5.0
# Our heading is within the gap for sure.
if minH < -padding and padding < maxH\
and Global.pWalkInfo.getCurrentMinorWalkType() != Action.GrabTurnOnlyMWT\
and Global.frame - gLastTurnFrame >= TURN_DURATION:
gForceTimeElapse = True
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 3000"
return performTimeElapsedAction(SEL_OFFENSIVE)
# Perform dodge, when there's something in front of you.
# May be dodge a goalie.
elif performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# Our heading is not within the gap and can detect the gap.
elif minH != 0 and maxH != 0:
turnCCW = (minH + maxH) / 2.0
# Our heading is not within the gap and can't detect the gap.
else:
turnCCW = Global.vTGoal.getHeading()
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
if performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
turnCCW = gLastVisTGoal.getHeading()
else:
if gTrustGpsGoal and performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
#print "turning",
h = hMath.getHeadingToFaceAt(Global.selfLoc,\
Constant.TARGET_GOAL_X,\
Constant.TARGET_GOAL_Y)
if abs(h) < 15:
#print ", turning with gps",
# If we see many obstacle in front of us, it may explain why we are not seeing any goals.
# Thus, do some dodging.
if gTrustGpsGoal\
and performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# Gps thinks, we are in front of the goal, but we can't see it.
# Thus, Gps is possibly wrong.
gTrustGpsGoalCounter += 1
if gTrustGpsGoalCounter > TRUST_GPS_GOAL_DURATION:
gTrustGpsGoal = False
# We still trust gps.
if gTrustGpsGoal:
turnCCW = h
#print ", trust gps goal",
# Otherwise, do something smart.
else:
# We tried both directions, we are completely lost...
# Maybe, do localise?
# At the moment, we just keep spinning...
if gTurningCnt >= 2:
if gTurningDir == Constant.dANTICLOCKWISE:
turnCCW = -Action.MAX_TURN
else:
turnCCW = Action.MAX_TURN
# Otherwise, turn both directions to find the target goal.
else:
if gTurningDir == None:
h1 = hMath.getHeadingToFaceAt(sGrab.gLastGrabSelfLoc,\
Constant.TARGET_GOAL_X,\
Constant.TARGET_GOAL_Y)
if h1 > 0:
gTurningDir = Constant.dANTICLOCKWISE
else:
gTurningDir = Constant.dCLOCKWISE
if gTurningDir == Constant.dANTICLOCKWISE\
and h > MAX_OVER_TURN:
gTurningDir = Constant.dCLOCKWISE
gTurningCnt += 1
elif gTurningDir == Constant.dCLOCKWISE\
and h < -MAX_OVER_TURN:
gTurningDir = Constant.dANTICLOCKWISE
gTurningCnt += 1
if gTurningDir == Constant.dANTICLOCKWISE:
turnCCW = Action.MAX_TURN
else:
turnCCW = -Action.MAX_TURN
# If we are in outside third, use diagonal grab dribbling.
selfX = Global.selfLoc.getX()
if selfX < Constant.LEFT_GOAL_POST or selfX > Constant.RIGHT_GOAL_POST:
left = hMath.CLIP(selfX - Constant.FIELD_WIDTH * 0.5,
Action.MAX_SKE_LEFT_STP * 0.5)
else:
left = 0
return performDribble(Action.MAX_FORWARD, left, turnCCW)
def performDribble(fwd, left, turnCCW):
global gLastActionFrame
global gLastStuckFrame, gLastStuckDir
global gLastDiagFrame
global gLastTurnFrame, gLastTurnDir
# Just in case
gLastActionFrame = Global.frame
if abs(turnCCW) <= 30:
gLastStuckFrame = 0
gLastStuckDir = 0
if left == 0\
or hMath.getTimeElapsed(sGrab.gLastGrabTime,VisionLink.getCurrentTime()) > 2000\
or (left > 0 and VisionLink.getNoObstacleBetween(-30,-20,-30,80) > MIN_OBS_TO_DODGE)\
or (left < 0 and VisionLink.getNoObstacleBetween(30,-20,30,80) > MIN_OBS_TO_DODGE):
if Global.frame - gLastTurnFrame < 8:
r = perform(fwd,
0,
turnCCW,
minorWalkType=Action.SkeGrabDribbleMWT)
else:
r = perform(fwd, 0, turnCCW)
else:
gLastDiagFrame = Global.frame
if Global.frame - gLastTurnFrame < 8:
r = perform(fwd,
left,
turnCCW,
minorWalkType=Action.SkeGrabDribbleMWT)
else:
r = perform(fwd, left, turnCCW)
else:
# Force high gain when turning only
Global.forceHighGain = True
# We are going to detect stuck here.
"""
if Global.frame - gLastTurnFrame < 5:
if gLastTurnDir == Constant.dANTICLOCKWISE:
rrr = VisionLink.getLastMaxStepPWM(Constant.ssRR_ROTATOR)
rla = VisionLink.getLastMaxStepPWM(Constant.ssRL_ABDUCTOR)
# check if you are stuck.
if rrr[1] > 650 or rla[1] > 300:
gLastStuckFrame = Global.frame
gLastStuckDir = gLastTurnDir
else:
rlr = VisionLink.getLastMaxStepPWM(Constant.ssRL_ROTATOR)
rra = VisionLink.getLastMaxStepPWM(Constant.ssRR_ABDUCTOR)
# check if you are stuck.
if rlr[1] > 675 or rra[1] > 300:
gLastStuckFrame = Global.frame
gLastStuckDir = gLastTurnDir
"""
gLastTurnFrame = Global.frame
if turnCCW > 0:
gLastTurnDir = Constant.dANTICLOCKWISE
else:
gLastTurnDir = Constant.dCLOCKWISE
sFindBall.setForce(sFindBall.FORCE_SPIN, gLastTurnDir)
r = perform(0, 0, turnCCW)
Global.forceHighGain = True
"""
if Global.frame - gLastStuckFrame < 90:
# We are turning to the direction which we got stuck, hence reverse the turnCCW.
if gLastStuckDir != gLastTurnDir:
turnCCW = hMath.getSign(1,turnCCW) * (abs(turnCCW) - 360)
# Turn more, because we are stuck.
turnCCW = turnCCW * 1.5
r = perform(5,0,turnCCW)
Indicator.showFacePattern([3,3,3,3,3])
else:
r = perform(0,0,turnCCW)
"""
return r
def performToDKD(dkd, minTimeToDribble=1500, dontKick=False):
global gLastActionFrame
global gForceToTargetGoal
global gForceTimeElapse
#print "performToDKD elapsed =",\
# hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime())
if not sGrab.isGrabbed:
print "Grab failed"
resetPerform()
return Constant.STATE_FAILED
Indicator.showFacePattern([1, 0, 0, 0, 1])
gLastActionFrame = Global.frame
# If we need to not kick at the end of this dribble, make sure here
if dontKick and hMath.getTimeElapsed(
sGrab.gLastGrabTime,
VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE:
#print "Time elapsed performing SEL_RELEASE"
return performTimeElapsedAction(SEL_RELEASE)
# If we can see the goal and we are in offensive half,
# then let performToTargetGoal take over.
# It may only take over if gForceTimeElapse is false.
if not dontKick\
and (gForceToTargetGoal
or (not gForceTimeElapse and Global.vTGoal.isVisible() and Global.selfLoc.getY() > Constant.FIELD_LENGTH * 0.6)
or hWhere.ballInTargetGoalBox()):
gForceToTargetGoal = True
return performToTargetGoal()
# If we are already doing edge behaviour, then continue.
if gEdgeCounter > 0 and performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# If we are already doing dodging behaviour, then continue.
if gDodgeCounter > 0 and performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# If the grab time has elapsed, then release
if gForceTimeElapse\
or hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE:
return performTimeElapsedAction(SEL_OFFENSIVE)
if performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
turnCCW = hMath.normalizeAngle_180(dkd - Global.selfLoc.getHeading())
# If we are lined up to the dkd, do something smart.
if abs(turnCCW) <= 5 and not dontKick:
# If we see our own goal and we are facing the right way (it's gotta be
# our gps is stuffed up!), then do avoid goal move.
if (not Global.lightingChallenge) and Global.vOGoal.isVisible():
return performTimeElapsedAction(SEL_DEFENSIVE)
# Dodge, if we need to.
if performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# If we exceed the minTimeToDribble, then release the ball.
if Global.frame - gLastTurnFrame >= TURN_DURATION\
and hMath.getTimeElapsed(sGrab.gLastGrabTime, VisionLink.getCurrentTime()) > minTimeToDribble:
# If we are in defensive area, we are lined up,
# then force time elapsed with defensive selection.
selfY = Global.selfLoc.getY()
if selfY < Constant.FIELD_LENGTH * 0.4:
return performTimeElapsedAction(SEL_DEFENSIVE)
# Otherwise use midfield selection.
else:
return performTimeElapsedAction(SEL_MIDFIELD)
# If we are in outside third, use diagonal grab dribbling.
selfX = Global.selfLoc.getX()
if selfX < Constant.LEFT_GOAL_POST or selfX > Constant.RIGHT_GOAL_POST:
left = hMath.CLIP(selfX - Constant.FIELD_WIDTH * 0.5,
Action.MAX_SKE_LEFT_STP * 0.5)
else:
left = 0
return performDribble(Action.MAX_FORWARD, left, turnCCW)
def performToTargetGoal():
global gLastActionFrame
global gTurningDir
global gTurningCnt
global gGapAimCount
global gLastVisTGoalFrame
global gLastVisTGoal
global gLastSelfLoc
global gTrustGpsGoal
global gTrustGpsGoalCounter
global gForceTimeElapse
global gLastSelectedGap
global gTurnCCWSpd
Indicator.showFacePattern([0, 3, 3, 3, 0])
if not sGrab.isGrabbed:
print "Grab failed"
resetPerform()
return Constant.STATE_FAILED
gLastActionFrame = Global.frame
#print "performToTargetGoal"
# If we are already doing edge behaviour, then continue.
if gEdgeCounter > 0 and performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# If we are already doing dodge behaviour, then continue.
if gDodgeCounter > 0 and performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
if gForceTimeElapse\
or (hMath.getTimeElapsed(sGrab.gLastGrabTime,VisionLink.getCurrentTime()) > MIN_TIME_TO_DRIBBLE
and Global.frame - gLastTurnFrame >= TURN_DURATION):
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 2500"
return performTimeElapsedAction(SEL_OFFENSIVE)
if hMath.getTimeElapsed(sGrab.gLastGrabTime,
VisionLink.getCurrentTime()) > MAX_TIME_TO_DRIBBLE:
#print "Camera Frame : ", Global.cameraFrame, " : over time!! 3000"
return performTimeElapsedAction(SEL_BREAK)
if performEdge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
left = None
if Global.vTGoal.isVisible():
gLastVisTGoalFrame = Global.frame
gLastVisTGoal = Global.vTGoal.getCopy()
gLastSelfLoc = Global.selfLoc.getCopy()
gTurningDir = None
gTurningCnt = 0
left = 0
# Choose gap
#minH, maxH = selectGap(gGapAimCount >= 2)
gLastSelectedGap = selectGap(gGapAimCount >= 2)
minH, maxH = gLastSelectedGap
padding = abs(maxH - minH) / 5.0
if minH < -padding and padding < maxH:
#VisionLink.sendYUVPlane()
gGapAimCount += 1
# Our heading is within the gap for sure.
if minH < -padding and padding < maxH\
and gGapAimCount >= MIN_GAP_AIM\
and hPWalk.isStepComplete():
gForceTimeElapse = True
return performTimeElapsedAction(SEL_OFFENSIVE)
# Perform dodge, when there's something in front of you.
# May be dodge a goalie.
elif performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
# Our heading is not within the gap and can detect the gap.
else:
turnCCW = (minH + maxH) / 2.0
if abs(turnCCW) < 20:
turnCCW = turnCCW * 0.8
gTurnCCWSpd = hMath.getSign(1, turnCCW) * 120
elif Global.frame - gLastVisTGoalFrame < TRUST_VIS_GOAL_DURATION:
if performDodge() == Constant.STATE_EXECUTING:
return Constant.STATE_EXECUTING
left = 0
turnCCW = 0
else:
turnCCW = turnToGPSGoal()
# If we are in outside third, use diagonal grab dribbling.
if left == None:
selfX = Global.selfLoc.getX()
if selfX < Constant.LEFT_GOAL_POST or selfX > Constant.RIGHT_GOAL_POST:
left = hMath.CLIP(selfX - Constant.FIELD_WIDTH * 0.5,
Action.MAX_SKE_LEFT_STP * 0.5)
else:
left = 0
return performDribble(Action.MAX_FORWARD, left, turnCCW)
def sendDebugInfo():
VisionLink.sendAtomicAction(*tuple(Action.finalValues))
VisionLink.sendWalkInfo(
hMath.getTimeElapsed(startTimeStamp, VisionLink.getCurrentTime()))
VisionLink.sendObjects()
def gtkGeneratorFunc():
global frameSinceLastGrabKick, aimCounter
# Ok, we've grabbed the ball, now spin towards the target
aimCounter = 0
#print "*** GTK"
# if we have enough time to do turn/side kick
while hMath.getTimeElapsed(
sGrab.gLastGrabTime,
VisionLink.getCurrentTime()) < Constant.GRAB_TIMEOUT:
# print "Vframe", Global.frame,
isLinedUp = False
# now decide what to do
#print "lining up,",
if isAimToGoal:
isLinedUp = performAimToGoal()
else:
isLinedUp = performNotAimToGoal()
# If visual aim fails (we turn too far) stop now
if isLinedUp == Constant.STATE_FAILED:
break
# Im lined up, so kick!
# DON'T wait for step complete
if isLinedUp == Constant.STATE_SUCCESS:
#print "kicking"
Action.forceStepComplete()
Action.kick(gKickType)
while Action.shouldIContinueKick():
Action.continueKick()
yield True
for _ in range(sGrabDribble.NU_FWD_DURATION):
yield True
frameSinceLastGrabKick = Global.frame
sFindBall.setHint(200, 90)
sFindBall.perform()
Action.closeMouth()
resetPerform()
return
# else line up still executing
if SEND_DEBUG_INFO_TO_ROBOLINK:
sendDebugInfo()
yield True
#print "timeout or turned too far...",
### The line-up has failed
minH, maxH = selectGap(True)
if minH < 0 < maxH:
#print "shooting"
Action.forceStepComplete()
Action.kick(gKickType)
while Action.shouldIContinueKick():
Action.continueKick()
yield True
for _ in sGrabDribble.NU_FWD_DURATION:
yield True
frameSinceLastGrabKick = Global.frame
sFindBall.setHint(200, 90)
sFindBall.perform()
Action.closeMouth()
resetPerform()
return
sFindBall.setHint(20, 90) # directly in front
sFindBall.perform()
Action.closeMouth()
resetPerform()
sGrab.resetPerform()
return