Merge branch 'pr/1'

.gitignore

@@ -104,3 +104,4 @@ venv.bak/
 .mypy_cache/
 .vscode/settings.json
 Server/tests.py
+Drone/test_animation/

.gitmodules

@@ -1,6 +1,6 @@
 [submodule "Drone/FlightLib"]
 	path = Drone/FlightLib
-	url = git://github.com/artem30801/CleverFlightLib.git
+	url = https://github.com/artem30801/CleverFlightLib.git
 [submodule "blender-csv-animation"]
 	path = blender-csv-animation
 	url = https://github.com/artem30801/blender-csv-animation

Drone/play_animation.py

@@ -5,38 +5,29 @@ 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
 
 
 def takeoff():
     if USE_LEDS:
-        LedLib.wipe_to(0, 255, 0)
-    FlightLib.takeoff()
+        LedLib.wipe_to(255, 0, 0)
+    FlightLib.takeoff1()
 
 
 def land():
     if USE_LEDS:
-        LedLib.blink(0, 255, 0)
-    FlightLib.land()
+        LedLib.blink(255, 0, 0)
+    FlightLib.land1()
     if USE_LEDS:
         LedLib.off()
 
 
-def do_next_animation(current_frame):
-    FlightLib.navto(
-        round(float(current_frame['x']), 4),
-        round(float(current_frame['y']), 4),
-        round(float(current_frame['z']), 4),
-        round(float(current_frame['yaw']), 4),
-    )
+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(
-            int(current_frame['green']),
-            int(current_frame['red']),
-            int(current_frame['blue'])
-        )
+        LedLib.fill(current_frame['red'], current_frame['green'], current_frame['blue'])
 
 
 def read_animation_file(filepath=animation_file_path):
@@ -45,16 +36,16 @@ 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
+            frame_number, x, y, z, yaw, red, green, blue = row
             frames.append({
-                'number': frame_number,
-                'x': x,
-                'y': y,
-                'z': z,
-                'yaw': yaw,
-                'red': red,
-                'green': green,
-                'blue': blue,
+                'number': int(frame_number),
+                'x': float(x),
+                'y': float(y),
+                'z': float(z),
+                'yaw': float(yaw),
+                'red': int(red),
+                'green': int(green),
+                'blue': int(blue),
             })
 
 
@@ -67,18 +58,14 @@ if __name__ == '__main__':
     rospy.init_node('Animation_player', anonymous=True)
     if USE_LEDS:
         LedLib.init_led()
-
+    X0 = 0.5
+    Y0 = 1.0
     read_animation_file()
-
+    rate = rospy.Rate(8)
     takeoff()
-    first_frame = frames[0]
-    FlightLib.reach(round(float(first_frame['x']), 4),
-                    round(float(first_frame['y']), 4),
-                    round(float(first_frame['z']), 4))
-
+    FlightLib.reach(x=frames[0]['x']+X0, y=frames[0]['y']+Y0, z=frames[0]['z'], yaw = 1.57)
     for frame in frames:
-        time.sleep(0.1)
-        do_next_animation(frame)
-
+        do_next_animation(frame, x0 = X0, y0 = Y0)
+        rate.sleep()
     land()
-    time.sleep(3)
+    time.sleep(1)