2020-1-capstone-design1 / JSH_Project7

Go to a project
Toggle navigation Toggle navigation pinning
이유진
2481ecf8 1 parent 1eba048e

최종파일 업로드

Inline Side-by-side
Showing 23 changed files with 613 additions and 92 deletions
1 +# Default ignored files
2 +/shelf/
3 +/workspace.xml
4 +# Datasource local storage ignored files
5 +/dataSources/
6 +/dataSources.local.xml
7 +# Editor-based HTTP Client requests
8 +/httpRequests/
1 +<?xml version="1.0" encoding="UTF-8"?>
2 +<module type="PYTHON_MODULE" version="4">
3 + <component name="NewModuleRootManager">
4 + <content url="file://$MODULE_DIR$" />
5 + <orderEntry type="inheritedJdk" />
6 + <orderEntry type="sourceFolder" forTests="false" />
7 + </component>
8 +</module>
...\ No newline at end of file ...\ No newline at end of file
1 +<component name="InspectionProjectProfileManager">
2 + <settings>
3 + <option name="USE_PROJECT_PROFILE" value="false" />
4 + <version value="1.0" />
5 + </settings>
6 +</component>
...\ No newline at end of file ...\ No newline at end of file
1 +<?xml version="1.0" encoding="UTF-8"?>
2 +<project version="4">
3 + <component name="JavaScriptSettings">
4 + <option name="languageLevel" value="ES6" />
5 + </component>
6 + <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
7 +</project>
...\ No newline at end of file ...\ No newline at end of file
1 +<?xml version="1.0" encoding="UTF-8"?>
2 +<project version="4">
3 + <component name="ProjectModuleManager">
4 + <modules>
5 + <module fileurl="file://$PROJECT_DIR$/.idea/JSH_Project7.iml" filepath="$PROJECT_DIR$/.idea/JSH_Project7.iml" />
6 + </modules>
7 + </component>
8 +</project>
...\ No newline at end of file ...\ No newline at end of file
1 +<?xml version="1.0" encoding="UTF-8"?>
2 +<project version="4">
3 + <component name="VcsDirectoryMappings">
4 + <mapping directory="$PROJECT_DIR$" vcs="Git" />
5 + </component>
6 +</project>
...\ No newline at end of file ...\ No newline at end of file
1 +import CoDrone
2 +import keyboard
3 +from CoDrone.system import Direction
4 +from CoDrone.system import Mode
5 +from time import sleep
6 +
7 +
8 +errorRange = 50 # 오차범위
9 +moveRange = 150 # 움직였다고 판단할 거리
10 +heightRange = 70 # 움직였다고 판단할 거리 height
11 +
12 +# 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드
13 +def stop(_master, _slave):
14 + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
15 + print('[Keyboard input occur: Quit!]')
16 + # 착륙에 성공할 때까지 LED 효과
17 + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
18 + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
19 + return True
20 + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
21 + print('[Slave Disconnected]')
22 + return True
23 +
24 +
25 +def setHeight(_master, _mHeight, _slave):
26 + print('목표높이 : ', _mHeight)
27 + while True:
28 + _master.hover()
29 + print('hover')
30 + _sHeight = _slave.get_height()
31 + print('현재높이 : ', _sHeight)
32 +
33 + if stop(_master, _slave):
34 + return
35 +
36 + if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange:
37 + print('[hit]')
38 + return
39 + elif _sHeight < _mHeight - errorRange:
40 + _slave.go(Direction.UP, 0, 100)
41 + # sleep(0.2)
42 + print('[up]')
43 + elif _sHeight > _mHeight + errorRange:
44 + _slave.go(Direction.DOWN, 0, 100)
45 + # sleep(0.2)
46 + print('[down]')
47 +
48 +
49 +def main():
50 + master = CoDrone.CoDrone()
51 + master.connect("None", "COM7", False)
52 + slave = CoDrone.CoDrone()
53 + slave.connect("None", "COM5", False)
54 +
55 + bHeight = master.get_height() # 고도
56 + if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작
57 + slave.takeoff()
58 +
59 +
60 + while True:
61 + master.arm_off()
62 + slave.arm_off()
63 + # master
64 + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
65 + mHeight = master.get_height() # 고도
66 + # slave
67 + sPosition = slave.get_opt_flow_position()
68 + sHeight = slave.get_height()
69 + print("master [x={} y={} z={}] slave [x={} y={} z={}]"
70 + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
71 +
72 + if stop(master, slave):
73 + break
74 +
75 + # master의 전 높이 대비 +- moveRange 만큼의 차이가 있으면 slave가 움직이도록
76 + if abs(mHeight - bHeight) > heightRange:
77 + print('[height change!]')
78 + # LED 효과
79 + master.arm_strobe()
80 + slave.arm_strobe()
81 + setHeight(master, mHeight, slave)
82 +
83 + print('Land')
84 + slave.land() # 착륙
85 + master.arm_off()
86 + slave.arm_off()
87 +
88 + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
89 + slave.disconnect()
90 + master.disconnect()
91 +
92 +
93 +if __name__ == '__main__':
94 + main()
1 +# step 2 : 높이 + 위치조정 (민감버전)
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 +def stop(_master, _slave):
10 + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
11 + print('[Keyboard input occur: Quit!]')
12 + # 착륙에 성공할 때까지 LED 효과
13 + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
14 + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
15 + return True
16 + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
17 + print('[Slave Disconnected]')
18 + return True
19 +
20 +
21 +def main():
22 + master = CoDrone.CoDrone()
23 + master.connect("None", "COM5", False)
24 + slave = CoDrone.CoDrone()
25 + slave.connect("None", "COM6", False)
26 +
27 + bHeight = master.get_height() # 고도
28 + if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작
29 + slave.takeoff()
30 + print('slave take off!!!')
31 +
32 + # master의 이전 좌표값(움직임 추적용) 초기화
33 + bHeight = 0
34 + bX = 0
35 + bY = 0
36 +
37 + while True:
38 + master.arm_off()
39 + slave.arm_off()
40 + slave.takeoff()
41 + # master
42 + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
43 + mHeight = master.get_height() # 고도
44 + # slave
45 + sPosition = slave.get_opt_flow_position()
46 + sHeight = slave.get_height()
47 + print("master [x={} y={} z={}] slave [x={} y={} z={}]"
48 + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
49 +
50 + if stop(master, slave):
51 + break
52 +
53 + # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록
54 + # if abs(mHeight - bHeight) > heightRange:
55 + # print('[height change!]')
56 + # # LED 효과
57 + # master.arm_strobe()
58 + # slave.arm_strobe()
59 + # setHeight(mHeight, slave)
60 +
61 + if mPosition.X > bX:
62 + slave.go(Direction.FORWARD, 1)
63 + print('[Move Forward]')
64 + elif mPosition.X < bX:
65 + slave.go(Direction.BACKWARD, 1)
66 + print('[Move Backward]')
67 +
68 + # elif mPosition.Y > bY:
69 + # slave.go(Direction.LEFT, 0.4)
70 + # print('[Move Left]')
71 + # elif mPosition.Y < bY:
72 + # slave.go(Direction.RIGHT, 0.4)
73 + # print('[Move Right]')
74 +
75 + # master의 이전 좌표값 저장
76 + bHeight = mHeight
77 + bX = mPosition.X
78 + bY = mPosition.Y
79 + # slave.hover()
80 +
81 + print('드론을 착륙시킵니다.')
82 + slave.arm_pattern() # LED 효과
83 +
84 + print('Land')
85 + slave.land() # 착륙
86 + master.arm_off()
87 + slave.arm_off()
88 +
89 + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
90 + slave.disconnect()
91 + master.disconnect()
92 +
93 +
94 +if __name__ == '__main__':
95 + main()
...@@ -33,10 +33,11 @@ def main(): ...@@ -33,10 +33,11 @@ def main():
33 33
34 master.takeoff()#master 드론 이륙 34 master.takeoff()#master 드론 이륙
35 slave.takeoff()#slave 드론 이륙 35 slave.takeoff()#slave 드론 이륙
36 + slave.hover(60)
36 bHeight = slave.get_height() # bHeight = slave의 높이 37 bHeight = slave.get_height() # bHeight = slave의 높이
37 38
38 # slave의 이전 좌표값(움직임 추적용) 초기화 39 # slave의 이전 좌표값(움직임 추적용) 초기화
39 - # bHeight = 0 40 + bHeight = 0
40 41
41 42
42 while True: 43 while True:
......
...@@ -14,7 +14,7 @@ def main(): ...@@ -14,7 +14,7 @@ def main():
14 14
15 drone.takeoff() 15 drone.takeoff()
16 getPosition(drone) 16 getPosition(drone)
17 - drone.move(-5, 4, 0, 0) # move(roll 좌우, pitch 전후, yaw, throttle) 17 + drone.move(5, 0, 0, 0) # move(roll 좌우, pitch 전후, yaw, throttle)
18 # drone.go(Direction.FORWARD, 0.5) 18 # drone.go(Direction.FORWARD, 0.5)
19 # print('forward') 19 # print('forward')
20 # getPosition(drone) 20 # getPosition(drone)
......
1 +import CoDrone
2 +import keyboard
3 +from CoDrone.system import Direction
4 +from CoDrone.system import Mode
5 +from time import sleep
6 +
7 +
8 +errorRange = 50 # 오차범위
9 +moveRange = 150 # 움직였다고 판단할 거리
10 +heightRange = 70 # 움직였다고 판단할 거리 height
11 +
12 +# 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드
13 +def stop(_master, _slave):
14 + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
15 + print('[Keyboard input occur: Quit!]')
16 + # 착륙에 성공할 때까지 LED 효과
17 + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
18 + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
19 + return True
20 + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
21 + print('[Slave Disconnected]')
22 + return True
23 +
24 +
25 +def setHeight(_master, _mHeight, _slave):
26 + print('목표높이 : ', _mHeight)
27 + while True:
28 + _master.hover()
29 + print('hover')
30 + _sHeight = _slave.get_height()
31 + print('현재높이 : ', _sHeight)
32 +
33 + if stop(_master, _slave):
34 + return
35 +
36 + if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange:
37 + print('[hit]')
38 + return
39 + elif _sHeight < _mHeight - errorRange:
40 + _slave.go(Direction.UP, 0, 100)
41 + # sleep(0.2)
42 + print('[up]')
43 + elif _sHeight > _mHeight + errorRange:
44 + _slave.go(Direction.DOWN, 0, 100)
45 + # sleep(0.2)
46 + print('[down]')
47 +
48 +
49 +def main():
50 + master = CoDrone.CoDrone()
51 + master.connect("None", "COM7", False)
52 + slave = CoDrone.CoDrone()
53 + slave.connect("None", "COM5", False)
54 +
55 + bHeight = master.get_height() # 고도
56 + if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작
57 + slave.takeoff()
58 +
59 +
60 + while True:
61 + master.arm_off()
62 + slave.arm_off()
63 + # master
64 + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
65 + mHeight = master.get_height() # 고도
66 + # slave
67 + sPosition = slave.get_opt_flow_position()
68 + sHeight = slave.get_height()
69 + print("master [x={} y={} z={}] slave [x={} y={} z={}]"
70 + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
71 +
72 + if stop(master, slave):
73 + break
74 +
75 + # master의 전 높이 대비 +- moveRange 만큼의 차이가 있으면 slave가 움직이도록
76 + if abs(mHeight - bHeight) > heightRange:
77 + print('[height change!]')
78 + # LED 효과
79 + master.arm_strobe()
80 + slave.arm_strobe()
81 + setHeight(master, mHeight, slave)
82 +
83 + print('Land')
84 + slave.land() # 착륙
85 + master.arm_off()
86 + slave.arm_off()
87 +
88 + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
89 + slave.disconnect()
90 + master.disconnect()
91 +
92 +
93 +if __name__ == '__main__':
94 + main()
1 -# step 2 : 높이 + 위치조정 1 +# step 2 : 높이 + 위치조정 (master도 날 때)
2 import CoDrone 2 import CoDrone
3 import keyboard 3 import keyboard
4 from CoDrone.system import Direction 4 from CoDrone.system import Direction
...@@ -11,9 +11,31 @@ heightRange = 30 # 움직였다고 판단할 거리 height ...@@ -11,9 +11,31 @@ heightRange = 30 # 움직였다고 판단할 거리 height
11 posiRange = 5 # position (x, y) 변화 인지 거리 11 posiRange = 5 # position (x, y) 변화 인지 거리
12 12
13 13
14 -def setHeight(_mHeight, _slave): 14 +def stop(_master, _slave):
15 + # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드
16 + # $pip3 install keyboard
17 + # 로 keyboard를 설치한 다음 실행해야 합니다.
18 + # 성공 : q를 계속 누르고 계세요! ex) qqqqqqqqqqqqqq
19 + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
20 + print('[Keyboard input occur: Quit!]')
21 + # 착륙에 성공할 때까지 LED 효과
22 + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
23 + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
24 + return True
25 + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
26 + print('[Slave Disconnected]')
27 + return True
28 +
29 +
30 +def setHeight(_master, _mHeight, _slave):
31 + print('목표높이 : ', _mHeight)
15 while True: 32 while True:
16 _sHeight = _slave.get_height() 33 _sHeight = _slave.get_height()
34 + print('현재높이 : ', _sHeight)
35 +
36 + if stop(_master, _slave):
37 + return
38 +
17 if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange: 39 if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange:
18 print('[hit]') 40 print('[hit]')
19 return 41 return
...@@ -31,8 +53,9 @@ def main(): ...@@ -31,8 +53,9 @@ def main():
31 master = CoDrone.CoDrone() 53 master = CoDrone.CoDrone()
32 master.connect("None", "COM5", False) 54 master.connect("None", "COM5", False)
33 slave = CoDrone.CoDrone() 55 slave = CoDrone.CoDrone()
34 - slave.connect("None", "COM6", False) 56 + slave.connect("None", "COM7", False)
35 57
58 + master.takeoff()
36 bHeight = master.get_height() # 고도 59 bHeight = master.get_height() # 고도
37 if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작 60 if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작
38 slave.takeoff() 61 slave.takeoff()
...@@ -54,19 +77,8 @@ def main(): ...@@ -54,19 +77,8 @@ def main():
54 print("master [x={} y={} z={}] slave [x={} y={} z={}]" 77 print("master [x={} y={} z={}] slave [x={} y={} z={}]"
55 .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm) 78 .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
56 79
57 - # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드 80 + if stop(master, slave):
58 - # $pip3 install keyboard
59 - # 로 keyboard를 설치한 다음 실행해야 합니다.
60 - # 성공 : q를 계속 누르고 계세요! ex) qqqqqqqqqqqqqq
61 - if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
62 - print('[Keyboard input occur: Quit!]')
63 - # 착륙에 성공할 때까지 LED 효과
64 - master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
65 - slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
66 break 81 break
67 - elif not slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
68 - print('[Slave Disconnected]')
69 - return
70 82
71 # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록 83 # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록
72 if abs(mHeight - bHeight) > heightRange: 84 if abs(mHeight - bHeight) > heightRange:
...@@ -74,7 +86,7 @@ def main(): ...@@ -74,7 +86,7 @@ def main():
74 # LED 효과 86 # LED 효과
75 master.arm_strobe() 87 master.arm_strobe()
76 slave.arm_strobe() 88 slave.arm_strobe()
77 - setHeight(mHeight, slave) 89 + setHeight(master, mHeight, slave)
78 90
79 # master의 좌표가 posiRange만큼 차이가 생기면 움직이도록 91 # master의 좌표가 posiRange만큼 차이가 생기면 움직이도록
80 if abs(bX - mPosition.X) + abs(bY - mPosition.Y) > posiRange: 92 if abs(bX - mPosition.X) + abs(bY - mPosition.Y) > posiRange:
...@@ -82,7 +94,7 @@ def main(): ...@@ -82,7 +94,7 @@ def main():
82 # LED 효과 94 # LED 효과
83 master.arm_strobe() 95 master.arm_strobe()
84 slave.arm_strobe() 96 slave.arm_strobe()
85 - slave.move(mPosition.Y - bY, bX - mPosition.X, 0, 0) # move(roll 좌우, pitch 전후, yaw = 0, throttle = 0) 97 + slave.move(abs(mPosition.Y - bY)/20, abs(bX - mPosition.X)/20, 0, 0) # move(roll 좌우, pitch 전후, yaw = 0, throttle = 0)
86 98
87 # master의 이전 좌표값 저장 99 # master의 이전 좌표값 저장
88 bHeight = mHeight 100 bHeight = mHeight
......
1 +# step 2 : 높이 + 위치조정 (민감버전)
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 +def stop(_master, _slave):
10 + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
11 + print('[Keyboard input occur: Quit!]')
12 + # 착륙에 성공할 때까지 LED 효과
13 + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
14 + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
15 + return True
16 + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
17 + print('[Slave Disconnected]')
18 + return True
19 +
20 +
21 +def main():
22 + master = CoDrone.CoDrone()
23 + master.connect("None", "COM5", False)
24 + slave = CoDrone.CoDrone()
25 + slave.connect("None", "COM6", False)
26 +
27 + bHeight = master.get_height() # 고도
28 + if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작
29 + slave.takeoff()
30 + print('slave take off!!!')
31 +
32 + # master의 이전 좌표값(움직임 추적용) 초기화
33 + bHeight = 0
34 + bX = 0
35 + bY = 0
36 +
37 + while True:
38 + master.arm_off()
39 + slave.arm_off()
40 + slave.takeoff()
41 + # master
42 + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
43 + mHeight = master.get_height() # 고도
44 + # slave
45 + sPosition = slave.get_opt_flow_position()
46 + sHeight = slave.get_height()
47 + print("master [x={} y={} z={}] slave [x={} y={} z={}]"
48 + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
49 +
50 + if stop(master, slave):
51 + break
52 +
53 + # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록
54 + # if abs(mHeight - bHeight) > heightRange:
55 + # print('[height change!]')
56 + # # LED 효과
57 + # master.arm_strobe()
58 + # slave.arm_strobe()
59 + # setHeight(mHeight, slave)
60 +
61 + if mPosition.X > bX:
62 + slave.go(Direction.FORWARD, 1)
63 + print('[Move Forward]')
64 + elif mPosition.X < bX:
65 + slave.go(Direction.BACKWARD, 1)
66 + print('[Move Backward]')
67 +
68 + # elif mPosition.Y > bY:
69 + # slave.go(Direction.LEFT, 0.4)
70 + # print('[Move Left]')
71 + # elif mPosition.Y < bY:
72 + # slave.go(Direction.RIGHT, 0.4)
73 + # print('[Move Right]')
74 +
75 + # master의 이전 좌표값 저장
76 + bHeight = mHeight
77 + bX = mPosition.X
78 + bY = mPosition.Y
79 + # slave.hover()
80 +
81 + print('드론을 착륙시킵니다.')
82 + slave.arm_pattern() # LED 효과
83 +
84 + print('Land')
85 + slave.land() # 착륙
86 + master.arm_off()
87 + slave.arm_off()
88 +
89 + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
90 + slave.disconnect()
91 + master.disconnect()
92 +
93 +
94 +if __name__ == '__main__':
95 + main()
1 +# step 2 : 높이 + 위치조정
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 = 20 # position (x, y) 변화 인지 거리
12 +posiErrorRange = 10 # position 오차범위
13 +
14 +# 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드
15 +def stop(_master, _slave):
16 + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
17 + print('[Keyboard input occur: Quit!]')
18 + # 착륙에 성공할 때까지 LED 효과
19 + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
20 + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
21 + return True
22 + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
23 + print('[Slave Disconnected]')
24 + return True
25 +
26 +
27 +def setHeight(_mHeight, _slave):
28 + while True:
29 + _sHeight = _slave.get_height()
30 + if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange:
31 + print('[hit]')
32 + return
33 + elif _sHeight < _mHeight - errorRange:
34 + _slave.go(Direction.UP)
35 + sleep(0.2)
36 + print('[up]')
37 + elif _sHeight > _mHeight + errorRange:
38 + _slave.go(Direction.DOWN)
39 + sleep(0.2)
40 + print('[down]')
41 +
42 +
43 +def setPosition(_master, _tox, _toy, _slave):
44 + print('목표좌표 : x:{} y:{}'.format(_tox, _toy))
45 + # x좌표 먼저 (눈 방향)
46 + print('(1) X 좌표 맞추기')
47 + while True:
48 + if stop(_master, _slave):
49 + return
50 +
51 + position = _slave.get_opt_flow_position()
52 + print("x={} y={}".format(position.X, position.Y)) # 단위(mm)
53 + if _tox - posiErrorRange <= position.X <= _tox + posiErrorRange:
54 + print('[hit]')
55 + break
56 + elif position.X < _tox - posiErrorRange:
57 + _slave.go(Direction.BACKWARD)
58 + print('[Move Backward]')
59 + elif position.X > _tox + posiErrorRange:
60 + _slave.go(Direction.FORWARD)
61 + print('[Move Forward]')
62 + # y 좌표
63 + print('(2) Y 좌표 맞추기')
64 + while True:
65 + if stop(_master, _slave):
66 + return
67 +
68 + position = _slave.get_opt_flow_position()
69 + print("x={} y={}".format(position.X, position.Y)) # 단위(mm)
70 + if _toy - posiErrorRange <= position.Y <= _toy + posiErrorRange:
71 + print('[hit]')
72 + print('!!! slave move success !!!')
73 + return
74 + elif position.Y < _toy - posiErrorRange:
75 + _slave.go(Direction.RIGHT)
76 + print('[Move right]')
77 + elif position.Y > _toy + posiErrorRange:
78 + _slave.go(Direction.LEFT)
79 + print('[Move left]')
80 +
81 +
82 +def main():
83 + master = CoDrone.CoDrone()
84 + master.connect("None", "COM5", False)
85 + slave = CoDrone.CoDrone()
86 + slave.connect("None", "COM6", False)
87 +
88 + bHeight = master.get_height() # 고도
89 + if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작
90 + slave.takeoff()
91 + print('slave take off!!!')
92 +
93 + # master의 이전 좌표값(움직임 추적용) 초기화
94 + bHeight = 0
95 + bX = 0
96 + bY = 0
97 +
98 + while True:
99 + master.arm_off()
100 + slave.arm_off()
101 + slave.takeoff()
102 + # master
103 + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
104 + mHeight = master.get_height() # 고도
105 + # slave
106 + sPosition = slave.get_opt_flow_position()
107 + sHeight = slave.get_height()
108 + print("master [x={} y={} z={}] slave [x={} y={} z={}]"
109 + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
110 +
111 + stop(master, slave)
112 +
113 + # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록
114 + # if abs(mHeight - bHeight) > heightRange:
115 + # print('[height change!]')
116 + # # LED 효과
117 + # master.arm_strobe()
118 + # slave.arm_strobe()
119 + # setHeight(mHeight, slave)
120 +
121 + # master의 좌표가 posiRange만큼 차이가 생기면 움직이도록
122 + if abs(bX - mPosition.X) + abs(bY - mPosition.Y) > posiRange:
123 + print('[position change!]')
124 + # LED 효과
125 + master.arm_strobe()
126 + slave.arm_strobe()
127 + setPosition(master, mPosition.X, mPosition.Y, slave)
128 +
129 +
130 + # master의 이전 좌표값 저장
131 + bHeight = mHeight
132 + bX = mPosition.X
133 + bY = mPosition.Y
134 + slave.hover()
135 +
136 + print('드론을 착륙시킵니다.')
137 + slave.arm_pattern() # LED 효과
138 +
139 + print('Land')
140 + slave.land() # 착륙
141 + master.arm_off()
142 + slave.arm_off()
143 +
144 + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
145 + slave.disconnect()
146 + master.disconnect()
147 +
148 +
149 +if __name__ == '__main__':
150 + main()
...@@ -6,8 +6,8 @@ from CoDrone.system import Mode ...@@ -6,8 +6,8 @@ from CoDrone.system import Mode
6 from time import sleep 6 from time import sleep
7 7
8 8
9 -errorRange = 50 # 오차범위 9 +errorRange = 100 # 오차범위
10 -heightRange = 30 # 움직였다고 판단할 거리 height 10 +heightRange = 70 # 움직였다고 판단할 거리 height
11 posiRange = 5 # position (x, y) 변화 인지 거리 11 posiRange = 5 # position (x, y) 변화 인지 거리
12 fbRange = 15 # 피드백받을 slave의 높이 범위 12 fbRange = 15 # 피드백받을 slave의 높이 범위
13 13
...@@ -31,6 +31,8 @@ def stop(_master, _slave): ...@@ -31,6 +31,8 @@ def stop(_master, _slave):
31 def setHeight(_master, _mHeight, _slave): 31 def setHeight(_master, _mHeight, _slave):
32 print('목표높이 : ', _mHeight) 32 print('목표높이 : ', _mHeight)
33 while True: 33 while True:
34 + _master.hover()
35 + print('hover')
34 _sHeight = _slave.get_height() 36 _sHeight = _slave.get_height()
35 print('현재높이 : ', _sHeight) 37 print('현재높이 : ', _sHeight)
36 38
...@@ -61,6 +63,7 @@ def main(): ...@@ -61,6 +63,7 @@ def main():
61 63
62 # master의 이전 좌표값(움직임 추적용) 초기화 64 # master의 이전 좌표값(움직임 추적용) 초기화
63 bHeight = 0 65 bHeight = 0
66 + sbHeight = 0
64 bX = 0 67 bX = 0
65 bY = 0 68 bY = 0
66 69
...@@ -81,6 +84,10 @@ def main(): ...@@ -81,6 +84,10 @@ def main():
81 if stop(master, slave): 84 if stop(master, slave):
82 break 85 break
83 86
87 + # slave의 이전 좌표값 저장
88 + sbHeight = sHeight
89 +
90 +
84 # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록 91 # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록
85 if abs(mHeight - bHeight) > heightRange: 92 if abs(mHeight - bHeight) > heightRange:
86 print('[height change!]') 93 print('[height change!]')
...@@ -101,10 +108,6 @@ def main(): ...@@ -101,10 +108,6 @@ def main():
101 if abs(sHeight - sbHeight) > fbRange: 108 if abs(sHeight - sbHeight) > fbRange:
102 print('[slave feedback!]') 109 print('[slave feedback!]')
103 setHeight(slave, sHeight, master) # master의 높이 이동 110 setHeight(slave, sHeight, master) # master의 높이 이동
104 -
105 - # slave의 이전 좌표값 저장
106 - sbHeight = sHeight
107 -
108 # master의 이전 좌표값 저장 111 # master의 이전 좌표값 저장
109 bHeight = mHeight 112 bHeight = mHeight
110 bX = mPosition.X 113 bX = mPosition.X
...@@ -114,6 +117,7 @@ def main(): ...@@ -114,6 +117,7 @@ def main():
114 slave.arm_pattern() # LED 효과 117 slave.arm_pattern() # LED 효과
115 118
116 print('Land') 119 print('Land')
120 + master.land()
117 slave.land() # 착륙 121 slave.land() # 착륙
118 master.arm_off() 122 master.arm_off()
119 slave.arm_off() 123 slave.arm_off()
......
1 -import CoDrone
2 -import keyboard
3 -from CoDrone.system import Direction
4 -from time import sleep
5 -
6 -
7 -errorRange = 50 # 오차범위
8 -moveRange = 150 # 움직였다고 판단할 거리
9 -
10 -
11 -def setHeight(_mHeight, _slave):
12 - while True:
13 - _sHeight = _slave.get_height()
14 - if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange:
15 - print('hit')
16 - return
17 - elif _sHeight < _mHeight - errorRange:
18 - _slave.go(Direction.UP)
19 - sleep(0.2)
20 - print('up')
21 - elif _sHeight > _mHeight + errorRange:
22 - _slave.go(Direction.DOWN)
23 - sleep(0.2)
24 - print('down')
25 -
26 -
27 -def main():
28 - master = CoDrone.CoDrone()
29 - master.connect("None", "COM5", False)
30 - slave = CoDrone.CoDrone()
31 - slave.connect("None", "COM6", False)
32 -
33 - bHeight = master.get_height() # 고도
34 - if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작
35 - slave.takeoff()
36 -
37 - while True:
38 - # master
39 - mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
40 - mHeight = master.get_height() # 고도
41 - # slave
42 - sPosition = slave.get_opt_flow_position()
43 - sHeight = slave.get_height()
44 - print("master [x={} y={} z={}] slave [x={} y={} z={}]"
45 - .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm)
46 -
47 - # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드인데
48 - # 한 번 성공하고 그 이후로 안 되네요
49 - # $pip3 install keyboard
50 - # 로 keyboard를 설치한 다음 실행해야 합니다.
51 - if keyboard.is_pressed('q'):
52 - print('드론을 착륙시킵니다.')
53 - slave.land()
54 - print('land')
55 - # slave.emergency_stop()
56 - # print('emergency_stop')
57 - break
58 -
59 - # master의 전 높이 대비 +- moveRange 만큼의 차이가 있으면 slave가 움직이도록
60 - if mHeight > bHeight + moveRange or mHeight < bHeight - moveRange:
61 - setHeight(mHeight, slave)
62 -
63 - bHeight = mHeight # 이전 높이 저장
64 -
65 -
66 -if __name__ == '__main__':
67 - main()