def entry(self):
#Resets all the variables:
#The wanted head position
self.wanted_head_position=[-pi/3,-0.733]
#The angles it has found to the goal
self.angles=[]
#The fail timer
self.fail_timer=time.time()+7
#A couple of boolean variables to help the robot
#through the different states of finding the two goal post
self.ending=False
self.failing=False
#Makes sure that the head hardness is right and
#updates the position of the head
robotbody.set_head_hardness(0.95)
set_head_position(self.wanted_head_position)
#Sets the walk speed to the actually zero
walk.set_velocity(0, 0, 0)
print ("searching for goal")
def update_speed(self):
if self.distance_to_ball> pi*3:
self.current_forward_speed = self.forward_speed
elif self.distance_to_ball< pi and self.distance_to_ball>0:
self.current_forward_speed = self.backward_speed
else:
self.current_forward_speed = 0
#Updates the circling direction if it finds that it's looking to the right of the right post
#or if it looks to the left of the left post
if self.has_new_goal_observation:
if self.goal_type==1 and self.goal_angle<0 and\
self.current_circling_speed==-self.circling_speed:
self.current_circling_speed=self.circling_speed
self.changed_speed=True
self.start_time=self.current_time+2
elif self.goal_type==2 and self.goal_angle>0 and\
self.current_circling_speed==self.circling_speed:
self.current_circling_speed=-self.circling_speed
self.changed_speed=True
self.start_time=self.current_time+2
#Sets the new walk instructions
walk.set_velocity(self.current_forward_speed, self.current_circling_speed, ball_angle()[0])
def update(self):
self.current_time=time.time()
if has_fallen():
return "fallen"
#if self.current_time>=self.lost_ball_timer:
#print ("lost ball")
#return "lost ball"
if self.timer and self.current_time>self.timer:
return "check again"
if not self.timer:
if vision.has_new_ball_observation():
self.first_ball_angle=ball_angle()[0]
self.last_ball_angle=self.first_ball_angle
self.timer=time.time()+fabs(self.first_ball_angle)*10
self.lost_ball_timer=self.current_time+10
else:
if like(self.first_ball_angle,self.goal_angle,pi/9):
print("lined up")
return "lined up"
else:
set_head_position(ball_angle())
if self.first_ball_angle<0:
walk.set_velocity(0, -0.4, self.last_ball_angle)
else:
walk.set_velocity(0, 0.4, self.last_ball_angle)
def entry(self):
print("facing the ball")
self.last_ball_angle=ball_angle()[0]
if self.last_ball_angle<0:
walk.set_velocity(0, -0.4, self.last_ball_angle)
else:
walk.set_velocity(0, 0.4, self.last_ball_angle)
def update(self):
if has_fallen():
return "fallen"
if self.timer<time.time():
if vision.has_new_line_observation() and self.get_pitch_angle()<=pi/3:
walk.set_velocity(0, 0, 0)
return "standing on line"
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 update_speed(self):
self.distance_to_ball=distance_to_ball()
if self.distance_to_ball> pi*3:
self.forward_velocity = self.const_forward_velocity;
elif self.distance_to_ball< pi and self.distance_to_ball>0:
self.forward_velocity = -self.const_forward_velocity
else:
self.forward_velocity = 0
walk.set_velocity(self.forward_velocity, self.circling_velocity, ball_angle()[0])
def start_turning(self):
#Updates the head position
self.last_ball_angle=ball_angle()
set_head_position(self.last_ball_angle)
#Updates the walking
if self.first_ball_angle<0:
walk.set_velocity(0, -0.2, self.last_ball_angle[0])
else:
walk.set_velocity(0, 0.2, self.last_ball_angle[0])
def entry(self):
self.wanted_rotation=pi
walk.set_velocity(0, 0, 0)
self.rotation_progress = 0
self.const_forward_velocity=0.02
self.forward_velocity=0
self.wanted_head_position=[0,pi/4.5]
set_head_position(self.wanted_head_position)
self.start_time = time.time()
self.time = 17
def update_speed(self):
if self.distance_to_ball> pi*3:
self.forward_velocity = self.const_forward_velocity;
elif self.distance_to_ball< pi and self.distance_to_ball>0:
self.forward_velocity = -self.const_forward_velocity
else:
self.forward_velocity = 0
if like(goal_angle()[0],0,self.slow_down_time):
self.circling_velocity=self.slow_circling_speed
self.timer=self.current_time+20
walk.set_velocity(self.forward_velocity, self.circling_velocity, ball_angle()[0])
def update(self):
if has_fallen():
return "fallen"
self.set_new_head_position()
if distance_to_ball() > 1.5:
self.forward_velocity = self.const_forward_velocity;
else:
self.forward_velocity = 0
walk.set_velocity(self.forward_velocity, 0.4, self.wanted_head_position[0]*1.2)
self.rotation_progress -= self.wanted_head_position[0]/12
if time.time() > self.start_time + self.time:
print("aiming away from our goal")
return "done"
def move_or_kick(self):
#Gets the angles for to the ball
self.ball_angle=ball_angle()[0]
#If they are close to the middle it will move sideways
if like(self.ball_angle,0):
walk.set_velocity(0, 0.4, 0)
#Else it will stop, look up, start the timer since the last kick
#and kick with the foot that has the ball in front of it.
else:
walk.set_velocity(0, 0, 0)
set_head_position([0,0])
self.time_since_kick = time.time()
if self.ball_angle>0:
kick.forward_left()
else:
kick.forward_right()
def entry(self):
#Reseting the forward velocity from the last time the state was used
self.forward_velocity=0
#Updating the head position so that DARwIn will look down at the ball
self.wanted_head_position=[0,pi/8]
set_head_position(self.wanted_head_position)
#Starting the sideways walk
walk.set_velocity(self.forward_velocity, self.circling_velocity, 0)
#Starting the turning timer
self.start_time = time.time()
self.time = 10
#starting the lost ball timer
self.lost_ball_timer=time.time()+7
print ("Turning away from our goal")
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 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):
if has_fallen():
return "fallen"
if vision.has_new_goal_observation():
if like(goal_angle()[0],0):
print("found goal")
return "done"
self.update_head_position()
if distance_to_ball() > 1:
self.forward_velocity = self.const_forward_velocity;
else:
self.forward_velocity = 0
walk.set_velocity(self.forward_velocity, 0.4*self.direction, ball_angle()[0]*1.2)
if time.time() > self.start_time + self.time:
print("aiming away from our goal")
return "done"
def walk_sideways(self):
if self.direction=="left" and self.traveled_time>-self.max_traveled_time:
walk.set_velocity(0,0.4,-pi)
elif self.direction=="right" and self.traveled_time<self.max_traveled_time:
walk.set_velocity(0,0.4,pi)
else:
walk.set_velocity(0, 0, 0)
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 move_or_kick(self):
#Gets the angles for to the ball
self.ball_angle=ball_angle()[0]
#If the ball is to the right of the left foot
if self.ball_angle<0.1:
walk.set_velocity(0, -0.01, 0)
#If the ball is to the left of the left foot
elif self.ball_angle>0.47:
walk.set_velocity(0, 0.01, 0)
#Else it will stop, look up, start the timer since the last kick
#and kick with the foot that has the ball in front of it.
else:
walk.set_velocity(0, 0, 0)
set_head_position([0,0])
self.time_since_kick = time.time()
kick.forward_left()
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 exit(self):
walk.set_velocity(-0.001, 0, 0)
def exit(self):
walk.set_velocity(0, 0, 0)
def entry(self):
self.timer+=time.time()
walk.set_velocity(0.1, 0, 0)
print("walking towards the line")
def exit(self):
#Reset the walking speed for the robot
walk.set_velocity(0, 0, 0)