From 7e554bbcba93f107001550a7ce2e7c1275f6397a Mon Sep 17 00:00:00 2001
From: Jonas <ge69jum@mytum.de>
Date: Sat, 30 Jun 2018 10:25:09 +0200
Subject: [PATCH] added dance after kick

---
 pykick/movements.py | 60 +++++++++++++++++++++++++++++++++++++++++++++
 pykick/striker.py   |  5 ++--
 2 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/pykick/movements.py b/pykick/movements.py
index ad2cf7d..f0a993d 100644
--- a/pykick/movements.py
+++ b/pykick/movements.py
@@ -190,6 +190,66 @@ class NaoMover(object):
             self.ready_to_move = True
         self.mp.post.moveToward(front, side, rotation)
 
+    def dance(self):
+        self.stand_up(0.5)
+
+
+        ###############################
+        # First we defined each step
+        ###############################
+        footStepsList = []
+
+        # 1) Step forward with your left foot
+        footStepsList.append([["LLeg"], [[0.06, 0.1, 0.0]]])
+
+        # 2) Sidestep to the left with your left foot
+        footStepsList.append([["LLeg"], [[0.00, 0.16, 0.0]]])
+
+        # 3) Move your right foot to your left foot
+        footStepsList.append([["RLeg"], [[0.00, -0.1, 0.0]]])
+
+        # 4) Sidestep to the left with your left foot
+        footStepsList.append([["LLeg"], [[0.00, 0.16, 0.0]]])
+
+        # 5) Step backward & left with your right foot
+        footStepsList.append([["RLeg"], [[-0.04, -0.1, 0.0]]])
+
+        # 6)Step forward & right with your right foot
+        footStepsList.append([["RLeg"], [[0.00, -0.16, 0.0]]])
+
+        # 7) Move your left foot to your right foot
+        footStepsList.append([["LLeg"], [[0.00, 0.1, 0.0]]])
+
+        # 8) Sidestep to the right with your right foot
+        footStepsList.append([["RLeg"], [[0.00, -0.16, 0.0]]])
+
+        ###############################
+        # Send Foot step
+        ###############################
+        stepFrequency = 0.8
+        clearExisting = False
+        nbStepDance = 2 # defined the number of cycle to make
+
+        for j in range( nbStepDance ):
+            for i in range( len(footStepsList) ):
+                try:
+                    self.mp.setFootStepsWithSpeed(
+                        footStepsList[i][0],
+                        footStepsList[i][1],
+                        [stepFrequency],
+                        clearExisting)
+                except Exception, errorMsg:
+                    print str(errorMsg)
+                    print "This example is not allowed on this robot."
+                    exit()
+
+
+        self.mp.waitUntilMoveIsFinished()
+
+        # Go to rest position
+        self.mp.rest()
+
+
     def wait(self):
         self.mp.waitUntilMoveIsFinished()
 
diff --git a/pykick/striker.py b/pykick/striker.py
index f8fd848..410ccf8 100644
--- a/pykick/striker.py
+++ b/pykick/striker.py
@@ -481,14 +481,15 @@ if __name__ == '__main__':
                 sleep(1)
                 striker.mover.move_to_fast(0.5, 0, 0)
                 striker.mover.wait()
-                break
-                # state = 'tracking'
+                break 
 
             elif state == 'kick':
                 print('KICK!')
                 striker.mover.stand_up()
                 sleep(0.3)
                 striker.mover.kick(fancy=True, foot='L')
+                striker.speak("Nice kick. Let's do a dance")
+                striker.mover.dance()
                 break
 
             loop_end = time()