slave의 피드백

+#step 3 : [feedback] slave드론의 접촉에 의한 반응으로 master드론 움직이기
+import CoDrone
+import keyboard
+from CoDrone.system import Direction
+from CoDrone.system import Mode
+
+errorRange = 50  # 오차범위
+heightRange = 30  # 움직였다고 판단할 거리 height
+posiRange = 10  # position (x, y) 변화 인지 거리
+
+
+# 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드
+def stop(_master, _slave):
+    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)
+        return True
+    elif not _slave.isConnected():  # slave 연결이 끊기면 프로그램 종료
+        print('[Slave Disconnected]')
+        return True
+
+
+def heightFeedback(_slave, _sHeight, _master):
+    while True:
+        if stop(_master, _slave):
+            return
+
+        _mHeight = _master.get_height()
+        if _sHeight - errorRange <= _mHeight <= _sHeight + errorRange:
+            print('[hit]')
+            return
+        elif _mHeight < _sHeight - errorRange:
+            _master.go(Direction.UP)
+            print('[up]')
+        elif _mHeight > _sHeight + errorRange:
+            _master.go(Direction.DOWN)
+            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의 이전 좌표값(움직임 추적용) 초기화
+    bHeight = 0
+   
+
+    while True:
+        master.arm_off()
+        slave.arm_off()
+        # 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 stop(master, slave):
+            print('드론을 착륙시킵니다.')
+            break
+
+        # slave의 전 높이 대비 heightRange 만큼의 차이가 있으면 master가 움직이도록
+        if abs(sHeight - bHeight) > heightRange:
+            print('[Get Feedback!]')
+            # LED 효과
+            master.arm_strobe()
+            slave.arm_strobe()
+            heightFeedback(sHeight, master)# master의 높이 이동
+
+        # slave의 이전 좌표값 저장
+        bHeight = sHeight
+
+    master.arm_off()
+    slave.arm_off()
+    print('Land')
+    slave.land()  # 착륙
+
+    # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
+    slave.disconnect()
+    master.disconnect()
+
+
+if __name__ == '__main__':
+    main()