Showing
1 changed file
with
60 additions
and
0 deletions
yujin_test/step1.py
0 → 100644
| 1 | +import CoDrone | ||
| 2 | +import keyboard | ||
| 3 | +from CoDrone.system import Direction | ||
| 4 | +from time import sleep | ||
| 5 | + | ||
| 6 | + | ||
| 7 | +def setHeight(_mHeight, _slave): | ||
| 8 | + while True: | ||
| 9 | + _sHeight = _slave.get_height() | ||
| 10 | + if _mHeight - 100 <= _sHeight <= _mHeight + 100: | ||
| 11 | + return | ||
| 12 | + elif _sHeight < _mHeight - 100: | ||
| 13 | + _slave.go(Direction.UP) | ||
| 14 | + sleep(0.2) | ||
| 15 | + elif _sHeight > _mHeight + 100: | ||
| 16 | + _slave.go(Direction.DOWN) | ||
| 17 | + sleep(0.2) | ||
| 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 | + | ||
| 30 | + while True: | ||
| 31 | + # master | ||
| 32 | + mPosition = master.get_opt_flow_position() # 상대좌표 (시작 0,0) | ||
| 33 | + mHeight = master.get_height() # 고도 | ||
| 34 | + # slave | ||
| 35 | + sPosition = slave.get_opt_flow_position() | ||
| 36 | + sHeight = slave.get_height() | ||
| 37 | + print("master [x={} y={} z={}] slave [x={} y={} z={}]" | ||
| 38 | + .format(mPosition.X, mPosition.Y, mHeight, sPosition.X, sPosition.Y, sHeight)) # 좌표출력/단위(mm) | ||
| 39 | + | ||
| 40 | + # 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드인데 | ||
| 41 | + # 한 번 성공하고 그 이후로 안 되네요 | ||
| 42 | + # $pip3 install keyboard | ||
| 43 | + # 로 깔아야 합니다. | ||
| 44 | + if keyboard.is_pressed('q'): | ||
| 45 | + print('드론을 착륙시킵니다.') | ||
| 46 | + slave.land() | ||
| 47 | + print('land') | ||
| 48 | + # slave.emergency_stop() | ||
| 49 | + # print('emergency_stop') | ||
| 50 | + break | ||
| 51 | + | ||
| 52 | + # master의 전 높이 대비 +- 150의 차이가 있으면 slave가 움직이도록 | ||
| 53 | + if mHeight > bHeight + 150 or mHeight < bHeight - 150: | ||
| 54 | + setHeight(mHeight, slave) | ||
| 55 | + | ||
| 56 | + bHeight = mHeight | ||
| 57 | + | ||
| 58 | + | ||
| 59 | +if __name__ == '__main__': | ||
| 60 | + main() |
-
Please register or login to post a comment