Toggle navigation
Toggle navigation
This project
Loading...
Sign in
2020-1-capstone-design1
/
JSH_Project7
Go to a project
Toggle navigation
Toggle navigation pinning
Projects
Groups
Snippets
Help
Project
Activity
Repository
Pipelines
Graphs
Issues
0
Merge Requests
0
Wiki
Snippets
Network
Create a new issue
Builds
Commits
Issue Boards
Authored by
이유진
2020-06-19 12:01:06 +0900
Browse Files
Options
Browse Files
Download
Email Patches
Plain Diff
Commit
1eba048ececa0454fc5e69a1ae22e88fc5e03ca6
1eba048e
1 parent
1967e86f
합친거
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
127 additions
and
0 deletions
final/teleOperation.py
final/teleOperation.py
0 → 100644
View file @
1eba048
# step 2 : 높이 + 위치조정 (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
=
5
# position (x, y) 변화 인지 거리
fbRange
=
15
# 피드백받을 slave의 높이 범위
def
stop
(
_master
,
_slave
):
# 키보드로 콘솔창에 'q'를 누르면 드론이 착륙하게 만드는 코드
# $pip3 install keyboard
# 로 keyboard를 설치한 다음 실행해야 합니다.
# 성공 : q를 계속 누르고 계세요! ex) qqqqqqqqqqqqqq
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
setHeight
(
_master
,
_mHeight
,
_slave
):
print
(
'목표높이 : '
,
_mHeight
)
while
True
:
_sHeight
=
_slave
.
get_height
()
print
(
'현재높이 : '
,
_sHeight
)
if
stop
(
_master
,
_slave
):
return
if
_mHeight
-
errorRange
<=
_sHeight
<=
_mHeight
+
errorRange
:
print
(
'[hit]'
)
return
elif
_sHeight
<
_mHeight
-
errorRange
:
_slave
.
go
(
Direction
.
UP
)
sleep
(
0.2
)
print
(
'[up]'
)
elif
_sHeight
>
_mHeight
+
errorRange
:
_slave
.
go
(
Direction
.
DOWN
)
sleep
(
0.2
)
print
(
'[down]'
)
def
main
():
master
=
CoDrone
.
CoDrone
()
master
.
connect
(
"None"
,
"COM5"
,
False
)
slave
=
CoDrone
.
CoDrone
()
slave
.
connect
(
"None"
,
"COM7"
,
False
)
master
.
takeoff
()
slave
.
takeoff
()
# master의 이전 좌표값(움직임 추적용) 초기화
bHeight
=
0
bX
=
0
bY
=
0
direction
=
1
# slave 방향 변화 (1:위/0:아래)
while
True
:
master
.
arm_off
()
slave
.
arm_off
()
# master
mPosition
=
master
.
get_opt_flow_position
()
# 상대좌표 (시작 0,0)
mHeight
=
master
.
get_height
()
# 고도
# slave
sPosition
=
slave
.
get_opt_flow_position
()
sHeight
=
slave
.
get_height
()
print
(
"master [x={} y={} z={}] slave [x={} y={} z={}]"
.
format
(
mPosition
.
X
,
mPosition
.
Y
,
mHeight
,
sPosition
.
X
,
sPosition
.
Y
,
sHeight
))
# 좌표출력/단위(mm)
if
stop
(
master
,
slave
):
break
# master의 전 높이 대비 heightRange 만큼의 차이가 있으면 slave가 움직이도록
if
abs
(
mHeight
-
bHeight
)
>
heightRange
:
print
(
'[height change!]'
)
# LED 효과
master
.
arm_strobe
()
slave
.
arm_strobe
()
setHeight
(
master
,
mHeight
,
slave
)
# master의 좌표가 posiRange만큼 차이가 생기면 움직이도록
if
abs
(
bX
-
mPosition
.
X
)
+
abs
(
bY
-
mPosition
.
Y
)
>
posiRange
:
print
(
'[position change!]'
)
# LED 효과
master
.
arm_strobe
()
slave
.
arm_strobe
()
slave
.
move
(
abs
(
mPosition
.
Y
-
bY
)
/
20
,
abs
(
bX
-
mPosition
.
X
)
/
20
,
0
,
0
)
# move(roll 좌우, pitch 전후, yaw = 0, throttle = 0)
# slave의 전 높이 대비 heightRange 만큼의 차이가 있으면 master가 움직이도록
if
abs
(
sHeight
-
sbHeight
)
>
fbRange
:
print
(
'[slave feedback!]'
)
setHeight
(
slave
,
sHeight
,
master
)
# master의 높이 이동
# slave의 이전 좌표값 저장
sbHeight
=
sHeight
# master의 이전 좌표값 저장
bHeight
=
mHeight
bX
=
mPosition
.
X
bY
=
mPosition
.
Y
print
(
'드론을 착륙시킵니다.'
)
slave
.
arm_pattern
()
# LED 효과
print
(
'Land'
)
slave
.
land
()
# 착륙
master
.
arm_off
()
slave
.
arm_off
()
# 연결해제 -> 여기까지 성공했다면 배터리 안 빼도 다시 연결 됩니다.
slave
.
disconnect
()
master
.
disconnect
()
if
__name__
==
'__main__'
:
main
()
Please
register
or
login
to post a comment