Demo that makes the crazyflie with the LED-ring fly 3 concentric circles in the colors: red, blue and green.
#!/usr/bin/env python3 | |
#Demo that makes the crazyflie fly 3 concentric circles in the colors: red, blue and green. | |
import sys | |
import time | |
import math | |
import numpy | |
import cflib.crtp | |
from cflib.crazyflie import Crazyflie | |
from cflib.crazyflie.log import LogConfig | |
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie | |
from cflib.crazyflie.syncLogger import SyncLogger | |
from cflib.positioning.position_hl_commander import PositionHlCommander | |
# URI to the Crazyflie to connect to | |
uri = 'radio://0/80/2M' | |
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(cf) | |
def position_callback(timestamp, data, logconf): | |
x = data['kalman.stateX'] | |
y = data['kalman.stateY'] | |
z = data['kalman.stateZ'] | |
print('pos: ({}, {}, {})'.format(x, y, z)) | |
def start_position_printing(scf): | |
log_conf = LogConfig(name='Position', period_in_ms=500) | |
log_conf.add_variable('kalman.stateX', 'float') | |
log_conf.add_variable('kalman.stateY', 'float') | |
log_conf.add_variable('kalman.stateZ', 'float') | |
scf.cf.log.add_config(log_conf) | |
log_conf.data_received_cb.add_callback(position_callback) | |
log_conf.start() | |
def vector_substract(v0, v1): | |
return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]] | |
def vector_add(v0, v1): | |
return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]] | |
def run_sequence(scf): | |
cf = scf.cf | |
cf.param.set_value('ring.effect','0') | |
radii = [0.5, 0.3, 0.1] | |
center = 1.3 | |
with PositionHlCommander( | |
scf, | |
x=0.0, y=0.0, z=0.0, | |
default_velocity=0.3, | |
default_height=1.0, | |
controller=PositionHlCommander.CONTROLLER_PID) as pc: | |
time.sleep(1.0) | |
setpoint = [0.0, 0.0, center-radii[0]] | |
pc.go_to(setpoint[0], setpoint[1],setpoint[2]) | |
time.sleep(10.0) | |
# make outer circle (green) | |
cf.param.set_value('ring.effect','7') | |
cf.param.set_value('ring.solidBlue','20') | |
cf.param.set_value('ring.solidRed','20') | |
cf.param.set_value('ring.solidGreen','100') | |
time.sleep(1.0) | |
for it_pi in numpy.arange(-math.pi+0.1,math.pi+0.2,0.1): | |
z_circle = radii[0]*math.sin(it_pi+0.5*math.pi)+center | |
y_circle = radii[0]*math.cos(it_pi+0.5*math.pi) | |
print(z_circle,y_circle) | |
pc.go_to(setpoint[0], y_circle, z_circle) | |
time.sleep(0.1) | |
# make inner circle | |
cf.param.set_value('ring.effect','0') | |
setpoint = [0.0, 0.0, center-radii[1]] | |
pc.go_to(setpoint[0], setpoint[1],setpoint[2]) | |
time.sleep(1.0) | |
cf.param.set_value('ring.effect','7') | |
cf.param.set_value('ring.solidBlue','20') | |
cf.param.set_value('ring.solidRed','100') | |
cf.param.set_value('ring.solidGreen','20') | |
time.sleep(1) | |
for it_pi in numpy.arange(-math.pi+0.2,math.pi+0.4,0.2): | |
z_circle = radii[1]*math.sin(it_pi+0.5*math.pi)+center | |
y_circle = radii[1]*math.cos(it_pi+0.5*math.pi) | |
print(z_circle,y_circle) | |
pc.go_to(setpoint[0], y_circle, z_circle) | |
time.sleep(0.1) | |
cf.param.set_value('ring.effect','0') | |
# make inner inner circle | |
setpoint = [0.0, 0.0, center-radii[2]] | |
pc.go_to(setpoint[0], setpoint[1],setpoint[2]) | |
time.sleep(1.0) | |
cf.param.set_value('ring.effect','7') | |
cf.param.set_value('ring.solidBlue','100') | |
cf.param.set_value('ring.solidRed','20') | |
cf.param.set_value('ring.solidGreen','20') | |
time.sleep(1) | |
for it_pi in numpy.arange(-math.pi+0.3,math.pi+0.6,0.3): | |
z_circle = radii[2]*math.sin(it_pi+0.5*math.pi)+center | |
y_circle = radii[2]*math.cos(it_pi+0.5*math.pi) | |
print(z_circle,y_circle) | |
pc.go_to(setpoint[0], y_circle, z_circle) | |
time.sleep(0.1) | |
cf.param.set_value('ring.effect','0') | |
pc.go_to(0.0, 0.0, 0.1) | |
# Make sure that the last packet leaves before the link is closed | |
# since the message queue is not flushed before closing | |
time.sleep(0.5) | |
if __name__ == '__main__': | |
cflib.crtp.init_drivers(enable_debug_driver=False) | |
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: | |
reset_estimator(scf) | |
# start_position_printing(scf) | |
run_sequence(scf) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment