Demo to make the crazyflie with the LED ring to fly the logo of Bitcraze

lighthouse_trace_logo.py

#!/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)