Showing
1 changed file
with
60 additions
and
0 deletions
yujin_test/step1.py
0 → 100644
1 | +import CoDrone | ||
2 | +import keyboard | ||
3 | +from CoDrone.system import Direction | ||
4 | +from time import sleep | ||
5 | + | ||
6 | + | ||
7 | +def setHeight(_mHeight, _slave): | ||
8 | + while True: | ||
9 | + _sHeight = _slave.get_height() | ||
10 | + if _mHeight - 100 <= _sHeight <= _mHeight + 100: | ||
11 | + return | ||
12 | + elif _sHeight < _mHeight - 100: | ||
13 | + _slave.go(Direction.UP) | ||
14 | + sleep(0.2) | ||
15 | + elif _sHeight > _mHeight + 100: | ||
16 | + _slave.go(Direction.DOWN) | ||
17 | + sleep(0.2) | ||
18 | + | ||
19 | + | ||
20 | +def main(): | ||
21 | + master = CoDrone.CoDrone() | ||
22 | + master.connect("None", "COM5", False) | ||
23 | + slave = CoDrone.CoDrone() | ||
24 | + slave.connect("None", "COM6", False) | ||
25 | + | ||
26 | + bHeight = master.get_height() # 고도 | ||
27 | + if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작 | ||
28 | + slave.takeoff() | ||
29 | + | ||
30 | + while True: | ||
31 | + # master | ||
32 | + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0) | ||
33 | + mHeight = master.get_height() # 고도 | ||
34 | + # slave | ||
35 | + sPosition = slave.get_opt_flow_position() | ||
36 | + sHeight = slave.get_height() | ||
37 | + print("master [x={} y={} z={}] slave [x={} y={} z={}]" | ||
38 | + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm) | ||
39 | + | ||
40 | + # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드인데 | ||
41 | + # 한 번 성공하고 그 이후로 안 되네요 | ||
42 | + # $pip3 install keyboard | ||
43 | + # 로 깔아야 합니다. | ||
44 | + if keyboard.is_pressed('q'): | ||
45 | + print('드론을 착륙시킵니다.') | ||
46 | + slave.land() | ||
47 | + print('land') | ||
48 | + # slave.emergency_stop() | ||
49 | + # print('emergency_stop') | ||
50 | + break | ||
51 | + | ||
52 | + # master의 전 높이 대비 +- 150의 차이가 있으면 slave가 움직이도록 | ||
53 | + if mHeight > bHeight + 150 or mHeight < bHeight - 150: | ||
54 | + setHeight(mHeight, slave) | ||
55 | + | ||
56 | + bHeight = mHeight | ||
57 | + | ||
58 | + | ||
59 | +if __name__ == '__main__': | ||
60 | + main() |
-
Please register or login to post a comment