From 5c4cfa084239d10ffb9f2e9d850789df7ee05705 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Sat, 30 Jun 2018 12:45:05 +0200 Subject: [PATCH] Improving sideways approach --- pykick/__main__.py | 55 ++++++++++++++++++++++++++++------------------ pykick/striker.py | 47 ++++++++++----------------------------- 2 files changed, 45 insertions(+), 57 deletions(-) diff --git a/pykick/__main__.py b/pykick/__main__.py index 8b02faa..2bb559c 100644 --- a/pykick/__main__.py +++ b/pykick/__main__.py @@ -70,13 +70,19 @@ if __name__ == '__main__': striker.mover.stand_up() state = 'init' - init_soll = 0.0 - align_start = 0.15 - curve_start = -0.1 - curve_stop = 0.1 - soll = init_soll + # init_soll = 0.0 + # align_start = 0.15 + # curve_start = -0.1 + # curve_stop = 0.1 + # soll = init_soll striker.speak("Initialized") + approach_steps = 0 + loop_start = time() while True: + loop_end = time() + print('Loop time:', loop_end - loop_start) + print('\n\n') + # meassure time for debbuging loop_start = time() print('State:', state) @@ -85,7 +91,9 @@ if __name__ == '__main__': striker.mover.set_head_angles(0, 0) striker.speak("Start the Ball tracking") striker.ball_tracking(tol=0.05) - striker.speak("I have found the Ball, starting with. Goal search") + striker.speak( + "I have found the Ball, starting with. Goal search" + ) goal_center = striker.goal_search() if goal_center <0: striker.speak("I have found the. goal on the right") @@ -93,7 +101,7 @@ if __name__ == '__main__': else: striker.speak("I have found the. goal on the left") approach = -1 - + striker.mover.set_head_angles(0, 0) #approach = 1 if goal_center < 0 else -1 #approach = 1 @@ -105,22 +113,29 @@ if __name__ == '__main__': print('Soll angle') striker.ball_tracking(tol=0.05) # break - state = 'align' + if approach_steps < 2: + state = 'ball_approach' + else: + state = 'goal_align' elif state == 'ball_approach': - # bil = striker.get_ball_angles_from_camera( - # striker.lower_camera - # ) # Ball in lower + bil = striker.get_ball_angles_from_camera( + striker.lower_camera + ) # Ball in lower - # print('Ball in lower', bil) - #striker.speak("I have found the ball. Starting ball approach") try: d = striker.distance_to_ball() except ValueError: - state = 'tracking' + if bil is not None: + state = 'goal_align' + striker.speak('Ball is close. Align to. Goal') + else: + state = 'tracking' continue + print('Distance to ball', d) - striker.speak("The distance to the ball is approximately "+str(round(d,2))+" Meters") + striker.speak("The distance to the ball is approximately " + + str(round(d,2)) + " Meters") angle = striker.walking_direction(approach, d) d_run = d * cos(angle) print('Approach angle', angle) @@ -128,14 +143,16 @@ if __name__ == '__main__': striker.mover.move_to(0, 0, angle) striker.mover.wait() striker.run_to_ball(d_run) - striker.speak("I think I have reached the ball. I will start rotating") + striker.speak("I think I have reached the ball. " + + "I will start rotating") + approach_steps += 1 striker.mover.move_to(0, 0, -pi/2 * approach) + striker.mover.wait() state = 'tracking' elif state == 'goal_align': try: if striker.align_to_goal(): - striker.speak('Why am I in align. This makes no sense') state = "align" except ValueError: state = 'tracking' @@ -176,10 +193,6 @@ if __name__ == '__main__': ##striker.speak("Nice kick. Let's do a dance") #striker.mover.dance() break - - loop_end = time() - print('Loop time:', loop_end - loop_start) - print('\n\n') finally: striker.close() striker.mover.rest() diff --git a/pykick/striker.py b/pykick/striker.py index 1faa204..b42ac27 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -219,8 +219,7 @@ class Striker(object): self.mover.move_to(d, 0, 0) self.mover.wait() - def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, fancy=False, - m_delta=0.2): + def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0): """Align robot to the ball. If head is not centered at the ball (within tolerance), then @@ -245,43 +244,19 @@ class Striker(object): print('Head yaw', head_yaw, end=' ') d_yaw = head_yaw - soll print('Head d_yaw', d_yaw) - print('Rotating', self.rotating, end=' ') - print('Rotation direction', self.rot_dir, end=' ') print('Allowed tolerance', tol) - if not fancy: - if abs(d_yaw) > tol: - self.mover.stop_moving() - print('Going to rotate by', d_yaw) - self.speak('Going to rotate') - self.mover.set_head_angles(soll, head_pitch, 0.3) - self.mover.move_to(0, 0, d_yaw) - self.mover.wait() - return False - else: - print('Ball locked') - return True - + if abs(d_yaw) > tol: + self.mover.stop_moving() + print('Going to rotate by', d_yaw) + self.speak('Going to rotate') + self.mover.set_head_angles(soll, head_pitch, 0.3) + self.mover.move_to(0, 0, d_yaw) + self.mover.wait() + return False else: - if not self.rotating: - if abs(d_yaw) > tol: - self.mover.stop_moving() - self.rotating = True - self.rot_dir = -1 if d_yaw > 0 else 1 - print('Going to rotate') - self.speak("Going to rotate") - self.mover.move_toward(0, 0, -self.rot_dir) - # sleep(1.5) - return False - else: - print('Success') - # self.speak('Ball locked') - return True - else: - if d_yaw * self.rot_dir > -tol - m_delta: - self.rotating = False - self.mover.stop_moving() - return False + print('Ball locked') + return True def align_to_ball(self): ball_angles = self.get_ball_angles_from_camera(self.lower_camera,