이유진

step1

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()