Previously, I was using both the LH deck and Flow Deck, which only 1 was working. I tried to remove the Flow deck, utilizing only the LH deck, but the results seemed to be worst. Both the drone didn't execute as expected. Following is the modified code for 2 drones for the swarmSequenceCircle.py:
Code: Select all
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2017 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# 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.
"""
A script to fly 5 Crazyflies in formation. One stays in the center and the
other four fly around it in a circle. Mainly intended to be used with the
Flow deck.
The starting positions are vital and should be oriented like this
>
^ + v
<
The distance from the center to the perimeter of the circle is around 0.5 m
"""
import math
import time
import cflib.crtp
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
# Change uris according to your setup
URI0 = 'radio://0/80/2M/E7E7E7E7EB'
#URI1 = 'radio://0/80/2M/E7E7E7E7E'
URI2 = 'radio://0/80/2M/E7E7E7E7EA'
#URI3 = 'radio://0/80/2M/E7E7E7E7EA'
#URI4 = 'radio://0/110/2M/E7E7E7E703'
# d: diameter of circle
# z: altitude
params0 = {'d': 1.0, 'z': 0.3}
#params1 = {'d': 1.0, 'z': 0.3}
params2 = {'d': 0.0, 'z': 0.5}
#params3 = {'d': 1.0, 'z': 0.3}
#params4 = {'d': 1.0, 'z': 0.3}
uris = {
URI0,
#URI1,
URI2,
#URI3,
#URI4,
}
params = {
URI0: [params0],
#URI1: [params1],
URI2: [params2],
#URI3: [params3],
#URI4: [params4],
}
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')
time.sleep(2)
def poshold(cf, t, z):
steps = t * 10
for r in range(steps):
cf.commander.send_hover_setpoint(0, 0, 0, z)
time.sleep(0.1)
def run_sequence(scf, params):
cf = scf.cf
# Number of setpoints sent per second
fs = 4
fsi = 1.0 / fs
# Compensation for unknown error :-(
comp = 1.3
# Base altitude in meters
base = 0.15
d = params['d']
z = params['z']
poshold(cf, 2, base)
ramp = fs * 2
for r in range(ramp):
cf.commander.send_hover_setpoint(0, 0, 0, base + r * (z - base) / ramp)
time.sleep(fsi)
poshold(cf, 2, z)
for _ in range(2):
# The time for one revolution
circle_time = 8
steps = circle_time * fs
for _ in range(steps):
cf.commander.send_hover_setpoint(d * comp * math.pi / circle_time,
0, 360.0 / circle_time, z)
time.sleep(fsi)
poshold(cf, 2, z)
for r in range(ramp):
cf.commander.send_hover_setpoint(0, 0, 0,
base + (ramp - r) * (z - base) / ramp)
time.sleep(fsi)
poshold(cf, 1, base)
cf.commander.send_stop_setpoint()
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
factory = CachedCfFactory(rw_cache='./cache')
with Swarm(uris, factory=factory) as swarm:
swarm.parallel(reset_estimator)
swarm.parallel(run_sequence, args_dict=params)
I am only using URI0 (Drone on the outer circle) and URI2 (Middle Drone). Is there any mistake in the modified example code?