Showing
1 changed file
with
95 additions
and
0 deletions
final_code/step2.py
0 → 100644
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() |
-
Please register or login to post a comment