def update(self): if has_fallen(): return "fallen" self.current_head_position=robotbody.get_head_position() if like(self.wanted_head_position,self.current_head_position): if self.ending: return "focus one" elif len(self.angles)==0: if vision.has_new_goal_observation(): self.get_goal_observation() self.searching_for_next() elif like(self.current_head_position[0],pi/2): return "fail" else: self.set_next_head_position() elif len(self.angles)==1: if vision.has_new_goal_observation(): self.get_goal_observation() self.focus_middle() else: self.set_next_head_position() else: return "focus middle"
def update_head_position(self): head_position=robotbody.get_head_position() if like(head_position,self.wanted_head_position): if like(head_position,[pi/2,0]): self.wanted_head_position=[-pi/2,pi/3.5] elif like(head_position[0],pi/2): self.wanted_head_position=[-pi/2,0] else: self.wanted_head_position[0]=head_position[0]+pi/15 robotbody.set_head_position(self.wanted_head_position[0],self.wanted_head_position[1])
def update_walk_direction(self): current_head_position = robotbody.get_head_position() if not like(current_head_position[0],0,pi/18): walk.set_velocity(self.speed,0.025,current_head_position[0]) else: walk.set_velocity(self.speed, 0, 0)
def get_goal_observation(self): temp_angles=goal_angle() if len(self.angles)==0 or not like(temp_angles,self.angles[0]): self.angles.append(temp_angles) else: self.focus_one() self.ending=True
def update(self): if has_fallen(): return "fallen" if vision.has_new_goal_observation(): if like(goal_angle()[0],0): return "done" else: return "standing in front of the goal" self.current_head_position=robotbody.get_head_position() if like(self.current_head_position,pi/2): return "can't find the goal" if like(self.wanted_head_position,self.current_head_position): self.update_head_position()
def update (self): #If the robot is sitting and the timer hasn't started #it's time for the robot to stand up and start the timer if self.sitting and not self.start_time: self.start_time=time.time() motion.stand_still() #Tests if the robot is sitting or standing by checking the #angle of the IMU. In not it starts the motion to enter a sitting position if like(imu.get_angle()[1],0.01): self.sitting=True #Test if the robot is finished with the motions if self.start_time and time.time()>self.start_time+1: return "done"
def update(self): if has_fallen(): return "fallen" if like(robotbody.get_head_position()[0],0): return "done"
def update (self): if like(imu.get_angle()[1],0.001): return ("done")
def looking_at_goal(self): print (str(self.goal_type)+" : "+str(self.goal_angle)) if self.goal_type==3: if like(self.goal_angle,0,self.to_big_angle): self.which_goal_type="middle" return True elif not self.adjusting and self.goal_angle>0: self.adjustment_timer=self.goal_angle*15/pi self.current_circling_speed=self.circling_speed self.adjusting=True return False elif self.goal_angle<0: self.adjustment_timer=-self.goal_angle*15/pi self.current_circling_speed=-self.circling_speed self.adjusting=True return False elif self.goal_type==2: if self.goal_angle<-self.allowed_angle_diff and self.goal_angle>-pi/6: self.which_goal_type="right" return True elif not self.adjusting and self.goal_angle>-self.allowed_angle_diff: self.adjustment_timer=self.current_time+abs(self.allowed_angle_diff+self.goal_angle)*15/pi self.current_circling_speed=self.circling_speed self.adjusting=True return False elif not self.adjusting and self.goal_angle<-pi/6: self.adjustment_timer=self.current_time+(-self.goal_angle-self.allowed_angle_diff)*15/pi self.current_circling_speed=-self.circling_speed self.adjusting=True return False elif self.goal_type==1: if self.goal_angle>self.allowed_angle_diff and self.goal_angle<pi/6: self.which_goal_type="left" return True elif not self.adjusting and self.goal_angle<self.allowed_angle_diff: self.adjustment_timer=self.current_time+(self.allowed_angle_diff-self.goal_angle)*15/pi self.current_circling_speed=-self.circling_speed self.adjusting=True return False elif not self.adjusting and self.goal_angle>pi/6: self.adjustment_timer=self.current_time+(self.goal_angle-self.allowed_angle_diff)*15/pi self.current_circling_speed=self.circling_speed self.adjusting=True return False elif self.goal_type==0: if like(self.goal_angle,0,pi/36): self.counter+=1 if not self.adjusting and self.counter>=10: if like(self.goal_angle,0,self.allowed_angle_diff): self.which_goal_type="unknown" return True elif not self.adjusting and self.goal_angle>0: self.adjustment_timer=self.goal_angle*15/pi self.current_circling_speed=self.circling_speed self.adjusting=True return False elif self.goal_angle<0: self.adjustment_timer=-self.goal_angle*15/pi self.current_circling_speed=-self.circling_speed self.adjusting=True return False else: return False else: return False