Showing
1 changed file
with
127 additions
and
0 deletions
final/teleOperation.py
0 → 100644
| 1 | +# step 2 : 높이 + 위치조정 (master도 날 때) | ||
| 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 = 5 # position (x, y) 변화 인지 거리 | ||
| 12 | +fbRange = 15 # 피드백받을 slave의 높이 범위 | ||
| 13 | + | ||
| 14 | + | ||
| 15 | +def stop(_master, _slave): | ||
| 16 | + # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드 | ||
| 17 | + # $pip3 install keyboard | ||
| 18 | + # 로 keyboard를 설치한 다음 실행해야 합니다. | ||
| 19 | + # 성공 : q를 계속 누르고 계세요! ex) qqqqqqqqqqqqqq | ||
| 20 | + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출 | ||
| 21 | + print('[Keyboard input occur: Quit!]') | ||
| 22 | + # 착륙에 성공할 때까지 LED 효과 | ||
| 23 | + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10) | ||
| 24 | + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10) | ||
| 25 | + return True | ||
| 26 | + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료 | ||
| 27 | + print('[Slave Disconnected]') | ||
| 28 | + return True | ||
| 29 | + | ||
| 30 | + | ||
| 31 | +def setHeight(_master, _mHeight, _slave): | ||
| 32 | + print('목표높이 : ', _mHeight) | ||
| 33 | + while True: | ||
| 34 | + _sHeight = _slave.get_height() | ||
| 35 | + print('현재높이 : ', _sHeight) | ||
| 36 | + | ||
| 37 | + if stop(_master, _slave): | ||
| 38 | + return | ||
| 39 | + | ||
| 40 | + if _mHeight - errorRange <= _sHeight <= _mHeight + errorRange: | ||
| 41 | + print('[hit]') | ||
| 42 | + return | ||
| 43 | + elif _sHeight < _mHeight - errorRange: | ||
| 44 | + _slave.go(Direction.UP) | ||
| 45 | + sleep(0.2) | ||
| 46 | + print('[up]') | ||
| 47 | + elif _sHeight > _mHeight + errorRange: | ||
| 48 | + _slave.go(Direction.DOWN) | ||
| 49 | + sleep(0.2) | ||
| 50 | + print('[down]') | ||
| 51 | + | ||
| 52 | + | ||
| 53 | +def main(): | ||
| 54 | + master = CoDrone.CoDrone() | ||
| 55 | + master.connect("None", "COM5", False) | ||
| 56 | + slave = CoDrone.CoDrone() | ||
| 57 | + slave.connect("None", "COM7", False) | ||
| 58 | + | ||
| 59 | + master.takeoff() | ||
| 60 | + slave.takeoff() | ||
| 61 | + | ||
| 62 | + # master의 이전 좌표값(움직임 추적용) 초기화 | ||
| 63 | + bHeight = 0 | ||
| 64 | + bX = 0 | ||
| 65 | + bY = 0 | ||
| 66 | + | ||
| 67 | + direction = 1 # slave 방향 변화 (1:위/0:아래) | ||
| 68 | + | ||
| 69 | + while True: | ||
| 70 | + master.arm_off() | ||
| 71 | + slave.arm_off() | ||
| 72 | + # master | ||
| 73 | + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0) | ||
| 74 | + mHeight = master.get_height() # 고도 | ||
| 75 | + # slave | ||
| 76 | + sPosition = slave.get_opt_flow_position() | ||
| 77 | + sHeight = slave.get_height() | ||
| 78 | + print("master [x={} y={} z={}] slave [x={} y={} z={}]" | ||
| 79 | + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm) | ||
| 80 | + | ||
| 81 | + if stop(master, slave): | ||
| 82 | + break | ||
| 83 | + | ||
| 84 | + # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록 | ||
| 85 | + if abs(mHeight - bHeight) > heightRange: | ||
| 86 | + print('[height change!]') | ||
| 87 | + # LED 효과 | ||
| 88 | + master.arm_strobe() | ||
| 89 | + slave.arm_strobe() | ||
| 90 | + setHeight(master, mHeight, slave) | ||
| 91 | + | ||
| 92 | + # master의 좌표가 posiRange만큼 차이가 생기면 움직이도록 | ||
| 93 | + if abs(bX - mPosition.X) + abs(bY - mPosition.Y) > posiRange: | ||
| 94 | + print('[position change!]') | ||
| 95 | + # LED 효과 | ||
| 96 | + master.arm_strobe() | ||
| 97 | + slave.arm_strobe() | ||
| 98 | + slave.move(abs(mPosition.Y - bY)/20, abs(bX - mPosition.X)/20, 0, 0) # move(roll 좌우, pitch 전후, yaw = 0, throttle = 0) | ||
| 99 | + | ||
| 100 | + # slave의 전 높이 대비 heightRange 만큼의 차이가 있으면 master가 움직이도록 | ||
| 101 | + if abs(sHeight - sbHeight) > fbRange: | ||
| 102 | + print('[slave feedback!]') | ||
| 103 | + setHeight(slave, sHeight, master) # master의 높이 이동 | ||
| 104 | + | ||
| 105 | + # slave의 이전 좌표값 저장 | ||
| 106 | + sbHeight = sHeight | ||
| 107 | + | ||
| 108 | + # master의 이전 좌표값 저장 | ||
| 109 | + bHeight = mHeight | ||
| 110 | + bX = mPosition.X | ||
| 111 | + bY = mPosition.Y | ||
| 112 | + | ||
| 113 | + print('드론을 착륙시킵니다.') | ||
| 114 | + slave.arm_pattern() # LED 효과 | ||
| 115 | + | ||
| 116 | + print('Land') | ||
| 117 | + slave.land() # 착륙 | ||
| 118 | + master.arm_off() | ||
| 119 | + slave.arm_off() | ||
| 120 | + | ||
| 121 | + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다. | ||
| 122 | + slave.disconnect() | ||
| 123 | + master.disconnect() | ||
| 124 | + | ||
| 125 | + | ||
| 126 | +if __name__ == '__main__': | ||
| 127 | + main() |
-
Please register or login to post a comment