def Unpenalized():
global phase
global playertype
memProxy.insertListData([['dntBallDist', '', 0], ['dntPhase', 'ReturnField', 0]])
# if case this nao is a keeper, convert to player until state changes to initial/ready/set, where it is set to keeper again
playertype = 0
visThread.startScan()
# so no matter what, it's always good to walk forward (2 meters, to the center of the field)
if not(mot.isWalking()):
mot.postWalkTo(2, 0, 0)
phase = 'ReturnField'
def UnpenalizedKeep():
global phase
global ball_loc
global position
if color == 0:
desiredPosition = [0, 2, 0]
else:
desiredPosition = [6, 2, math.pi]
goal = vis.scanCircleGoal()
localize( goal, 'Goal' , desiredPosition )
print 'At position', position
x1,y1,t1 = position
x2,y2,t2 = desiredPosition
# walk straight towards a goal if you see one
mot.postWalkTo( x2-x1,y2-y1, minimizedAngle( (t2-t1)-math.pi ) )
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'
