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'