Skip to content

Instantly share code, notes, and snippets.

@knmcguire
Created March 18, 2019 13:37
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save knmcguire/1b3a3b183d1293edcdc08ea464960b89 to your computer and use it in GitHub Desktop.
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