Created
March 18, 2019 13:37
Revisions
-
knmcguire created this gist
Mar 18, 2019 .There are no files selected for viewing
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 charactersOriginal file line number Diff line number Diff line change @@ -0,0 +1,202 @@ #!/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)