From e11f268508c57673cd64d404e87afab06b3b882c Mon Sep 17 00:00:00 2001
From: Jonas <ge69jum@mytum.de>
Date: Mon, 18 Jun 2018 15:01:08 +0200
Subject: [PATCH] improved approach

---
 pykick/movements.py | 15 ++++++++-------
 pykick/striker.py   | 15 +++++++++------
 2 files changed, 17 insertions(+), 13 deletions(-)

diff --git a/pykick/movements.py b/pykick/movements.py
index 40b070f..b7eb2c4 100644
--- a/pykick/movements.py
+++ b/pykick/movements.py
@@ -11,18 +11,19 @@ class NaoMover(object):
 
         [[(0, 1, 'ShoulderRoll', -70, 0.025)], 1],
 
-        [[(0, 1, 'AnkleRoll', -10, 0.05),
-          (1, 1, 'AnkleRoll', -10, 0.05)],
+        [[(0, 1, 'AnkleRoll', -9, 0.05),
+          (1, 1, 'AnkleRoll', -9, 0.05)],
          2],
 
         [[(1, 0, 'KneePitch', 90, 0.05),
           (1, 0, 'AnklePitch', -40, 0.05)],
-         3,],
+         2,],
 
         [[(1, 0, 'HipPitch', -45, 0.08),
           (1, 0, 'KneePitch', 10, 0.20),
-          (1, 0, 'AnklePitch', 30, 0.16)],
-         4]
+          (1, 0, 'AnklePitch', 20, 0.16)],
+         1],
+        [[(1, 0, 'HipPitch', -55, 0.08)], 1]
     ]
 
     def __init__(self, nao_ip, nao_port=9559):
@@ -44,8 +45,8 @@ class NaoMover(object):
         self.set_arm_stiffness(0.8)
         self.set_hip_stiffness(0.8)
         self.set_knee_stiffness(0.8)
-        self.set_ankle_stiffness(0.8)
-        multiplier = 4
+        self.set_ankle_stiffness(1)
+        multiplier = 5
         if foot == 'L':
             sides = ['R', 'L']
         elif foot == 'R':
diff --git a/pykick/striker.py b/pykick/striker.py
index 2ced96d..9ce77bf 100644
--- a/pykick/striker.py
+++ b/pykick/striker.py
@@ -103,15 +103,17 @@ class Striker(object):
         if ball_angles is None:
             raise ValueError('No ball')
         x, y = ball_angles
-        goal_x, goal_y = 0.115, 0.25
+        goal_x, goal_y = 0.115, 0.32
         dx, dy = goal_x - x, goal_y - y
         if abs(dx) < 0.05 and abs(dy) < 0.05:
             print(x, y)
             return True
-        self.mover.move_to(dy * 0.5, 0, 0)
-        self.mover.wait()
-        self.mover.move_to(0, -dx * 0.5, 0)
-        self.mover.wait()
+        if abs(dy) > 0.05:
+            self.mover.move_to(dy * 0.5, 0, 0)
+            self.mover.wait()
+        if abs(dx) > 0.05:
+            self.mover.move_to(0, -dx * 0.5, 0)
+            self.mover.wait()
         return False
 
     def align_to_goal(self):
@@ -202,7 +204,7 @@ if __name__ == '__main__':
 
                 print(ball_in_lower)
                 if (ball_in_lower is not None
-                    and ball_in_lower[1] > 0.25):
+                    and ball_in_lower[1] > 0.28):
 
                     print('Ball is in lower camera, go to align')
                     striker.mover.stop_moving()
@@ -216,6 +218,7 @@ if __name__ == '__main__':
                 sleep(0.5)
                 try:
                     success = striker.align_to_ball()
+                    sleep(0.3)
                     if success:
                         state = 'kick'
                 except ValueError: