step3.py
2.52 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
#step 3 : slave드론의 접촉에 의한 반응으로 master드론 움직이기
import CoDrone
import keyboard
from CoDrone.system import Direction
from CoDrone.system import Mode
from time import sleep
errorRange = 50 # 오차범위
heightRange = 30 # 움직였다고 판단할 거리 height
posiRange = 10 # position (x, y) 변화 인지 거리
def heightFeedback(_sHeight, _master):
while True:
_mHeight = _master.get_height()
if _sHeight - errorRange <= _mHeight <= _sHeight + errorRange:
print('[hit]')
return
elif _mHeight < _sHeight - errorRange:
_master.go(Direction.UP)
sleep(0.2)
print('[up]')
elif _mHeight > _sHeight + errorRange:
_master.go(Direction.DOWN)
sleep(0.2)
print('[down]')
def main():
master = CoDrone.CoDrone()
master.connect("None", "COM7", False)
slave = CoDrone.CoDrone()
slave.connect("None", "COM5", False)
master.takeoff()#master 드론 이륙
slave.takeoff()#slave 드론 이륙
slave.hover(60)
bHeight = slave.get_height() # bHeight = slave의 높이
# slave의 이전 좌표값(움직임 추적용) 초기화
bHeight = 0
while True:
# master
mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0)
mHeight = master.get_height() # master고도
# slave
sPosition = slave.get_opt_flow_position()
sHeight = slave.get_height() # slave고도
print("master [x={} y={} z={}] slave [x={} y={} z={}]"
.format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight))
if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출
print('[Keyboard input occur: Quit!]')
# 착륙에 성공할 때까지 LED 효과
master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10)
break
elif not slave.isConnected(): # slave 연결이 끊기면 프로그램 종료
print('[Slave Disconnected]')
return
# slave의 전 높이 대비 heightRange 만큼의 차이가 있으면 master가 움직이도록
if abs(sHeight - bHeight) > heightRange:
print('[height change!]')
heightFeedback(sHeight, master)# master의 높이 이동
# slave의 이전 좌표값 저장
bHeight = sHeight
if __name__ == '__main__':
main()