def setTurnKickParams(forward=0, left=0, turnccw=0):
Action.openMouth()
sGrab.moveHeadForward()
Action.finalValues[Action.Tilty] = -55
#Action.finalValues[Action.Panx] = -gTurnDir*20
Action.walk(forward,
left,
turnccw,
walkType=Action.NormalWalkWT,
minorWalkType=Action.TurnKickMWT)
def grabSideWalk(direction):
if direction > 0:
left = SIDE_STEP_MAX
else:
left = -SIDE_STEP_MAX
headColor(Indicator.RGB_BLUE)
Action.walk(0,
left,
0,
walkType=Action.NormalWalkWT,
minorWalkType=Action.GrabSideOnlyMWT)
sGrab.moveHeadForward()
Action.openMouth()
def DecideNextAction():
global grabCounter
grabCounter -= 1
#testKick()
#return
Action.openMouth()
sGrab.moveHeadForward()
Action.walk(0, 0, 0)
obsFront = VisionLink.getNoObstacleInBox(-25, 100, 25, 20,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
obsLeft = VisionLink.getNoObstacleInBox(-80, 80, -40, -80,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
obsRight = VisionLink.getNoObstacleInBox(40, 80, 80, -80,
Constant.MIN_VIS_OBSTACLE_BOX,
Constant.OBS_USE_LOCAL)
print "Obstacle Front : ", obsFront, " Left : ", obsLeft, " Right: ", obsRight
return
#print "Camera Frame : ", Global.cameraFrame
Indicator.setDefault()
if not sGrab.isGrabbed or performToTargetGoal(
) != Constant.STATE_EXECUTING:
if grabCounter == 60:
hTrack.panLow = True
resetPerform()
sGrab.resetPerform()
if grabCounter > 0:
sFindBall.perform()
else:
sGrab.perform()
grabCounter -= 1
else:
grabCounter = 60
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 GrabGenerator():
global counter, grabbing_time_when_stop, moveTo, isCorrectstandingPosition
grab_counter = 0
sensor_counter = 0
kickType = None
# grabbing
local_isGrabed = False
if not local_isGrabed:
sGrab.resetPerform()
while sGrab.perform() == Constant.STATE_EXECUTING:
if not isCorrectstandingPosition and Global.ballD <= 30: #sGrab.REALLY_SLOW_DIST :
isCorrectstandingPosition = True
print "distance ok to stop"
return
yield True
while local_isGrabed:
if grabbing_time_when_stop > MAX_GRABBING_TIME_WHEN_STOP:
grabbing_time_when_stop = 0
local_isGrabed = True
sGrab.moveHeadForward()
Action.openMouth()
grabbing_time_when_stop = grabbing_time_when_stop + 1
yield True
while not kickType:
sGrab.moveHeadForward()
Action.openMouth()
#posx,posy = Global.selfLoc.getPos()
#print "float:",posx, posy
#posx = int(posx)
#posy = int(posy)
#print "int:",posx, posy
if moveTo == None:
print "WARNING moveTo == None"
print "This is broken"
#sGrabDribble.GrabSideWalk(moveTo)
#kickType == Action.DiveKickWT
grab_counter += 1
if grab_counter % 15 != 0:
if VisionLink.getAnySensor(Constant.ssCHEST_INFRARED) > 138000:
sensor_counter += 1
else:
if sensor_counter < 3:
print "Oops, fail grabbing the ball.."
reset()
return
else:
sensor_counter = 0
# Check if we've grabbed for too long
if grab_counter > GRAB_FRAME_MAX:
kickType = Action.DiveKickWT
#headColor((True, False, False))
#Action.standStill()
#reset()
#return # Raise a StopIteration exception
yield True
# Start kick
# if Debug.TurnAndSideMoveDebug :
print "kicking"
if kickType == Action.DiveKickWT:
kickCount = 15 #30
else:
kickCount = 22
# Do kick
for _ in range(kickCount):
Action.kick(kickType)
Action.openMouth()
yield True
isCorrectstandingPosition = False
Global.lostBall = 0
reset()