diff --git a/.gitignore b/.gitignore index 44de96f..d343f30 100644 --- a/.gitignore +++ b/.gitignore @@ -104,3 +104,4 @@ venv.bak/ .mypy_cache/ .vscode/settings.json Server/tests.py +Drone/test_animation/ diff --git a/.gitmodules b/.gitmodules index 01eaa73..2be8bca 100644 --- a/.gitmodules +++ b/.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 diff --git a/Drone/play_animation.py b/Drone/play_animation.py index f93135b..9b58b39 100644 --- a/Drone/play_animation.py +++ b/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)