Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Demo to make the crazyflie with the LED ring to fly the logo of Bitcraze
#!/usr/bin/env python3
# Demo to make the crazyflie with the LED ring to fly the logo of bitcraze
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')
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.5, 0.925]
pc.go_to(setpoint[0], setpoint[1],setpoint[2])
time.sleep(10.0)
# make outer circle
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)
#make square
setpoint = [0.0, -0.5, 1.8]
pc.go_to(setpoint[0], setpoint[1],setpoint[2])
time.sleep(0.1)
setpoint = [0.0, 0.5, 1.8]
pc.go_to(setpoint[0], setpoint[1],setpoint[2])
time.sleep(0.1)
setpoint = [0.0, 0.5, 0.8]
pc.go_to(setpoint[0], setpoint[1],setpoint[2])
time.sleep(0.1)
setpoint = [0.0, -0.375, 0.8]
pc.go_to(setpoint[0], setpoint[1],setpoint[2])
time.sleep(0.1)
#outer circle
setpoint = [0.0, -0.125, 1.0]
pc.go_to(setpoint[0], setpoint[1],setpoint[2])
for it_pi in numpy.arange(-math.pi-1/6*math.pi,math.pi-2/6*math.pi,0.1):
z_circle = 0.36*math.sin(it_pi+0.5*math.pi)+center
y_circle = 0.36*math.cos(it_pi+0.5*math.pi)
print(z_circle,y_circle)
pc.go_to(setpoint[0], y_circle, z_circle)
time.sleep(1.0)
setpoint = [0.0, -0.5, 0.925]
pc.go_to(setpoint[0], setpoint[1],setpoint[2])
time.sleep(1.0)
cf.param.set_value('ring.effect','0')
time.sleep(0.5)
# inner circle
setpoint = [0.0, 0.0, 1.13]
pc.go_to(setpoint[0], setpoint[1],setpoint[2])
time.sleep(1.0)
time.sleep(1)
turn_on_green = True
for it_pi in numpy.arange(-math.pi+0.3,math.pi+1.5,0.3):
z_circle = 0.13*math.sin(it_pi+0.5*math.pi)+center
y_circle = 0.13*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)
if turn_on_green is True:
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')
turn_on_green = False
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
You can’t perform that action at this time.