2020-1-capstone-design1 / JSH_Project7

Go to a project
Toggle navigation Toggle navigation pinning
이유진
40e72a1b 1 parent d120b2af

드론 높이 조정

Inline Side-by-side
Showing 2 changed files with 67 additions and 7 deletions
1 +# 드론 높이조정 테스트 코드 (드론 1대용)
2 +import CoDrone
3 +from CoDrone.system import Direction
4 +from time import sleep
5 +
6 +
7 +errorRange = 50 # 오차범위
8 +
9 +
10 +def getPosition(drone):
11 + position = drone.get_opt_flow_position() # 상대좌표 (시작 0,0)
12 + height = drone.get_height() # 고
13 + print("x={} y={} z={}".format(position.X, position.Y, height)) # 단위(mm)
14 +
15 +
16 +def setHeight(_mHeight, _slave):
17 + while True:
18 + _sHeight = _slave.get_height()
19 + if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange:
20 + print('hit')
21 + return
22 + elif _sHeight < _mHeight - errorRange:
23 + _slave.go(Direction.UP)
24 + getPosition(_slave)
25 + sleep(0.2)
26 + print('up')
27 + elif _sHeight > _mHeight + errorRange:
28 + _slave.go(Direction.DOWN)
29 + getPosition(_slave)
30 + sleep(0.2)
31 + print('down')
32 +
33 +
34 +def main():
35 + drone = CoDrone.CoDrone()
36 + drone.connect()
37 +
38 + oHeight = 200 # 목적 높이
39 +
40 + drone.takeoff()
41 + getPosition(drone)
42 + setHeight(oHeight, drone)
43 + getPosition(drone)
44 + drone.hover(3)
45 + print('Land')
46 + drone.land()
47 +
48 +
49 +if __name__ == '__main__':
50 + main()
51 +
52 +
53 +
...@@ -4,17 +4,24 @@ from CoDrone.system import Direction ...@@ -4,17 +4,24 @@ from CoDrone.system import Direction
4 from time import sleep 4 from time import sleep
5 5
6 6
7 +errorRange = 50 # 오차범위
8 +moveRange = 150 # 움직였다고 판단할 거리
9 +
10 +
7 def setHeight(_mHeight, _slave): 11 def setHeight(_mHeight, _slave):
8 while True: 12 while True:
9 _sHeight = _slave.get_height() 13 _sHeight = _slave.get_height()
10 - if _mHeight - 100 <= _sHeight <= _mHeight + 100: 14 + if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange:
15 + print('hit')
11 return 16 return
12 - elif _sHeight < _mHeight - 100: 17 + elif _sHeight < _mHeight - errorRange:
13 _slave.go(Direction.UP) 18 _slave.go(Direction.UP)
14 sleep(0.2) 19 sleep(0.2)
15 - elif _sHeight > _mHeight + 100: 20 + print('up')
21 + elif _sHeight > _mHeight + errorRange:
16 _slave.go(Direction.DOWN) 22 _slave.go(Direction.DOWN)
17 sleep(0.2) 23 sleep(0.2)
24 + print('down')
18 25
19 26
20 def main(): 27 def main():
...@@ -40,7 +47,7 @@ def main(): ...@@ -40,7 +47,7 @@ def main():
40 # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드인데 47 # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드인데
41 # 한 번 성공하고 그 이후로 안 되네요 48 # 한 번 성공하고 그 이후로 안 되네요
42 # $pip3 install keyboard 49 # $pip3 install keyboard
43 - # 로 깔아야 합니다. 50 + # 로 keyboard를 설치한 다음 실행해야 합니다.
44 if keyboard.is_pressed('q'): 51 if keyboard.is_pressed('q'):
45 print('드론을 착륙시킵니다.') 52 print('드론을 착륙시킵니다.')
46 slave.land() 53 slave.land()
...@@ -49,11 +56,11 @@ def main(): ...@@ -49,11 +56,11 @@ def main():
49 # print('emergency_stop') 56 # print('emergency_stop')
50 break 57 break
51 58
52 - # master의 전 높이 대비 +- 150의 차이가 있으면 slave가 움직이도록 59 + # master의 전 높이 대비 +- moveRange 만큼의 차이가 있으면 slave가 움직이도록
53 - if mHeight > bHeight + 150 or mHeight < bHeight - 150: 60 + if mHeight > bHeight + moveRange or mHeight < bHeight - moveRange:
54 setHeight(mHeight, slave) 61 setHeight(mHeight, slave)
55 62
56 - bHeight = mHeight 63 + bHeight = mHeight # 이전 높이 저장
57 64
58 65
59 if __name__ == '__main__': 66 if __name__ == '__main__':
......