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
