From a01189e016d5985a992a0d396f325ccc39ea647f Mon Sep 17 00:00:00 2001
From: Pavel Lutskov <pavel.lutskov@gmail.com>
Date: Sun, 1 Jul 2018 17:54:59 +0200
Subject: [PATCH] Small but important tweaks

---
 pykick/__main__.py | 13 ++++++++-----
 pykick/striker.py  | 40 +++++++++++++++++++++-------------------
 2 files changed, 29 insertions(+), 24 deletions(-)

diff --git a/pykick/__main__.py b/pykick/__main__.py
index 2452e87..98bcd06 100644
--- a/pykick/__main__.py
+++ b/pykick/__main__.py
@@ -48,12 +48,12 @@ if __name__ == '__main__':
                 _, _, gcc = striker.goal_search()
                 print('Goal center', gcc, 'Ball dist', bdist)
 
-                if abs(gcc) < 0.2 or bdist <= 0.35:
+                if abs(gcc) < 0.3 or bdist <= 0.40:
                     print('Straight approach')
                     state = 'straight_approach'
                     approach = 0
 
-                elif 0.35 < bdist < 0.50:
+                elif 0.40 < bdist < 0.60:
                     print('Rdist is hypo')
                     state = 'rdist_is_hypo'
                     approach = 1 if gcc < 0 else - 1
@@ -91,8 +91,10 @@ if __name__ == '__main__':
 
                 striker.mover.move_to(0, 0, angle)
                 striker.mover.wait()
-                if rdist > 1.5:
-                    striker.run_to_ball(1.5)
+                if rdist > 2:
+                    striker.run_to_ball(rdist / 2)
+                    striker.mover.wait()
+                    striker.mover.move_to(0, 0, -angle)
                     striker.mover.wait()
                 else:
                     striker.run_to_ball(rdist)
@@ -179,7 +181,7 @@ if __name__ == '__main__':
 # [    |                                yes        v              ]
 # [    |                  Ball distance <--  Goal angle > thr     ]
 # [    |                 /   |         \           |              ]
-# [    |        > 50 cm /    |(35,50)   \ < 35cm   | no           ]
+# [    |        > 60 cm /    |(40,60)   \ < 40cm   | no           ]
 # [    |               /     v           \         v              ]
 # [    +- Distance is <   Walk is hypo    \  Straight approach    ]
 # [    |    hypo              |            >  until goal align    ]
@@ -190,3 +192,4 @@ if __name__ == '__main__':
 # [      |(  | (   |(       Ball           Goal align             ]
 # [      | \ |  \_ | \ <-- align <-- (if lost ball run backwards) ]
 # [_______________________________________________________________]
+#
diff --git a/pykick/striker.py b/pykick/striker.py
index 1a3f2d1..2571f2c 100644
--- a/pykick/striker.py
+++ b/pykick/striker.py
@@ -15,12 +15,13 @@ from naoqi import ALProxy
 class Striker(object):
 
     def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv,
-                 ball_min_radius):
+                 ball_min_radius, do_capture=False):
 
         # Maintenance
         self.run_id = strftime('%Y%m%d%H%M%S')
         self.is_over = False
         self.last_goal = 'right'
+        self.doing_caputre = do_capture
 
         # Motion
         self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port)
@@ -34,17 +35,18 @@ class Striker(object):
         )
 
         # POV
-        self.upper_pov = NaoImageReader(
-            nao_ip, port=nao_port, res=1, fps=10, cam_id=0,
-            video_file='./cam0_' + self.run_id + '.avi'
-        )
+        if do_capture:
+            self.upper_pov = NaoImageReader(
+                nao_ip, port=nao_port, res=1, fps=10, cam_id=0,
+                video_file='./cam0_' + self.run_id + '.avi'
+            )
 
-        self.lower_pov = NaoImageReader(
-            nao_ip, port=nao_port, res=1, fps=10, cam_id=1,
-            video_file='./cam1_' + self.run_id + '.avi'
-        )
-        self.pov_thread = Thread(target=self._pov)
-        self.pov_thread.start()
+            self.lower_pov = NaoImageReader(
+                nao_ip, port=nao_port, res=1, fps=10, cam_id=1,
+                video_file='./cam1_' + self.run_id + '.avi'
+            )
+            self.pov_thread = Thread(target=self._pov)
+            self.pov_thread.start()
 
         # Recognition
         self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]),
@@ -70,13 +72,13 @@ class Striker(object):
 
         if self.tts_thread.isAlive():
             self.tts_thread.join()
-        if self.pov_thread.isAlive():
+        if self.doing_caputre and self.pov_thread.isAlive():
             self.pov_thread.join()
+            self.upper_pov.close()
+            self.lower_pov.close()
 
         self.upper_camera.close()
         self.lower_camera.close()
-        self.upper_pov.close()
-        self.lower_pov.close()
         self.mover.stop_moving()
 
     def _speaker(self):
@@ -118,11 +120,11 @@ class Striker(object):
 
         # the robot starts to move arround his z-Axis in the direction where his
         # head is aligned when the head yaw angle has reached his maximum
-        if yaw > 0.8:
+        if yaw > pi/3:
             self.mover.set_head_angles(-pi / 8, pitch, 0.5)
             sleep(0.5)
-        elif yaw < -0.8:
-            self.mover.move_to_fast(0, 0, -pi / 4)
+        elif yaw < -pi/3:
+            self.mover.move_to(0, 0, -pi / 4)
             self.mover.wait()
         # rotate head to the left, if head yaw angle is equally zero or larger
         # rotate head to the right, if head yaw angle is smaller than zero
@@ -290,7 +292,7 @@ class Striker(object):
         if ball_angles is None:
             raise ValueError('No ball')
         x, y = ball_angles
-        goal_x, goal_y = 0.088, 0.4
+        goal_x, goal_y = 0.092, 0.38
         dx, dy = goal_x - x, goal_y - y
 
         dx = -dx * 0.2 if abs(dx) > 0.03 else 0
@@ -325,7 +327,7 @@ class Striker(object):
         gcl, gcr, gcc = goal
         print('Goal:', gcl, gcr, gcc)
 
-        if gcl > 0 > gcr:
+        if gcl > 0.15 > -0.22 > gcr:
             return True
 
         if y > 0.38: