From 7f7af0b2f709269f14ce8988947d48f2e150075f Mon Sep 17 00:00:00 2001
From: Arthur <goldartt@gmail.com>
Date: Tue, 26 Feb 2019 17:28:55 +0000
Subject: [PATCH 1/2] Changed test animation file path

---
 Drone/play_animation.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/Drone/play_animation.py b/Drone/play_animation.py
index 5b49881..83a5c74 100644
--- a/Drone/play_animation.py
+++ b/Drone/play_animation.py
@@ -5,7 +5,7 @@ from FlightLib.FlightLib import FlightLib
 #FlightLib.init('SingleCleverFlight')
 from FlightLib.FlightLib import LedLib
 
-animation_file_path = 'animation.csv'
+animation_file_path = 'test_animation/test_1.csv'
 frames = []
 USE_LEDS = True
 
@@ -65,7 +65,7 @@ if __name__ == '__main__':
     #takeoff(round(float(first_frame['x']), 4), round(float(first_frame['y']), 4), round(float(first_frame['z']), 4))
     takeoff()
     FlightLib.reach(x=frames[0]['x'], y=frames[0]['y'], z=frames[0]['z'])
-    for frame in frames:
-        rate.sleep()
-        do_next_animation(frame)
+    #for frame in frames:
+    #    rate.sleep()
+    #    do_next_animation(frame)
     land()
\ No newline at end of file

From 328350e2ab00f725b8beb0a5f6973c184dad6658 Mon Sep 17 00:00:00 2001
From: Arthur <goldartt@gmail.com>
Date: Tue, 26 Feb 2019 21:01:24 +0000
Subject: [PATCH 2/2] Final test version of play_animation.py

---
 Drone/play_animation.py | 25 +++++++++++++------------
 1 file changed, 13 insertions(+), 12 deletions(-)

diff --git a/Drone/play_animation.py b/Drone/play_animation.py
index 83a5c74..3567238 100644
--- a/Drone/play_animation.py
+++ b/Drone/play_animation.py
@@ -13,19 +13,19 @@ USE_LEDS = True
 def takeoff():  #x, y, z
     if USE_LEDS:
         LedLib.wipe_to(0, 255, 0)
-    FlightLib.takeoff()
+    FlightLib.takeoff1()
 
 
 def land():
     if USE_LEDS:
         LedLib.blink(0, 255, 0)
-    FlightLib.land()
+    FlightLib.land1()
     if USE_LEDS:
         LedLib.off()
 
 
-def do_next_animation(current_frame):
-    FlightLib.navto(current_frame['x'], current_frame['y'], current_frame['z'], speed=current_frame['speed'])
+def do_next_animation(current_frame, x0 = 0, y0 = 0):
+    FlightLib.navto(current_frame['x']+x0, current_frame['y']+y0, current_frame['z'], yaw = 1.57)
     if USE_LEDS:
         LedLib.fill(current_frame['green'], current_frame['red'], current_frame['blue'])
 
@@ -36,8 +36,7 @@ def read_animation_file(filepath=animation_file_path):
             animation_file, delimiter=',', quotechar='|'
         )
         for row in csv_reader:
-            frame_number, x, y, z, yaw, red, green, blue = row
-            speed = FlightLib.get_distance()
+            frame_number, x, y, z, speed, red, green, blue = row
             frames.append({
                 'number': int(frame_number),
                 'x': float(x),
@@ -57,15 +56,17 @@ def get_frames():
 
 if __name__ == '__main__':
     rospy.init_node('Animation_player', anonymous=True)
-    LedLib.init_led()
-
+    if USE_LEDS:
+        LedLib.init_led()
+    X0 = 0.5
+    Y0 = 1.0
     read_animation_file()
     rate = rospy.Rate(10)
     #first_frame = frames[0]
     #takeoff(round(float(first_frame['x']), 4), round(float(first_frame['y']), 4), round(float(first_frame['z']), 4))
     takeoff()
-    FlightLib.reach(x=frames[0]['x'], y=frames[0]['y'], z=frames[0]['z'])
-    #for frame in frames:
-    #    rate.sleep()
-    #    do_next_animation(frame)
+    FlightLib.reach(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw = 1.57)
+    for frame in frames:
+        rate.sleep()
+        do_next_animation(frame, x0 = X0, y0 = Y0)
     land()
\ No newline at end of file