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")