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'