def has_fallen(): if like(imu.get_angle()[1],-pi/2,pi/4) or like(imu.get_angle()[1],pi/2,pi/4)\ or like(imu.get_angle()[0],-pi/2,pi/4) or like(imu.get_angle()[0],pi/2,pi/4): return True else: return False
def entry(self): #Resets the variables since the last use of the state: #starting angle self.start_angle = imu.get_angle()[2] #Timers self.start_time=time.time() self.timer=self.start_time+self.time_between #Which direction it should start turning its head self.left_or_right=self.direction #Makes sure that the head hardness is correct and #stores the current head position robotbody.set_head_hardness(0.95) self.wanted_head_position=robotbody.get_head_position() #Initiates the turn in the right direction if self.direction=="left": walk.turn_left(0.2) else: walk.turn_right(0.2) #Sets a fancy blue eye colour robotbody.set_eyes_led(0, 0, 31) print("Tracking ball")
def set_left_arm_position (x,y,z,relative="body"): if relative=="body": robotbody.set_left_arm_position(x,y,z) return (x,y,z) elif relative=="ground": x-=imu.get_angle()[1] robotbody.set_left_arm_position(x,y,z) return (x,y,z)
def entry(self): print("Tracking ball") robotbody.set_head_hardness(0.95) self.wanted_head_position=robotbody.get_head_position() self.start_angle = imu.get_angle()[2] self.start_time=time.time() self.timer=self.start_time+self.time_between self.left_or_right="right" walk.turn_left(0.2) set_head_position(self.wanted_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.001): 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): self.update_head_position() if vision.has_new_ball_observation(): walk.turn_left(0) robotbody.set_eyes_led(0, 31, 0) print("found ball") return "done" if imu.get_angle()[2] > self.start_angle + self.angle: walk.turn_left(0) robotbody.set_eyes_led(31,0,0) print ("can't find the ball") return "out of sight"
def update(self): #Tests if it has found a ball observation if vision.has_new_ball_observation(): walk.turn_left(0) robotbody.set_eyes_led(0, 31, 0) print("found ball") return "done" #Tests if it's time to stop turning and accept a failure if imu.get_angle()[2] > self.start_angle + self.angle: walk.turn_left(0) robotbody.set_eyes_led(31,0,0) print ("can't find the ball") return "out of sight" #Updates the head position self.update_head_position()
def update (self): if like(imu.get_angle()[1],0.001): return ("done")
def update(self): if has_fallen(): return "fallen" if imu.get_angle()[2] > self.start_angle + self.angle: return "done"
def entry(self): print("Entry gyro turn") self.start_angle = imu.get_angle()[2] self.number_of_turns += 1 walk.turn_left(0.4)
def get_right_arm_position (relative="body"): if relative=="body": return robotbody.get_right_arm_position() elif relative=="ground": return (robotbody.get_right_arm_position()[0]+imu.get_angle()[1],robotbody.get_right_arm_position()[1],robotbody.get_right_arm_position()[2])