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 Kick():
global phase
memProxy.insertData('dntPhase', 'Kick')
visThread.stopScan()
ledProxy.fadeRGB('LeftFaceLeds',0x00000000, 0) # no goal yet, left led turns black
ledProxy.fadeRGB('RightFaceLeds', 0x00ff0000, 0) # no ball anymore, right led turns red
# scan for a goal
goal = vis.scanCircleGoal()
motProxy.post.angleInterpolation(['HeadPitch', 'HeadYaw'], [[0.5], [0]], [[0.15], [0.15]], True)
visThread.startScan()
# Case 0 : Ball stolen/lost.
# Check if the ball is still there, wait until ball is found or 1 second has passed
now = time.time()
while time.time() - now < 1 and not(visThread.findBall()):
pass
ball = visThread.findBall()
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:
mot.walkTo(0,0.04 + ball[1],0)
mot.rKickAngled(0)
phase = 'BallNotFound'
else:
print "Kick phase:", goal
ledProxy.fadeRGB('RightFaceLeds', 0x0000ff00, 0) # no ball anymore, right led turns red
# else a goal is found, together with it's color
(color, kickangles ) = goal
if len(kickangles) == 2:
(first, second) = kickangles
kickangle = (3 * first + second) / 4.0 # kick slightly more towards left pole
else:
kickangle = kickangles[0]
if color == 'Blue':
goalColor = 0
ledProxy.fadeRGB('LeftFaceLeds',0x000000ff, 0) # blue goal
else:
goalColor = 1
ledProxy.fadeRGB('LeftFaceLeds',0x00ff3000, 0) # yellow goal , anyone got a better value?
# Cases 1-3, if you see your own goal, kick to the other side
if goalColor == teamColor:
# Case 1, goal is left, kick to the right.
if kickangle >= 0.7:
kickangle = -1
# Case 2, goal is right, kick to the left.
if kickangle <= -0.7:
kickangle = 1
else:
# Case 3, goal is straight forward, HAK
mot.walkTo(0,0.04 + ball[1],0)
mot.normalPose()
time.sleep(0.5)
ledProxy.fadeRGB('LeftFaceLeds',0x00000000, 0) # led turns black
mot.hakje(kickangle * -0.1)
phase = 'BallNotFound'
else:
# Case 4, other player's goal is found.
# Kick towards it.
if kickangle > 1.1:
kickangle = 1.1
if kickangle < -1.1:
kickangle = -1.1
# conversion to real kickangle
convert = 1.1 # (60 degrees rad)
# kick either left or right
if kickangle <= 0:
mot.walkTo(0, -0.04 + ball[1], 0)
mot.lKickAngled(kickangle/ -convert)
elif kickangle > 0:
mot.walkTo(0, 0.04 + ball[1], 0)
mot.rKickAngled(kickangle/ convert)
ledProxy.fadeRGB('LeftFaceLeds',0x00000000, 0)
phase = 'BallNotFound'
