def set_head_position(head_position):
    if head_position[0] > pi/3:
        head_position[0] = pi/3
    elif head_position[0] < -pi/3:
        head_position[0] = -pi/3
        
    if head_position[1] > pi/8:
        head_position[1] = pi/8
    elif head_position[1] < -0.8:
        head_position[1] = -0.8
        
    robotbody.set_head_position_list(head_position)
 def entry(self):
     robotbody.set_head_hardness(1.95)
     self.current_head_position=robotbody.get_head_position()
     
     self.wanted_head_position=[-pi/2,0]
     robotbody.set_head_position_list(self.wanted_head_position)
        
     self.ending=False
     
     self.numbers_of_observation=0
     self.angles=[]
     
     print ("searching for goal")
 def set_next_head_position(self):
     if len(self.angles)==0:
         self.wanted_head_position[0]+=pi/15
     else:
         self.wanted_head_position[0]-=pi/15
     robotbody.set_head_position_list(self.wanted_head_position)
 def focus_one(self):
     self.wanted_head_position=[self.angles[0]]
     robotbody.set_head_position_list(self.wanted_head_position)
 def focus_middle(self):
     self.wanted_head_position=[(self.angles[0][0]+self.angles[1][0])/2,0]
     robotbody.set_head_position_list(self.wanted_head_position)    
 def searching_for_next(self):
     self.wanted_head_position=[pi/2,0]
     robotbody.set_head_position_list(self.wanted_head_position)
 def update_head_position(self):
     self.wanted_head_position[0]+=pi/15
     robotbody.set_head_position_list(self.wanted_head_position)
 def entry(self):
     self.wanted_head_position=[-pi/2,0]
     robotbody.set_head_position_list(self.wanted_head_position)
     print("Test if standing in the goal")