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 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'
