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 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(self):
if has_fallen():
return "fallen"
if not vision.has_new_ball_observation():
if self.last_observation_of_ball+5<time.time() or self.last_distance>=tan(5*pi/12):
walk.set_velocity(self.speed, 0, 0)
robotbody.set_eyes_led(31, 0, 0)
print("lost ball")
return "no ball"
else:
self.current_head_position = robotbody.get_head_position()
if like(self.current_head_position,self.wanted_head_position):
self.update_head_position()
self.last_observation_of_ball=time.time()
self.last_distance=distance_to_ball()
if self.last_distance>0 and self.last_distance<self.distance:
print ("standing in front of ball")
return "done"
if not like(self.current_head_position[0],0,pi/18):
walk.set_velocity(self.speed, 0.4, self.current_head_position[0])
else:
walk.set_velocity(self.speed, 0, 0)
def update(self):
#Tests if the robot has fallen
if has_fallen():
return "fallen"
#Stores the if it has a new goal observation for further tests
self.has_new_goal_observation=vision.has_new_goal_observation()
#Stores the current head position for further tests
self.current_head_position=robotbody.get_head_position()[0]
self.current_time=time.time()
#If the timer has run out or if the robot has failed to find a goal post
#it leaves the state depending with a few commands.
if self.fail_timer<=self.current_time or self.failing:
#If it's not failing it knows that it have found at least one goal post
#Then it will update the fail timer so that it has time enough to move
#its head towards that direction
if not self.failing:
self.failing=True
self.fail_timer=self.current_time+1
#If the timer has run out it's time to leave the state
if self.fail_timer<=self.current_time:
if len(self.angles)==0:
robotbody.set_eyes_led(31, 0, 0)
print("fail")
return "fail"
elif len(self.angles)==1:
robotbody.set_eyes_led(0, 31, 0)
return "focus one"
else:
self.focus_middle()
robotbody.set_eyes_led(0, 31, 0)
print("focus middle")
return "focus middle"
#If the timer hasn't run out it should focus on whatever it has found
else:
self.focus_on_results()
else:
#Tests if it is time to end with only one goal post found
if self.ending and like(self.current_head_position,self.angles[0][0]):
robotbody.set_eyes_led(0, 31, 0)
return "focus one"
#Tests if it has found two goal posts
if len(self.angles)==2 and \
like(self.current_head_position,(self.angles[0][0]+self.angles[1][0])/2):
robotbody.set_eyes_led(0, 31, 0)
print("focus middle")
return "focus middle"
#Updates the head position, variables, depending on how many goal post it has found
if not self.ending and like(self.current_head_position,self.wanted_head_position[0],pi/45):
self.update_values()
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.2,current_head_position[0])
else:
walk.set_velocity(self.speed, 0, 0)
def entry(self):
robotbody.set_head_hardness(1.95)
self.wanted_head_position=robotbody.get_head_position()
self.last_distance=0
self.last_observation=0
self.start_travel=[]
self.direction="stand still"
def entry(self):
print("following the ball")
robotbody.set_head_hardness(0.95)
self.last_observation_of_ball=-1
if self.look_down:
self.wanted_head_position=[0,pi/4.5]
else:
self.wanted_head_position=robotbody.get_head_position()
set_head_position(self.wanted_head_position)
def set_new_head_position(self):
if like(self.wanted_head_position,robotbody.get_head_position()):
current_ball_angle=ball_angle()
if current_ball_angle[1]>pi/4.5:
self.wanted_head_position[1]=pi/4.5
else:
self.wanted_head_position[1]=current_ball_angle[1]
self.wanted_head_position[0]=current_ball_angle[0]
set_head_position(self.wanted_head_position)
def left_or_right(self):
head_position=robotbody.get_head_position()
if head_position[0]<-pi/18:
self.direction="left"
self.start_travel=[time.time(),"-"]
elif head_position[0]>pi/18:
self.start_travel=[time.time(),"+"]
self.direction="right"
else:
self.direction="stand still"
self.start_travel=[]
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 entry(self):
print("lining up...")
robotbody.set_head_hardness(0.95)
self.goal_angle=robotbody.get_head_position()[0]
self.wanted_head_position=[self.goal_angle,pi/4.5]
set_head_position(self.wanted_head_position)
self.timer=None
self.lost_ball_timer=time.time()+10
self.first_ball_angle=None
self.last_ball_angle=None
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 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 entry(self):
#Starts the timer from when the ball was last seen
self.lost_ball_timer=time.time()+5
#Sets the wanted head position, either to the current or
#to one looking down at the ball
if self.look_down:
self.wanted_head_position=[0,pi/8]
else:
self.wanted_head_position=robotbody.get_head_position()
#Makes sure that the head hardness is correct and updates the head position
robotbody.set_head_hardness(0.95)
set_head_position(self.wanted_head_position)
print("following the ball")
def update(self):
self.current_head_position=robotbody.get_head_position()[0]
if has_fallen():
return "fallen"
if self.opponents_goal():
print("fire at opponents goal")
return "fire"
else:
print ("turning towards opponents goal")
return "turn"
self.update_head_position()
if like(self.current_head_position,pi/3):
return "fire"
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 has_fallen():
return "fallen"
if not self.start_time:
if like(self.wanted_head_position,robotbody.get_head_position()):
self.ball_angle=ball_angle()[0]
if like(self.ball_angle,0):
walk.set_velocity(0, 0.4, 0)
else:
walk.set_velocity(0, 0, 0)
set_head_position([0,0])
self.start_time = time.time()
if self.ball_angle>0:
kick.forward_left()
else:
kick.forward_right()
if self.start_time and time.time()>self.time+self.start_time:
return "done"
def entry(self):
#Resets all the variables for the last use of the state
#The movement timer
self.timer=None
#The last and first angle to the ball
self.first_ball_angle=None
self.last_ball_angle=None
#The lost ball timer
self.lost_ball_timer=time.time()+7
#Makes sure that the head hardness is right and sets the
#wanted head position to the same as last by looking down at the ball
robotbody.set_head_hardness(0.95)
self.goal_angle=robotbody.get_head_position()[0]
self.wanted_head_position=[self.goal_angle,pi/8]
set_head_position(self.wanted_head_position)
#Makes the robot stand still since it shouldn't move during while looking
walk.set_velocity(0,0,0)
print("lining up...")
def entry(self):
robotbody.set_head_hardness(1.95)
current_head_position=robotbody.get_head_position()
self.wanted_head_position=[current_head_position[0],current_head_position[1]]
def entry(self):
self.head_position=robotbody.get_head_position()
walk.set_velocity(0, 0.4, self.head_position[0])
robotbody.set_head_position(0, pi/3.2)
print("turning towards the goal")
def update(self):
if has_fallen():
return "fallen"
if like(robotbody.get_head_position()[0],0):
return "done"
