이유진

합친거

1 +# step 2 : 높이 + 위치조정 (master도 날 때)
2 +import CoDrone
3 +import keyboard
4 +from CoDrone.system import Direction
5 +from CoDrone.system import Mode
6 +from time import sleep
7 +
8 +
9 +errorRange = 50 # 오차범위
10 +heightRange = 30 # 움직였다고 판단할 거리 height
11 +posiRange = 5 # position (x, y) 변화 인지 거리
12 +fbRange = 15 # 피드백받을 slave의 높이 범위
13 +
14 +
15 +def stop(_master, _slave):
16 + # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드
17 + # $pip3 install keyboard
18 + # 로 keyboard를 설치한 다음 실행해야 합니다.
19 + # 성공 : q를 계속 누르고 계세요! ex) qqqqqqqqqqqqqq
20 + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
21 + print('[Keyboard input occur: Quit!]')
22 + # 착륙에 성공할 때까지 LED 효과
23 + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
24 + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
25 + return True
26 + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
27 + print('[Slave Disconnected]')
28 + return True
29 +
30 +
31 +def setHeight(_master, _mHeight, _slave):
32 + print('목표높이 : ', _mHeight)
33 + while True:
34 + _sHeight = _slave.get_height()
35 + print('현재높이 : ', _sHeight)
36 +
37 + if stop(_master, _slave):
38 + return
39 +
40 + if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange:
41 + print('[hit]')
42 + return
43 + elif _sHeight < _mHeight - errorRange:
44 + _slave.go(Direction.UP)
45 + sleep(0.2)
46 + print('[up]')
47 + elif _sHeight > _mHeight + errorRange:
48 + _slave.go(Direction.DOWN)
49 + sleep(0.2)
50 + print('[down]')
51 +
52 +
53 +def main():
54 + master = CoDrone.CoDrone()
55 + master.connect("None", "COM5", False)
56 + slave = CoDrone.CoDrone()
57 + slave.connect("None", "COM7", False)
58 +
59 + master.takeoff()
60 + slave.takeoff()
61 +
62 + # master의 이전 좌표값(움직임 추적용) 초기화
63 + bHeight = 0
64 + bX = 0
65 + bY = 0
66 +
67 + direction = 1 # slave 방향 변화 (1:위/0:아래)
68 +
69 + while True:
70 + master.arm_off()
71 + slave.arm_off()
72 + # master
73 + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
74 + mHeight = master.get_height() # 고도
75 + # slave
76 + sPosition = slave.get_opt_flow_position()
77 + sHeight = slave.get_height()
78 + print("master [x={} y={} z={}] slave [x={} y={} z={}]"
79 + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
80 +
81 + if stop(master, slave):
82 + break
83 +
84 + # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록
85 + if abs(mHeight - bHeight) > heightRange:
86 + print('[height change!]')
87 + # LED 효과
88 + master.arm_strobe()
89 + slave.arm_strobe()
90 + setHeight(master, mHeight, slave)
91 +
92 + # master의 좌표가 posiRange만큼 차이가 생기면 움직이도록
93 + if abs(bX - mPosition.X) + abs(bY - mPosition.Y) > posiRange:
94 + print('[position change!]')
95 + # LED 효과
96 + master.arm_strobe()
97 + slave.arm_strobe()
98 + slave.move(abs(mPosition.Y - bY)/20, abs(bX - mPosition.X)/20, 0, 0) # move(roll 좌우, pitch 전후, yaw = 0, throttle = 0)
99 +
100 + # slave의 전 높이 대비 heightRange 만큼의 차이가 있으면 master가 움직이도록
101 + if abs(sHeight - sbHeight) > fbRange:
102 + print('[slave feedback!]')
103 + setHeight(slave, sHeight, master) # master의 높이 이동
104 +
105 + # slave의 이전 좌표값 저장
106 + sbHeight = sHeight
107 +
108 + # master의 이전 좌표값 저장
109 + bHeight = mHeight
110 + bX = mPosition.X
111 + bY = mPosition.Y
112 +
113 + print('드론을 착륙시킵니다.')
114 + slave.arm_pattern() # LED 효과
115 +
116 + print('Land')
117 + slave.land() # 착륙
118 + master.arm_off()
119 + slave.arm_off()
120 +
121 + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
122 + slave.disconnect()
123 + master.disconnect()
124 +
125 +
126 +if __name__ == '__main__':
127 + main()