/LongsynchronizedSequence.py Secret
Created
July 4, 2019 16:54
Star
You must be signed in to star a gist
Longer synchronized sequence
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python3 | |
# -*- coding: utf-8 -*- | |
# | |
# || ____ _ __ | |
# +------+ / __ )(_) /_______________ _____ ___ | |
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ | |
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ | |
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ | |
# | |
# Copyright (C) 2019 Bitcraze AB | |
# | |
# This program is free software; you can redistribute it and/or | |
# modify it under the terms of the GNU General Public License | |
# as published by the Free Software Foundation; either version 2 | |
# of the License, or (at your option) any later version. | |
# | |
# This program is distributed in the hope that it will be useful, | |
# but WITHOUT ANY WARRANTY; without even the implied warranty of | |
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
# GNU General Public License for more details. | |
# You should have received a copy of the GNU General Public License | |
# along with this program; if not, write to the Free Software | |
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, | |
# MA 02110-1301, USA. | |
""" | |
Simple example of a synchronized swarm choreography using the High level commander. | |
The swarm takes off and flies a synchronous choreography before landing. | |
The take-of is relative to the start position but the Goto are absolute. | |
The sequence contains a list of commands to be executed at each step. | |
This example is intended to work with any absolute positioning system. | |
It aims at documenting how to use the High Level Commander together with | |
the Swarm class to achieve synchronous sequences. | |
""" | |
import time | |
import cflib.crtp | |
from cflib.crazyflie.log import LogConfig | |
from cflib.crazyflie.swarm import CachedCfFactory | |
from cflib.crazyflie.swarm import Swarm | |
from cflib.crazyflie.syncLogger import SyncLogger | |
from collections import namedtuple | |
import threading | |
from queue import Queue | |
# Time for one step in second | |
STEP_TIME = 1 | |
# Possible commands, all times are in seconds | |
Takeoff = namedtuple('Takeoff', ['height', 'time']) | |
Land = namedtuple("Land", ['time']) | |
Goto = namedtuple('Goto', ['x', 'y', 'z', 'time']) | |
Ring = namedtuple('Ring', ['r', 'g', 'b', 'intensity', 'time']) # RGB [0-255], Intensity [0.0-1.0] | |
Quit = namedtuple('Quit', []) # Reserved for the control loop, do not use in sequence | |
uris = [ | |
'radio://0/10/2M/E7E7E7E701', # cf_id 0 | |
'radio://0/10/2M/E7E7E7E702', # cf_id 1 | |
'radio://0/10/2M/E7E7E7E703', # cf_id 3 | |
'radio://0/10/2M/E7E7E7E704', # cf_id 4 | |
'radio://0/10/2M/E7E7E7E705', # cf_id 5 | |
'radio://0/10/2M/E7E7E7E706', # cf_id 6 | |
'radio://0/10/2M/E7E7E7E707', # cf_id 7 | |
'radio://0/10/2M/E7E7E7E708', # cf_id 8 | |
'radio://0/10/2M/E7E7E7E709', # cf_id 9 | |
# Add more URIs if you want more copters in the swarm | |
] | |
Z_center = 1.5 | |
Z_inner = 1 | |
Z_outer = 0.5 | |
X_inner = 0.4 | |
X_outer = 0.8 | |
sequence = [ | |
# Step, CF_id, action | |
# Takeoff for outer square | |
( 0, 0, Takeoff(Z_outer, 2)), | |
( 0, 1, Takeoff(Z_outer, 2)), | |
( 0, 2, Takeoff(Z_outer, 2)), | |
( 0, 3, Takeoff(Z_outer, 2)), | |
# Takeoff for inner square | |
( 2, 4, Takeoff(Z_inner, 3)), | |
( 2, 5, Takeoff(Z_inner, 3)), | |
( 2, 6, Takeoff(Z_inner, 3)), | |
( 2, 7, Takeoff(Z_inner, 3)), | |
# Takeoff for center | |
( 5, 8, Takeoff(Z_center, 3)), | |
# Goto starting point | |
# Outer square | |
( 8, 0, Goto(X_outer, X_outer, Z_outer, 1)), | |
( 8, 1, Goto(X_outer, -X_outer, Z_outer, 1)), | |
( 8, 2, Goto(-X_outer, -X_outer, Z_outer, 1)), | |
( 8, 3, Goto(-X_outer, X_outer, Z_outer, 1)), | |
# Inner square | |
( 8, 4, Goto(X_inner, X_inner, Z_inner, 1)), | |
( 8, 5, Goto(X_inner, -X_inner, Z_inner, 1)), | |
( 8, 6, Goto(-X_inner, -X_inner, Z_inner, 1)), | |
( 8, 7, Goto(-X_inner, X_inner, Z_inner, 1)), | |
# Center | |
( 8, 8, Goto(0, 0, Z_center, 1)), | |
# Turn lights on | |
# Outer square | |
( 9, 0, Ring(255, 255, 255, 1, 0)), | |
( 9, 1, Ring(255, 255, 255, 1, 0)), | |
( 9, 2, Ring(255, 255, 255, 1, 0)), | |
( 9, 3, Ring(255, 255, 255, 1, 0)), | |
# Inner square | |
( 11, 4, Ring(128, 0, 128, 1, 0)), | |
( 11, 5, Ring(128, 0, 128, 1, 0)), | |
( 11, 6, Ring(128, 0, 128, 1, 0)), | |
( 11, 7, Ring(128, 0, 128, 1, 0)), | |
# Center | |
( 13, 8, Ring(255, 0, 255, 1, 0)), | |
# Start rotation | |
( 16, 0, Goto(X_outer, -X_outer, Z_outer, 2.5)), | |
( 16, 1, Goto(-X_outer, -X_outer, Z_outer, 2.5)), | |
( 16, 2, Goto(-X_outer, X_outer, Z_outer, 2.5)), | |
( 16, 3, Goto(X_outer, X_outer, Z_outer, 2.5)), | |
( 16, 4, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 16, 5, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 16, 6, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
( 16, 7, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 18.5, 0, Goto(-X_outer, -X_outer, Z_outer, 2.5)), | |
( 18.5, 1, Goto(-X_outer, X_outer, Z_outer, 2.5)), | |
( 18.5, 2, Goto(X_outer, X_outer, Z_outer, 2.5)), | |
( 18.5, 3, Goto(X_outer, -X_outer, Z_outer, 2.5)), | |
( 18.5, 4, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 18.5, 5, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
( 18.5, 6, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 18.5, 7, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 21, 0, Goto(-X_outer, X_outer, Z_outer, 2.5)), | |
( 21, 1, Goto(X_outer, X_outer, Z_outer, 2.5)), | |
( 21, 2, Goto(X_outer, -X_outer, Z_outer, 2.5)), | |
( 21, 3, Goto(-X_outer, -X_outer, Z_outer, 2.5)), | |
( 21, 4, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
( 21, 5, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 21, 6, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 21, 7, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 23.5, 0, Goto(X_outer, X_outer, Z_outer, 2.5)), | |
( 23.5, 1, Goto(X_outer, -X_outer, Z_outer, 2.5)), | |
( 23.5, 2, Goto(-X_outer, -X_outer, Z_outer, 2.5)), | |
( 23.5, 3, Goto(-X_outer, X_outer, Z_outer, 2.5)), | |
( 23.5, 4, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 23.5, 5, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 23.5, 6, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 23.5, 7, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
# Change color | |
# Outer square | |
( 24.5, 0, Ring(255, 0, 255, 1, 0)), | |
( 24.5, 1, Ring(255, 0, 255, 1, 0)), | |
( 24.5, 2, Ring(255, 0, 255, 1, 0)), | |
( 24.5, 3, Ring(255, 0, 255, 1, 0)), | |
# Inner square | |
( 25.5, 4, Ring(255, 255, 255, 1, 0)), | |
( 25.5, 5, Ring(255, 255, 255, 1, 0)), | |
( 25.5, 6, Ring(255, 255, 255, 1, 0)), | |
( 25.5, 7, Ring(255, 255, 255, 1, 0)), | |
# Center | |
( 26.5, 8, Ring(128, 0, 128, 1, 0)), | |
# Switch Z for outer and center | |
( 28, 0, Goto(X_outer, X_outer, Z_center, 2.5)), | |
( 28, 1, Goto(X_outer, -X_outer, Z_center, 2.5)), | |
( 28, 2, Goto(-X_outer, -X_outer, Z_center, 2.5)), | |
( 28, 3, Goto(-X_outer, X_outer, Z_center, 2.5)), | |
( 28, 8, Goto(0, 0, Z_outer, 2.5)), | |
# Change lights on all crazyflies | |
( 31, 0, Ring(0, 0, 255, 1, 0)), | |
( 31, 1, Ring(0, 0, 255, 1, 0)), | |
( 31, 2, Ring(0, 0, 255, 1, 0)), | |
( 31, 3, Ring(0, 0, 255, 1, 0)), | |
( 31, 4, Ring(0, 0, 255, 1, 0)), | |
( 31, 5, Ring(0, 0, 255, 1, 0)), | |
( 31, 6, Ring(0, 0, 255, 1, 0)), | |
( 31, 7, Ring(0, 0, 255, 1, 0)), | |
( 31, 8, Ring(0, 0, 255, 1, 0)), | |
# Start rotation - Opposite way | |
( 33, 0, Goto(-X_outer, X_outer, Z_center, 2.5)), | |
( 33, 1, Goto(X_outer, X_outer, Z_center, 2.5)), | |
( 33, 2, Goto(X_outer, -X_outer, Z_center, 2.5)), | |
( 33, 3, Goto(-X_outer, -X_outer, Z_center, 2.5)), | |
( 33, 4, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
( 33, 5, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 33, 6, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 33, 7, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 35.5, 0, Goto(-X_outer, -X_outer, Z_center, 2.5)), | |
( 35.5, 1, Goto(-X_outer, X_outer, Z_center, 2.5)), | |
( 35.5, 2, Goto(X_outer, X_outer, Z_center, 2.5)), | |
( 35.5, 3, Goto(X_outer, -X_outer, Z_center, 2.5)), | |
( 35.5, 4, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 35.5, 5, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
( 35.5, 6, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 35.5, 7, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 38, 0, Goto(X_outer, -X_outer, Z_center, 2.5)), | |
( 38, 1, Goto(-X_outer, -X_outer, Z_center, 2.5)), | |
( 38, 2, Goto(-X_outer, X_outer, Z_center, 2.5)), | |
( 38, 3, Goto(X_outer, X_outer, Z_center, 2.5)), | |
( 38, 4, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 38, 5, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 38, 6, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
( 38, 7, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 40.5, 0, Goto(X_outer, X_outer, Z_center, 2.5)), | |
( 40.5, 1, Goto(X_outer, -X_outer, Z_center, 2.5)), | |
( 40.5, 2, Goto(-X_outer, -X_outer, Z_center, 2.5)), | |
( 40.5, 3, Goto(-X_outer, X_outer, Z_center, 2.5)), | |
( 40.5, 4, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 40.5, 5, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 40.5, 6, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 40.5, 7, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
# Go into cube formation | |
( 45, 0, Goto(X_inner, X_inner, Z_outer, 2.5)), | |
( 45, 1, Goto(X_inner, -X_inner, Z_outer, 2.5)), | |
( 45, 2, Goto(-X_inner, -X_inner, Z_outer, 2.5)), | |
( 45, 3, Goto(-X_inner, X_inner, Z_outer, 2.5)), | |
( 45, 4, Goto(X_inner, X_inner, Z_center, 2.5)), | |
( 45, 5, Goto(X_inner, -X_inner, Z_center, 2.5)), | |
( 45, 6, Goto(-X_inner, -X_inner, Z_center, 2.5)), | |
( 45, 7, Goto(-X_inner, X_inner, Z_center, 2.5)), | |
( 45, 8, Goto(0, 0, Z_inner, 2.5)), | |
# Flash lights after each turn | |
( 48, 0, Ring(255, 255, 255, 1, 0)), | |
( 48, 1, Ring(255, 255, 255, 1, 0)), | |
( 48, 2, Ring(255, 255, 255, 1, 0)), | |
( 48, 3, Ring(255, 255, 255, 1, 0)), | |
( 48, 4, Ring(255, 255, 255, 1, 0)), | |
( 48, 5, Ring(255, 255, 255, 1, 0)), | |
( 48, 6, Ring(255, 255, 255, 1, 0)), | |
( 48, 7, Ring(255, 255, 255, 1, 0)), | |
( 48, 8, Ring(255, 255, 255, 1, 0)), | |
# Start rotation in the cube formation - Clockwise | |
( 50, 0, Goto(X_inner, -X_inner, Z_outer, 2.5)), | |
( 50, 1, Goto(-X_inner, -X_inner, Z_outer, 2.5)), | |
( 50, 2, Goto(-X_inner, X_inner, Z_outer, 2.5)), | |
( 50, 3, Goto(X_inner, X_inner, Z_outer, 2.5)), | |
( 50, 4, Goto(X_inner, -X_inner, Z_center, 2.5)), | |
( 50, 5, Goto(-X_inner, -X_inner, Z_center, 2.5)), | |
( 50, 6, Goto(-X_inner, X_inner, Z_center, 2.5)), | |
( 50, 7, Goto(X_inner, X_inner, Z_center, 2.5)), | |
# Flash lights after each turn | |
( 50, 0, Ring(0, 255, 0, 1, 0)), | |
( 50, 1, Ring(0, 255, 0, 1, 0)), | |
( 50, 2, Ring(0, 255, 0, 1, 0)), | |
( 50, 3, Ring(0, 255, 0, 1, 0)), | |
( 50, 4, Ring(0, 255, 0, 1, 0)), | |
( 50, 5, Ring(0, 255, 0, 1, 0)), | |
( 50, 6, Ring(0, 255, 0, 1, 0)), | |
( 50, 7, Ring(0, 255, 0, 1, 0)), | |
( 50, 8, Ring(0, 255, 0, 1, 0)), | |
( 52.5, 0, Goto(-X_inner, -X_inner, Z_outer, 2.5)), | |
( 52.5, 1, Goto(-X_inner, X_inner, Z_outer, 2.5)), | |
( 52.5, 2, Goto(X_inner, X_inner, Z_outer, 2.5)), | |
( 52.5, 3, Goto(X_inner, -X_inner, Z_outer, 2.5)), | |
( 52.5, 4, Goto(-X_inner, -X_inner, Z_center, 2.5)), | |
( 52.5, 5, Goto(-X_inner, X_inner, Z_center, 2.5)), | |
( 52.5, 6, Goto(X_inner, X_inner, Z_center, 2.5)), | |
( 52.5, 7, Goto(X_inner, -X_inner, Z_center, 2.5)), | |
# Flash lights after each turn | |
( 52.5, 0, Ring(255, 255, 0, 1, 0)), | |
( 52.5, 1, Ring(255, 255, 0, 1, 0)), | |
( 52.5, 2, Ring(255, 255, 0, 1, 0)), | |
( 52.5, 3, Ring(255, 255, 0, 1, 0)), | |
( 52.5, 4, Ring(255, 255, 0, 1, 0)), | |
( 52.5, 5, Ring(255, 255, 0, 1, 0)), | |
( 52.5, 6, Ring(255, 255, 0, 1, 0)), | |
( 52.5, 7, Ring(255, 255, 0, 1, 0)), | |
( 52.5, 8, Ring(255, 255, 0, 1, 0)), | |
( 55, 0, Goto(-X_inner, X_inner, Z_outer, 2.5)), | |
( 55, 1, Goto(X_inner, X_inner, Z_outer, 2.5)), | |
( 55, 2, Goto(X_inner, -X_inner, Z_outer, 2.5)), | |
( 55, 3, Goto(-X_inner, -X_inner, Z_outer, 2.5)), | |
( 55, 4, Goto(-X_inner, X_inner, Z_center, 2.5)), | |
( 55, 5, Goto(X_inner, X_inner, Z_center, 2.5)), | |
( 55, 6, Goto(X_inner, -X_inner, Z_center, 2.5)), | |
( 55, 7, Goto(-X_inner, -X_inner, Z_center, 2.5)), | |
# Flash lights after each turn | |
( 55, 0, Ring(0, 255, 255, 1, 0)), | |
( 55, 1, Ring(0, 255, 255, 1, 0)), | |
( 55, 2, Ring(0, 255, 255, 1, 0)), | |
( 55, 3, Ring(0, 255, 255, 1, 0)), | |
( 55, 4, Ring(0, 255, 255, 1, 0)), | |
( 55, 5, Ring(0, 255, 255, 1, 0)), | |
( 55, 6, Ring(0, 255, 255, 1, 0)), | |
( 55, 7, Ring(0, 255, 255, 1, 0)), | |
( 55, 8, Ring(0, 255, 255, 1, 0)), | |
( 57.5, 0, Goto(X_inner, X_inner, Z_outer, 2.5)), | |
( 57.5, 1, Goto(X_inner, -X_inner, Z_outer, 2.5)), | |
( 57.5, 2, Goto(-X_inner, -X_inner, Z_outer, 2.5)), | |
( 57.5, 3, Goto(-X_inner, X_inner, Z_outer, 2.5)), | |
( 57.5, 4, Goto(X_inner, X_inner, Z_center, 2.5)), | |
( 57.5, 5, Goto(X_inner, -X_inner, Z_center, 2.5)), | |
( 57.5, 6, Goto(-X_inner, -X_inner, Z_center, 2.5)), | |
( 57.5, 7, Goto(-X_inner, X_inner, Z_center, 2.5)), | |
# Flash lights after each turn | |
( 57.5, 0, Ring(128, 0, 128, 1, 0)), | |
( 57.5, 1, Ring(128, 0, 128, 1, 0)), | |
( 57.5, 2, Ring(128, 0, 128, 1, 0)), | |
( 57.5, 3, Ring(128, 0, 128, 1, 0)), | |
( 57.5, 4, Ring(128, 0, 128, 1, 0)), | |
( 57.5, 5, Ring(128, 0, 128, 1, 0)), | |
( 57.5, 6, Ring(128, 0, 128, 1, 0)), | |
( 57.5, 7, Ring(128, 0, 128, 1, 0)), | |
( 57.5, 8, Ring(128, 0, 128, 1, 0)), | |
# Goto back to starting formation | |
( 60, 0, Goto(X_outer, X_outer, Z_outer, 2.5)), | |
( 60, 1, Goto(X_outer, -X_outer, Z_outer, 2.5)), | |
( 60, 2, Goto(-X_outer, -X_outer, Z_outer, 2.5)), | |
( 60, 3, Goto(-X_outer, X_outer, Z_outer, 2.5)), | |
( 65, 0, Ring(255, 255, 255, 1, 0)), | |
( 65, 1, Ring(255, 255, 255, 1, 0)), | |
( 65, 2, Ring(255, 255, 255, 1, 0)), | |
( 65, 3, Ring(255, 255, 255, 1, 0)), | |
( 67, 4, Goto(X_inner, X_inner, Z_inner, 2.5)), | |
( 67, 5, Goto(X_inner, -X_inner, Z_inner, 2.5)), | |
( 67, 6, Goto(-X_inner, -X_inner, Z_inner, 2.5)), | |
( 67, 7, Goto(-X_inner, X_inner, Z_inner, 2.5)), | |
( 69, 4, Ring(255, 255, 255, 1, 0)), | |
( 69, 5, Ring(255, 255, 255, 1, 0)), | |
( 69, 6, Ring(255, 255, 255, 1, 0)), | |
( 69, 7, Ring(255, 255, 255, 1, 0)), | |
( 71, 8, Goto(0, 0, Z_center, 1)), | |
( 73, 8, Ring(255, 255, 255, 1, 0)), | |
# Land all crazyflies | |
( 75, 0, Land(3)), | |
( 75, 1, Land(3)), | |
( 75, 2, Land(3)), | |
( 75, 3, Land(3)), | |
( 75, 4, Land(3)), | |
( 75, 5, Land(3)), | |
( 75, 6, Land(3)), | |
( 75, 7, Land(3)), | |
( 75, 8, Land(3)), | |
# Turn off lights on all crazyflies | |
( 78, 0, Ring(0,0,0,0, 5)), | |
( 78, 1, Ring(0,0,0,0, 5)), | |
( 78, 2, Ring(0,0,0,0, 5)), | |
( 78, 3, Ring(0,0,0,0, 5)), | |
( 78, 4, Ring(0,0,0,0, 5)), | |
( 78, 5, Ring(0,0,0,0, 5)), | |
( 78, 6, Ring(0,0,0,0, 5)), | |
( 78, 7, Ring(0,0,0,0, 5)), | |
( 78, 8, Ring(0,0,0,0, 5)), | |
] | |
""" | |
sequence = [ | |
# Step, CF_id, action | |
( 0, 0, Takeoff(0.5, 2.5)), | |
( 0, 2, Takeoff(0.5, 2.5)), | |
( 1, 1, Takeoff(1.0, 2)), | |
( 2, 0, Goto(-0.5, -0.5, 0.5, 1)), | |
( 2, 2, Goto(0.5, 0.5, 0.5, 1)), | |
( 3, 1, Goto(0, 0, 1, 1)), | |
( 4, 0, Ring(255, 255, 255, 0.2, 0)), | |
( 4, 1, Ring(255, 0, 0, 0.2, 0)), | |
( 4, 2, Ring(255, 255, 255, 0.2, 0)), | |
( 5, 0, Goto(0.5, -0.5, 0.5, 2)), | |
( 5, 2, Goto(-0.5, 0.5, 0.5, 2)), | |
( 7, 0, Goto(0.5, 0.5, 0.5, 2)), | |
( 7, 2, Goto(-0.5, -0.5, 0.5, 2)), | |
( 9, 0, Goto(-0.5, 0.5, 0.5, 2)), | |
( 9, 2, Goto(0.5, -0.5, 0.5, 2)), | |
( 11, 0, Goto(-0.5, -0.5, 0.5, 2)), | |
( 11, 2, Goto(0.5, 0.5, 0.5, 2)), | |
( 13, 0, Land(2)), | |
( 13, 1, Land(2)), | |
( 13, 2, Land(2)), | |
( 15, 0, Ring(0,0,0,0, 5)), | |
( 15, 1, Ring(0,0,0,0, 5)), | |
( 15, 2, Ring(0,0,0,0, 5)), | |
] | |
""" | |
def wait_for_position_estimator(scf): | |
print('Waiting for estimator to find position...') | |
log_config = LogConfig(name='Kalman Variance', period_in_ms=500) | |
log_config.add_variable('kalman.varPX', 'float') | |
log_config.add_variable('kalman.varPY', 'float') | |
log_config.add_variable('kalman.varPZ', 'float') | |
var_y_history = [1000] * 10 | |
var_x_history = [1000] * 10 | |
var_z_history = [1000] * 10 | |
threshold = 0.001 | |
with SyncLogger(scf, log_config) as logger: | |
for log_entry in logger: | |
data = log_entry[1] | |
var_x_history.append(data['kalman.varPX']) | |
var_x_history.pop(0) | |
var_y_history.append(data['kalman.varPY']) | |
var_y_history.pop(0) | |
var_z_history.append(data['kalman.varPZ']) | |
var_z_history.pop(0) | |
min_x = min(var_x_history) | |
max_x = max(var_x_history) | |
min_y = min(var_y_history) | |
max_y = max(var_y_history) | |
min_z = min(var_z_history) | |
max_z = max(var_z_history) | |
# print("{} {} {}". | |
# format(max_x - min_x, max_y - min_y, max_z - min_z)) | |
if (max_x - min_x) < threshold and ( | |
max_y - min_y) < threshold and ( | |
max_z - min_z) < threshold: | |
break | |
def reset_estimator(scf): | |
cf = scf.cf | |
cf.param.set_value('kalman.resetEstimation', '1') | |
time.sleep(0.1) | |
cf.param.set_value('kalman.resetEstimation', '0') | |
wait_for_position_estimator(scf) | |
def activate_high_level_commander(scf): | |
scf.cf.param.set_value('commander.enHighLevel', '1') | |
def activate_mellinger_controller(scf, use_mellinger): | |
controller = 1 | |
if use_mellinger: | |
controller = 2 | |
scf.cf.param.set_value('stabilizer.controller', str(controller)) | |
def set_ring_color(cf, r, g, b, intensity, time): | |
cf.param.set_value('ring.fadeTime', str(time)) | |
r *= intensity | |
g *= intensity | |
b *= intensity | |
color = (int(r) << 16) | (int(g) << 8) | int(b) | |
cf.param.set_value('ring.fadeColor', str(color)) | |
def crazyflie_control(scf): | |
cf = scf.cf | |
control = controlQueues[uris.index(cf.link_uri)] | |
activate_mellinger_controller(scf, True) | |
commander = scf.cf.high_level_commander | |
# Set fade to color effect and reset to Led-ring OFF | |
set_ring_color(cf, 0,0,0, 0, 0) | |
cf.param.set_value('ring.effect', '14') | |
while True: | |
command = control.get() | |
if type(command) is Quit: | |
return | |
elif type(command) is Takeoff: | |
commander.takeoff(command.height, command.time) | |
elif type(command) is Land: | |
commander.land(0.0, command.time) | |
elif type(command) is Goto: | |
commander.go_to(command.x, command.y, command.z, 0, command.time) | |
elif type(command) is Ring: | |
set_ring_color(cf, command.r, command.g, command.b, | |
command.intensity, command.time) | |
pass | |
else: | |
print("Warning! unknown command {} for uri {}".format(command, | |
cf.uri)) | |
def control_thread(): | |
pointer = 0 | |
step = 0 | |
stop = False | |
while not stop: | |
print("Step {}:".format(step)) | |
while sequence[pointer][0] <= step: | |
cf_id = sequence[pointer][1] | |
command = sequence[pointer][2] | |
print(" - Running: {} on {}".format(command, cf_id)) | |
controlQueues[cf_id].put(command) | |
pointer += 1 | |
if pointer >= len(sequence): | |
print("Reaching the end of the sequence, stopping!") | |
stop = True | |
break | |
step += 1 | |
time.sleep(STEP_TIME) | |
for ctrl in controlQueues: | |
ctrl.put(Quit()) | |
if __name__ == '__main__': | |
controlQueues = [Queue() for _ in range(len(uris))] | |
cflib.crtp.init_drivers(enable_debug_driver=False) | |
factory = CachedCfFactory(rw_cache='./cache') | |
with Swarm(uris, factory=factory) as swarm: | |
swarm.parallel_safe(activate_high_level_commander) | |
swarm.parallel_safe(reset_estimator) | |
print("Starting sequence!") | |
threading.Thread(target=control_thread).start() | |
swarm.parallel_safe(crazyflie_control) | |
time.sleep(1) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment