이유진

높이, 좌표 동기화

1 +# step 2 : 높이 + 위치조정
2 +import CoDrone
3 +import keyboard
4 +from CoDrone.system import Direction
5 +from CoDrone.system import Mode
6 +
7 +
8 +def stop(_master, _slave):
9 + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
10 + print('[Keyboard input occur: Quit!]')
11 + # 착륙시작시 LED 효과
12 + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
13 + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
14 + return True
15 + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
16 + print('[Slave Disconnected]')
17 + return True
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 + print('slave take off!!!')
30 +
31 + # master의 이전 좌표값(움직임 추적용) 초기화
32 + bHeight = 0
33 + bX = 0
34 + bY = 0
35 +
36 + while True:
37 + master.arm_off()
38 + slave.arm_off()
39 + slave.takeoff()
40 + # master
41 + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
42 + mHeight = master.get_height() # 고도
43 + # slave
44 + sPosition = slave.get_opt_flow_position()
45 + sHeight = slave.get_height()
46 + print("master [x={} y={} z={}] slave [x={} y={} z={}]"
47 + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
48 +
49 + if stop(master, slave):
50 + print('드론을 착륙시킵니다.')
51 + break
52 +
53 + #### step1의 높이 조정 코드 (주석해제하면 됨)
54 + # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록
55 + # if abs(mHeight - bHeight) > heightRange:
56 + # print('[height change!]')
57 + # # LED 효과
58 + # master.arm_strobe()
59 + # slave.arm_strobe()
60 + # setHeight(mHeight, slave)
61 +
62 + # 전진, 후진
63 + if mPosition.X > bX:
64 + slave.go(Direction.FORWARD, 1)
65 + print('[Move Forward]')
66 + elif mPosition.X < bX:
67 + slave.go(Direction.BACKWARD, 1)
68 + print('[Move Backward]')
69 +
70 + # 좌진, 우진
71 + # if mPosition.Y > bY:
72 + # slave.go(Direction.LEFT, 0.4)
73 + # print('[Move Left]')
74 + # elif mPosition.Y < bY:
75 + # slave.go(Direction.RIGHT, 0.4)
76 + # print('[Move Right]')
77 +
78 + # master의 이전 좌표값 저장
79 + bHeight = mHeight
80 + bX = mPosition.X
81 + bY = mPosition.Y
82 + # slave.hover()
83 +
84 + master.arm_off()
85 + slave.arm_off()
86 + print('Land')
87 + slave.land() # 착륙
88 +
89 + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
90 + slave.disconnect()
91 + master.disconnect()
92 +
93 +
94 +if __name__ == '__main__':
95 + main()