Showing
1 changed file
with
95 additions
and
0 deletions
final_code/step2.py
0 → 100644
| 1 | +# step 2 : 높이 + 위치조정 | ||
| 2 | +import CoDrone | ||
| 3 | +import keyboard | ||
| 4 | +from CoDrone.system import Direction | ||
| 5 | +from CoDrone.system import Mode | ||
| 6 | + | ||
| 7 | + | ||
| 8 | +def stop(_master, _slave): | ||
| 9 | + if keyboard.is_pressed('q'): # 키보드에서 'q'가 입력되면 while문 탈출 | ||
| 10 | + print('[Keyboard input occur: Quit!]') | ||
| 11 | + # 착륙시작시 LED 효과 | ||
| 12 | + _master.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10) | ||
| 13 | + _slave.arm_default_pattern(0, 125, 155, Mode.DOUBLE_BLINK, 10) | ||
| 14 | + return True | ||
| 15 | + elif not _slave.isConnected(): # slave 연결이 끊기면 프로그램 종료 | ||
| 16 | + print('[Slave Disconnected]') | ||
| 17 | + return True | ||
| 18 | + | ||
| 19 | + | ||
| 20 | +def main(): | ||
| 21 | + master = CoDrone.CoDrone() | ||
| 22 | + master.connect("None", "COM5", False) | ||
| 23 | + slave = CoDrone.CoDrone() | ||
| 24 | + slave.connect("None", "COM6", False) | ||
| 25 | + | ||
| 26 | + bHeight = master.get_height() # 고도 | ||
| 27 | + if bHeight > 20: # 마스터의 높이가 20이상이면 slave 날기 시작 | ||
| 28 | + slave.takeoff() | ||
| 29 | + print('slave take off!!!') | ||
| 30 | + | ||
| 31 | + # master의 이전 좌표값(움직임 추적용) 초기화 | ||
| 32 | + bHeight = 0 | ||
| 33 | + bX = 0 | ||
| 34 | + bY = 0 | ||
| 35 | + | ||
| 36 | + while True: | ||
| 37 | + master.arm_off() | ||
| 38 | + slave.arm_off() | ||
| 39 | + slave.takeoff() | ||
| 40 | + # master | ||
| 41 | + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0) | ||
| 42 | + mHeight = master.get_height() # 고도 | ||
| 43 | + # slave | ||
| 44 | + sPosition = slave.get_opt_flow_position() | ||
| 45 | + sHeight = slave.get_height() | ||
| 46 | + print("master [x={} y={} z={}] slave [x={} y={} z={}]" | ||
| 47 | + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm) | ||
| 48 | + | ||
| 49 | + if stop(master, slave): | ||
| 50 | + print('드론을 착륙시킵니다.') | ||
| 51 | + break | ||
| 52 | + | ||
| 53 | + #### step1의 높이 조정 코드 (주석해제하면 됨) | ||
| 54 | + # master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록 | ||
| 55 | + # if abs(mHeight - bHeight) > heightRange: | ||
| 56 | + # print('[height change!]') | ||
| 57 | + # # LED 효과 | ||
| 58 | + # master.arm_strobe() | ||
| 59 | + # slave.arm_strobe() | ||
| 60 | + # setHeight(mHeight, slave) | ||
| 61 | + | ||
| 62 | + # 전진, 후진 | ||
| 63 | + if mPosition.X > bX: | ||
| 64 | + slave.go(Direction.FORWARD, 1) | ||
| 65 | + print('[Move Forward]') | ||
| 66 | + elif mPosition.X < bX: | ||
| 67 | + slave.go(Direction.BACKWARD, 1) | ||
| 68 | + print('[Move Backward]') | ||
| 69 | + | ||
| 70 | + # 좌진, 우진 | ||
| 71 | + # if mPosition.Y > bY: | ||
| 72 | + # slave.go(Direction.LEFT, 0.4) | ||
| 73 | + # print('[Move Left]') | ||
| 74 | + # elif mPosition.Y < bY: | ||
| 75 | + # slave.go(Direction.RIGHT, 0.4) | ||
| 76 | + # print('[Move Right]') | ||
| 77 | + | ||
| 78 | + # master의 이전 좌표값 저장 | ||
| 79 | + bHeight = mHeight | ||
| 80 | + bX = mPosition.X | ||
| 81 | + bY = mPosition.Y | ||
| 82 | + # slave.hover() | ||
| 83 | + | ||
| 84 | + master.arm_off() | ||
| 85 | + slave.arm_off() | ||
| 86 | + print('Land') | ||
| 87 | + slave.land() # 착륙 | ||
| 88 | + | ||
| 89 | + # 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다. | ||
| 90 | + slave.disconnect() | ||
| 91 | + master.disconnect() | ||
| 92 | + | ||
| 93 | + | ||
| 94 | +if __name__ == '__main__': | ||
| 95 | + main() |
-
Please register or login to post a comment