From 67a8cccd51b25f1bd10706969d4b4fb9d47131c2 Mon Sep 17 00:00:00 2001
From: Pavel Lutskov <pavel.lutskov@gmail.com>
Date: Sat, 23 Jun 2018 20:50:28 +0200
Subject: [PATCH] More work on goal-ball alignment

---
 pykick/finders.py        |  3 ++-
 pykick/imagereaders.py   |  1 +
 pykick/move_robot.py     | 23 ++++++++++++++---------
 pykick/nao_defaults.json |  6 +++---
 pykick/striker.py        | 21 +++++++++++++--------
 5 files changed, 33 insertions(+), 21 deletions(-)

diff --git a/pykick/finders.py b/pykick/finders.py
index d99fe0f..e10d8df 100644
--- a/pykick/finders.py
+++ b/pykick/finders.py
@@ -39,7 +39,6 @@ class FieldFinder(object):
 
     def mask_it(self, frame, field, inverse=False):
         if field is not None:
-            print(frame.shape)
             mask = np.zeros(frame.shape[:2], dtype=np.uint8)
             cv2.drawContours(mask, (field,), -1, 255, -1)
             if inverse:
@@ -123,6 +122,8 @@ class GoalFinder(object):
 
     def goal_center(self, contour):
         l, r = self.left_right_post(contour)
+        print('Left goal post:', l,
+              'Right goal post:', r)
         return (l + r) / 2
 
     def draw(self, frame, goal):
diff --git a/pykick/imagereaders.py b/pykick/imagereaders.py
index fcc83cd..9d01880 100644
--- a/pykick/imagereaders.py
+++ b/pykick/imagereaders.py
@@ -32,6 +32,7 @@ class NaoImageReader(object):
         )
 
     def to_relative(self, x, y):
+        print('Camera resolution:', self.res)
         return x / self.res[1], y / self.res[0]
 
     def get_frame(self):
diff --git a/pykick/move_robot.py b/pykick/move_robot.py
index fbe695d..84f5fa2 100644
--- a/pykick/move_robot.py
+++ b/pykick/move_robot.py
@@ -7,13 +7,18 @@ if __name__ == "__main__":
     mover = NaoMover(cfg['ip'], cfg['port'])
     mover.stand_up()
     while True:
-        axis = int(raw_input('Axis: '))
         amount = float(raw_input('How much: '))
-        if axis == 0:
-            mover.move_to(amount, 0, 0)
-        elif axis == 1:
-            mover.move_to(0, amount, 0)
-        elif axis == 2:
-            mover.move_to(0, 0, amount)
-        else:
-            print('Axis out of range (0, 1, 2)')
+        mover.move_to(0, 0, 0.5 * amount)
+        mover.wait()
+        mover.move_to(0, -0.3 * amount, 0)
+        mover.wait()
+        mover.move_to(-0.1 * abs(amount), 0, 0)
+        # amount = float(raw_input('How much: '))
+        # if axis == 0:
+            # mover.move_to(amount, 0, 0)
+        # elif axis == 1:
+            # mover.move_to(0, amount, 0)
+        # elif axis == 2:
+            # mover.move_to(0, 0, amount)
+        # else:
+            # print('Axis out of range (0, 1, 2)')
diff --git a/pykick/nao_defaults.json b/pykick/nao_defaults.json
index 0b03ce2..cd70528 100644
--- a/pykick/nao_defaults.json
+++ b/pykick/nao_defaults.json
@@ -11,7 +11,6 @@
             255
         ]
     ], 
-    "cam": 1, 
     "goal": [
         [
             0, 
@@ -24,6 +23,7 @@
             255
         ]
     ], 
+    "fps": 30, 
     "res": 2, 
     "ball_min_radius": 0.01, 
     "field": [
@@ -38,7 +38,7 @@
             255
         ]
     ], 
-    "fps": 30, 
+    "cam": 1, 
     "ip": "192.168.0.11", 
     "port": 9559
-}
+}
\ No newline at end of file
diff --git a/pykick/striker.py b/pykick/striker.py
index 4b2d929..49728d8 100644
--- a/pykick/striker.py
+++ b/pykick/striker.py
@@ -212,12 +212,14 @@ class Striker(object):
 
         print(x, y)
         self.turn_to_ball(x, y, tol=0.15)
-
-        goal_center_x = self.get_goal_center_angle_from_camera(
-            self.upper_camera
-        )
+        goal_center_x = None
+        for i in range(3):
+            if goal_center_x is None:
+                goal_center_x = self.get_goal_center_angle_from_camera(
+                    self.upper_camera
+                )
         print('Goal center:', goal_center_x)
-        if goal_center_x is not None and abs(goal_center_x) < 0.01:
+        if goal_center_x is not None and abs(goal_center_x) < 0.1:
             print('Goal ball aligned!')
             raise SystemExit
 
@@ -232,7 +234,9 @@ class Striker(object):
             # return False
 
         self.mover.move_to(0, 0.2, 0)
+        print('Moving sideways')
         self.mover.wait()
+        print('Finished moving')
         return False
 
     def close(self):
@@ -362,7 +366,7 @@ if __name__ == '__main__':
     # (see diagram above)
     else:
         try:  # Hit Ctrl-C to stop, cleanup and exit
-            state = 'goal_align'
+            state = 'tracking'
             # t = None
             while True:
                 # meassure time for debbuging
@@ -381,10 +385,10 @@ if __name__ == '__main__':
                         striker.lower_camera
                     )
                     print(ball_in_lower)
-                    if (ball_in_lower is not None and ball_in_lower[1] > 0.10):
+                    if (ball_in_lower is not None and ball_in_lower[1] > 0):
                         print('Ball is close enough, stop approach')
                         striker.mover.stop_moving()
-                        striker.say('Align to goal')
+                        striker.speak('Align to goal')
                         state = 'goal_align'
                     else:
                         print('Continue running')
@@ -425,5 +429,6 @@ if __name__ == '__main__':
 
                 loop_end = time()
                 print('Loop time:', loop_end - loop_start)
+                print('\n\n')
         finally:
             striker.close()