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