def InGoalArea():
global phase
mot.normalPose()
ball = visThread.findBall()
if ball:
(x,y) = ball
print x,y
if x < 0.17 and -0.06 < y < -0.02:
print 'Kick'
# BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE CORRECTION
mot.killWalk()
mot.rKickAngled(0)
phase = 'ReturnToGoal'
else:
# hacked influencing of perception, causing walking forward to have priority
theta = math.atan(y/x) / 2.5
x = 3 * (x-0.17)
if -0.1 < y < 0.1 :
y = 0.5 * y
else:
y = 2.0 * y
mot.SWTV(x , y, theta, 1.0)
else:
print 'No Ball'
mot.killWalk()
phase = 'ReturnToGoal'
def Penalized():
global phase
global firstCall
if firstCall['Penalized']:
print 'In penalized state'
visThread.stopScan() # stop looking for the ball
mot.killWalk() # stop walking
mot.stiff() # stiffness on (just in case?)
mot.keepNormalPose()
ledProxy.fadeRGB('ChestLeds',0x00ff0000, 0) # ledProxy on chest: red
if playerType == 0: # if a player, goto unpenalized phase
phase = 'Unpenalized'
else: # if a keeper, go to ballnotfoundkeep if penalty shootout
if sc.getSecondaryState():
phase = 'BallNotFoundKeep'
else: # else, become a player
phase = 'Unpenalized'
firstCall['Initial'] = True
firstCall['Ready'] = True
firstCall['Set'] = True
firstCall['Playing'] = True
firstCall['Penalized'] = False
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 Ready():
global firstCall
# Reinitialize phases, stand ready for match
if firstCall['Ready']:
# stop ballfinding, there is no ball anyway
visThread.stopScan()
print 'In ready state'
mot.killWalk()
ledProxy.fadeRGB('ChestLeds',0x000000ff, 0) # set ChestLeds
firstCall['Initial'] = True
firstCall['Ready'] = False
firstCall['Set'] = True
firstCall['Playing'] = True
firstCall['Penalized'] = True
def BallNotFound():
global ball_loc
global phase
global control
# if this is the first time the ball is not founnd
if firstCall['BallNotFound']:
memProxy.insertData('dntPhase', 'BallNotFound')
memProxy.insertData('dntBallDist', 0)
ledProxy.fadeRGB('RightFaceLeds',0x00ff0000, 0) # no ball, led turns red
mot.killWalk()
firstCall['BallNotFound'] = False
control = [0,0,0]
# try to find a ball
if vis.scanCircle(visThread, 0.2):
phase = 'BallFound'
firstCall['BallNotFound'] = True
else:
# if no ball is found circle slowly while searching for it
mot.postWalkTo(0, 0, 1.3)
def BallFound():
global phase
global control
seen = False
# FIND A BALL #
ball = visThread.findBall()
if ball:
seen = True
ball = kalmanFilterBall.iterate(ball, control)
(x,y) = ball
memProxy.insertData('dntPhase', 'BallFound')
memProxy.insertData('dntBallDist', math.sqrt(x**2 + y**2))
if firstCall['BallFound']:
ledProxy.fadeRGB('RightFaceLeds', 0x0000ff00, 0)
firstCall['BallFound'] = False
# initialize mean mu for kalman filter
if seen:
kalmanFilterBall.setFirstCall(True, ball)
if seen and x < 0.17 and -0.02 < y < 0.02:
print 'Kick'
# BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE CORRECTION
mot.killWalk()
control = [0,0,0]
phase = 'Kick'
else:
if seen:
print 'Seen ball', x,y
# hacked influencing of perception, causing walking forward to have priority
theta = math.atan(y/x) / 2.5
x = 3 * (x-0.17)
if -0.1 < y < 0.1 :
y = 0.5 * y
else:
y = 2.0 * y
else:
print 'Not seen ball',x,y
# find the 'ideal' headposition based on the kalman filter position
# (yaw, pitch) = vis.calcHeadAngle( (x,y), (320,480), True)
# mot.changeHead( yaw * 0.5, pitch * 0.5 )
# hacked influencing, turn towards last found position first
theta = math.atan(y/x) / 2.0
x = (x/3.0 -0.2)
y = 0
mot.SWTV(x , y, theta, 1.0)
# maxXSpeed 8 cm / s for nao
vX = max(-1, min(1,x)) * 0.08
# maxYSppeed is the same
vY = max(-1, min(1, y)) * 0.08
# maxTSpeed 2pi / 8 s = 1/4 pi / s
vT = max(-1, min(1, theta))* 0.25 * math.pi
control = [vX,vY,vT]
# If covariance becomes too large, when?
if kalmanFilterBall.Sigma[0][0] > 4.0:
print kalmanFilterBall.Sigma
print 'Perhaps the ball is lost, turning ', y,'to find it'
mot.postWalkTo(0,0, math.atan(y / x))
# TODO somehow influence kalman mu based on turn
phase = 'BallNotFound'
def Standby():
global phase
ttsProxy.say('Waiting')
mot.killWalk()
time.sleep(1)
phase = 'BallFound'
