Toggle navigation
Toggle navigation
This project
Loading...
Sign in
2021-1-capstone-design2
/
2014104077
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
Wonseok Kwak
2021-06-12 20:15:22 +0900
Browse Files
Options
Browse Files
Download
Email Patches
Plain Diff
Commit
b686682f8cb8c5219514f6f63e672693dc2d9065
b686682f
1 parent
95c6610a
update alpha 1.1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
29 additions
and
43 deletions
source/DroneControl/capstonedesign2/capstonedesign2.ino
source/DroneControl/capstonedesign2/capstonedesign2.ino
View file @
b686682
#include <CoDrone.h> // 코드론 라이브러리
#define ShockLed 12 // 충격 상태 확인을 위한 LED 핀번호
#define buzzer 7 // 피에조 버저 핀번호
#define LeftVib 27 // 왼쪽 진동자 핀번호
#define RightVib 28 // 오른쪽 진동자 핀번호
...
...
@@ -55,9 +56,9 @@ void accel_calculate()
*/
// Data SHIFT From CoDrone Accelerometer
ac_x
=
CoDrone
.
getAccelerometer
().
x
;
ac_y
=
CoDrone
.
getAccelerometer
().
y
;
ac_z
=
CoDrone
.
getAccelerometer
().
z
;
ac_x
=
CoDrone
.
ImuAccX
/
2
;
ac_y
=
CoDrone
.
ImuAccY
/
2
;
ac_z
=
CoDrone
.
ImuAccZ
/
2
;
//맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
normal_x
=
map
(
int
(
ac_x
),
-
16384
,
16384
,
-
5000
,
mapping_value
);
...
...
@@ -76,17 +77,6 @@ void accel_calculate()
void
Emergency_state_
()
{
digitalWrite
(
ShockLed
,
HIGH
);
// 릴레이핀을 True값으로 바꿔 13번 핀의 LED를 ON시킨다.
Serial
.
print
(
"SHOCK level : "
);
Serial
.
println
(
shock_level
);
// Serial 모니터로 확인
Serial
.
print
(
" x: "
);
Serial
.
print
(
deltha_x
[
0
]);
Serial
.
print
(
" y: "
);
Serial
.
print
(
deltha_y
[
0
]);
Serial
.
print
(
" z: "
);
Serial
.
println
(
deltha_z
[
0
]);
/*Serial.print(" deltha_1 : "); Serial.print(deltha_x[1]);
Serial.print(" deltha_2 : "); Serial.println(deltha_x[2]);*/
}
// 충격 감지함수
...
...
@@ -168,6 +158,9 @@ void Shock_Sensing()
// 진동함수
void
vibrate
()
{
// if 충격값이 왼쪽인지 오른쪽인지 대소비교로 판단
// 판단되면 그위치에 나눈것만큼의 진동을 나눠서 크기별로 부여
// analogWrite();
}
...
...
@@ -176,10 +169,15 @@ void vibrate()
// Main 함수부
void
setup
()
{
// 부속 하드웨어 제어
pinMode
(
LeftVib
,
OUTPUT
);
pinMode
(
RightVib
,
OUTPUT
);
pinMode
(
ShockLed
,
OUTPUT
);
// 핀모드사용, led로 육안확인 가능하게끔
pinMode
(
buzzer
,
OUTPUT
);
digitalWrite
(
ShockLed
,
LOW
);
// HIGH - LED on, LOW - LED off (DEFAULT : OFF)
digitalWrite
(
buzzer
,
LOW
);
CoDrone
.
begin
(
115200
);
// 드론 플러그의 통신 개시 (115200bps)
CoDrone
.
AutoConnect
(
NearbyDrone
);
// 가장 가까운 위치의 드론과 연결
CoDrone
.
DroneModeChange
(
Flight
);
// 드론을 플라이트 모드로 설정
...
...
@@ -192,47 +190,35 @@ void loop()
byte
bt1
=
digitalRead
(
11
);
// ■ □ □ □ □ □ □ 밑면 적외선 센서를 입력으로 사용
byte
bt4
=
digitalRead
(
14
);
// □ □ □ ■ □ □ □ 밑면 적외선 센서를 입력으로 사용
byte
bt8
=
digitalRead
(
18
);
// □ □ □ □ □ □ ■ 밑면 적외선 센서를 입력으로 사용
if
(
bt1
&&
!
bt4
&&
!
bt8
)
// 밑면 센서 가장 끝 11번 센서에 손을 대면 실행합니다.
{
CoDrone
.
FlightEvent
(
Stop
);
// 드론을 정지시킵니다.
CoDrone
.
FlightEvent
(
Stop
);
// 드론을 정지시킵니다.
}
if
(
!
bt1
&&
!
bt4
&&
bt8
)
// 밑면 센서 가장 끝 18번 센서에 손을 대면 실행합니다.
if
(
!
bt1
&&
!
bt4
&&
bt8
)
// 밑면 센서 가장 끝 18번 센서에 손을 대면 실행합니다.
{
CoDrone
.
FlightEvent
(
Landing
);
// 드론을 착륙시킵니다.
CoDrone
.
FlightEvent
(
Landing
);
// 드론을 착륙시킵니다.
}
Shock_Sensing
();
if
(
State_Parameter
==
true
){
shock_sum
+=
deltha
;
//currentMillis = millis();
}
if
(
State_Parameter
==
true
)
{
shock_sum
+=
deltha
;
//currentMillis = millis();
}
if
(
State_Parameter
==
false
&&
shock_sum
!=
0
){
if
(
State_Parameter
==
false
&&
shock_sum
!=
0
)
{
//bluetooth.write(char_maker(shock_sum)); // 블루투스로 충격정도 전송 (shock_maker 라는 char 리턴함수를 통해 진동강도 전송)
Serial
.
print
(
"누적 충격값 : "
);
Serial
.
println
(
shock_sum
);
// 충격 누적값 전송
shock_sum
=
0
;
// 충격의 지속이 끝났으니 원래값인 0으로 초기화
}
// 연결(페어링)이 성공한 경우에만 실행
if
(
PAIRING
==
true
)
if
(
PAIRING
==
true
)
// 연결(페어링)이 성공한 경우에만 실행
{
YAW
=
-
1
*
CoDrone
.
AnalogScaleChange
(
analogRead
(
A3
));
// 아날로그 3번 핀의 값을 YAW 값으로 사용합니다. - 좌우회전
THROTTLE
=
CoDrone
.
AnalogScaleChange
(
analogRead
(
A4
));
// 아날로그 4번 핀의 값을 THROTTLE 값으로 사용합니다. - 승하강
ROLL
=
-
1
*
CoDrone
.
AnalogScaleChange
(
analogRead
(
A5
));
// 아날로그 5번 핀의 값을 ROLL 값으로 사용합니다. - 좌우이동
PITCH
=
CoDrone
.
AnalogScaleChange
(
analogRead
(
A6
));
// 아날로그 6번 핀의 값을 PITCH 값으로 사용합니다. - 전후진
CoDrone
.
Control
(
SEND_INTERVAL
);
// 제어 신호를 보냅니다. 통신이 안정하게 가도록 시간을 두고 보냄 (최소 50ms)
YAW
=
-
1
*
CoDrone
.
AnalogScaleChange
(
analogRead
(
A3
));
// 아날로그 3번 핀의 값을 YAW 값으로 사용합니다. - 좌우회전
THROTTLE
=
CoDrone
.
AnalogScaleChange
(
analogRead
(
A4
));
// 아날로그 4번 핀의 값을 THROTTLE 값으로 사용합니다. - 승하강
ROLL
=
-
1
*
CoDrone
.
AnalogScaleChange
(
analogRead
(
A5
));
// 아날로그 5번 핀의 값을 ROLL 값으로 사용합니다. - 좌우이동
PITCH
=
CoDrone
.
AnalogScaleChange
(
analogRead
(
A6
));
// 아날로그 6번 핀의 값을 PITCH 값으로 사용합니다. - 전후진
CoDrone
.
Control
(
SEND_INTERVAL
);
// 제어 신호를 보냅니다. 통신이 안정하게 가도록 시간을 두고 보냄 (최소 50ms)
}
}
...
...
Please
register
or
login
to post a comment