-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcflibsetparams.py
62 lines (47 loc) · 1.85 KB
/
cflibsetparams.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
from cflib.crazyflie import Crazyflie
import cflib.crtp
from cflib.utils.power_switch import PowerSwitch
import time
def on_fully_connected(link_uri):
print('Connected to %s' % link_uri)
cf.param.set_value('stabilizer.controller', 5)
# 08.03.2024
# cf.param.set_value('ctrlLee.Kpos_Px', 7.0)
# cf.param.set_value('ctrlLee.Kpos_Py', 7.0)
# cf.param.set_value('ctrlLee.Kpos_Pz', 20.0)
# cf.param.set_value('ctrlLee.Kpos_Ix', 4.0)
# cf.param.set_value('ctrlLee.Kpos_Iy', 4.0)
# cf.param.set_value('ctrlLee.Kpos_Iz', 20.0)
# cf.param.set_value('ctrlLee.Kpos_Dx', 6.0)
# cf.param.set_value('ctrlLee.Kpos_Dy', 6.0)
# cf.param.set_value('ctrlLee.Kpos_Dz', 20.0)
# 11.03.2024
cf.param.set_value('ctrlLee.Kpos_Px', 6.0)
cf.param.set_value('ctrlLee.Kpos_Py', 6.0)
cf.param.set_value('ctrlLee.Kpos_Pz', 10.0)
cf.param.set_value('ctrlLee.Kpos_Ix', 1.0)
cf.param.set_value('ctrlLee.Kpos_Iy', 1.0)
cf.param.set_value('ctrlLee.Kpos_Iz', 1.0)
cf.param.set_value('ctrlLee.Kpos_Dx', 3.0)
cf.param.set_value('ctrlLee.Kpos_Dy', 3.0)
cf.param.set_value('ctrlLee.Kpos_Dz', 20.0)
cf.param.set_value('ctrlLee.KR_x', 0.004)
cf.param.set_value('ctrlLee.KR_y', 0.004)
cf.param.set_value('ctrlLee.KR_z', 0.004)
cf.param.set_value('ctrlLee.KI_x', 0.003)
cf.param.set_value('ctrlLee.KI_y', 0.003)
cf.param.set_value('ctrlLee.KI_z', 0.003)
cf.param.set_value('ctrlLee.Kw_x', 0.001)
cf.param.set_value('ctrlLee.Kw_y', 0.001)
cf.param.set_value('ctrlLee.Kw_z', 0.001)
cf.param.set_value('ctrlLee.mass', 0.032)
print("done")
# time.sleep(3)
# cf.close_link()
cflib.crtp.init_drivers()
uri = "radio://0/80/2M/E7E7E7E70B"
PowerSwitch(uri).stm_power_cycle()
time.sleep(2)
cf = Crazyflie(rw_cache='./cache')
cf.fully_connected.add_callback(on_fully_connected)
cf.open_link(uri)