def Finished():
visThread.stopScan() # stop looking for the ball
mHandler.killWalk() # stop walking
mot.stance() # sit down
mot.setHead(0, 0) # set head straight
time.sleep(1) # wait until seated etc
mot.killKnees() # stiffness off to prevent heating
if not gsc.getSecondaryState(): # if not penalty shootout
gsc.close()
audProxy.setOutputVolume(85) # volume on
def Finished():
visThread.stopScan() # stop looking for the ball
ledProxy.off('AllLeds') # turn off ledProxy
mot.killWalk() # stop walking
mot.stance() # sit down
mot.setHead(0,0) # set head straight
time.sleep(1) # wait until seated etc
mot.killKnees() # stiffness off to prevent heating
if not sc.getSecondaryState():
sc.close()
audProxy.setOutputVolume(85) # volume on
def Playing():
"""Playing state: play game according to phase transitions
ledProxy: Chest Green
"""
global phase
global firstCall
if firstCall["Set"]:
mot.stiff() # stiffness on (if it wasnt already)
mot.setHead(0, 0) # head in normal position
# if a regular player
if playerType == 0:
phase = "BallNotFound"
mot.normalPose(True) # normalPose, forcing call
# if a keeper
else:
phase = "BallNotFoundKeep"
mot.keepNormalPose()
firstCall["Set"] = False
if firstCall["Playing"]:
mHandler.killWalk()
visThread.startScan()
print "In playing state"
firstCall["Initial"] = True
firstCall["Ready"] = True
firstCall["Set"] = True
firstCall["Playing"] = False
firstCall["Penalized"] = True
if mot.standUp():
print "Fallen"
try:
if memProxy.getData("dntAction"):
phase = memProxy.getData("dntAction")
print "coach says: " + phase
else:
print "coach says: nothing"
except:
pass
# Execute the phase as specified by phase variable
phases.get(phase)()
def Initial():
global phase # to change, call global, else not possible
global ball_loc
global firstCall
if firstCall['Initial']:
print 'In initial state'
visThread.stopScan() # do not find the ball when in Initial
print 'TeamColor: ' , teamColor # print the teamcolor as a check
# Empty variables
ball_loc = dict()
# Team color must be displayed on left foot (Reference to rules)
ledProxy.off('AllLeds')
if (teamColor == 0):
ledProxy.fadeRGB('LeftFootLeds', 0x000000ff, 0)
else:
ledProxy.fadeRGB('LeftFootLeds', 0x00ff0000, 0)
mot.stiff() # stiffness on (if it wasnt already)
mot.setHead(0,0) # head in normal position
# if a regular player
if playerType == 0:
phase = 'BallNotFound'
mot.normalPose(True) # normalPose, forcing call
# if a keeper
else:
phase = 'BallNotFoundKeep'
mot.keepNormalPose()
firstCall['Initial'] = False
firstCall['Ready'] = True
firstCall['Set'] = True
firstCall['Playing'] = True
firstCall['Penalized'] = True
def Kick():
global phase
global goalPosKnown
mot.setFME(False)
memProxy.insertData("dntPhase", "Kick")
visThread.stopScan()
# scan for a goal
goal = None
if goalPosKnown[0]:
goal = goalPosKnown[1]
else:
goal = vis.scanCircleGoal()
if goal:
goalPosKnown = (True, goal)
print "Kick phase: ", goal
mot.setHead(0, 0.45)
visThread.startScan()
ball = vis.scanCircle(visThread)
# Case 0 : Ball stolen/lost.
# Check if the ball is still there, wait until ball is found has passed
# Something of a mean, ...
if not ball:
print "Ball gone"
phase = "BallNotFound"
elif ball[0] > 0.25 or ball[1] > 0.1 or ball[1] < -0.1:
print "Ball too far"
phase = "BallFound"
elif ball and not goal:
mHandler.kick(0)
phase = "BallNotFound"
else:
# else a goal is found, together with it's color
(color, kickangles) = goal
if type(kickangles[0]) == tuple:
(first, second) = kickangles
kickangle = (3 * first[0] + second[0]) / 4.0 # kick slightly more towards left pole
else:
kickangle = kickangles[0]
if color == "Blue":
goalColor = 0
else:
goalColor = 1
# use kalman filter to position ball in 10 measurements
c = 0
while c < 10:
c += 1
ball = visThread.findBall()
mHandler.setBallLoc(ball)
ball = mHandler.getKalmanBallPos()
# Cases 1-3, if you see your own goal, kick to the other side OR if the keeper saw the ball
print "keep saw ball: " + str(memProxy.getData("dntKeepSawBall"))
if memProxy.getData("dntKeepSawBall"):
# Case 1, goal is left, kick to the right.
if kickangle >= 0.7:
mHandler.kick(-1.1)
# Case 2, goal is right, kick to the left.
if kickangle <= -0.7:
mHandler.kick(1.1)
else:
# Case 3, goal is straight forward, HAK, but only in the first 2 minutes of each half.
if gsc.getSecondsRemaining() > 8:
mHandler.kick(1.1)
else:
mHandler.kick(1.1)
else:
# Case 4, other player's goal is found.
# Kick towards it.
# general kick interface distributes kick
if abs(kickangle < 0.6):
mHandler.walkTo(0, math.copysign(0.03, kickangle), 0)
while c < 10:
c += 1
ball = visThread.findBall()
mHandler.setBallLoc(ball)
ball = mHandler.getKalmanBallPos()
print "Goalcolor", goalColor, "KickAngle", kickangle
print "TeamColor", teamColor, "Locatie", ball
mHandler.kick(kickangle)
phase = "BallNotFound"
ledProxy.fadeRGB("LeftFaceLeds", 0x00000000, 0) # no goal yet, left led turns black
