Wonseok Kwak

update alpha 1.3

1 1
2 #include <CoDrone.h> // 코드론 라이브러리 2 #include <CoDrone.h> // 코드론 라이브러리
3 -#include <math.h>
4 #define ShockLed 12 // 충격 상태 확인을 위한 LED 핀번호 3 #define ShockLed 12 // 충격 상태 확인을 위한 LED 핀번호
5 #define buzzer 7 // 피에조 버저 핀번호 4 #define buzzer 7 // 피에조 버저 핀번호
6 #define LeftVib 27 // 왼쪽 진동자 핀번호 5 #define LeftVib 27 // 왼쪽 진동자 핀번호
...@@ -10,15 +9,10 @@ ...@@ -10,15 +9,10 @@
10 long int ac_x, ac_y, ac_z; 9 long int ac_x, ac_y, ac_z;
11 long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시 10 long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시
12 const int mapping_value = 5000; 11 const int mapping_value = 5000;
13 -const int Emergency_value = 10000; // 충격상태로 판단하는 값 12 +const int Emergency_value = 300; // 충격상태로 판단하는 값
14 -const int Emergency_angle = 25; // 충격상태로 판단하는 각도(기울기)
15 -const int Emergency_value2 = 1200;// 충격상태로 판단하는 값
16 -const int Emergency_angle2 = 9; // 충격상태로 판단하는 각도(기울기)
17 boolean State_Parameter = false; // 충격상태 판단 13 boolean State_Parameter = false; // 충격상태 판단
18 -unsigned short int shock_level; // 충격정도 상,중,하 char로 받을 생각 14 +long int shock_level;
19 const long int sum_count = 4; // 평균 내는 횟수 15 const long int sum_count = 4; // 평균 내는 횟수
20 -const long interval = 50; // 충격을 감지후 다음 충격을 감지하는 최소 간극 시간 (ms)
21 -unsigned long previousMillis = 0; // 최초 시작시간
22 long shock_sum = 0; // 누적 충격값 16 long shock_sum = 0; // 누적 충격값
23 17
24 //사용자지정 함수부 18 //사용자지정 함수부
...@@ -44,29 +38,37 @@ void accel_calculate() ...@@ -44,29 +38,37 @@ void accel_calculate()
44 38
45 // Data SHIFT From CoDrone Accelerometer 39 // Data SHIFT From CoDrone Accelerometer
46 acceldata accel; 40 acceldata accel;
47 - accel = CoDrone.getAccelerometer(); 41 + accel = CoDrone.getAccelerometer(); // CoDrone에서 가속도 전달받음.
48 - ac_x = accel.x; 42 + ac_x = accel.x; // 넘겨 받은 x축 raw data
49 - ac_y = accel.y; 43 + ac_y = accel.y; // 넘겨 받은 y축 raw data
50 - ac_z = accel.z; 44 + ac_z = accel.z; // 넘겨 받은 z축 raw data
51 45
52 // 맵핑화 시킨 것 - 즉 10000으로 맵핑시킴 46 // 맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
53 - normal_x = map(int(ac_x), -32768, 32767, -5000, 5000); 47 + normal_x = map(int(ac_x), -16384, 16383, -1 * mapping_value, mapping_value);
54 - normal_y = map(int(ac_y), -32768, 32767, -5000, 5000); 48 + normal_y = map(int(ac_y), -16384, 16383, -1 * mapping_value, mapping_value);
55 - normal_z = map(int(ac_z), -32768, 32767, -5000, 5000); 49 + normal_z = map(int(ac_z), -16384, 16383, -1 * mapping_value, mapping_value);
56 50
57 } 51 }
58 52
59 // 충격상태함수 53 // 충격상태함수
60 void Emergency_state_() 54 void Emergency_state_()
61 { 55 {
62 - digitalWrite(ShockLed , HIGH); // 릴레이핀을 True값으로 바꿔 12번 핀의 LED를 ON시킨다. 56 + digitalWrite(ShockLed , HIGH); // LED를 True값으로 바꿔 12번 핀의 LED를 ON시킨다.
57 + digitalWrite(buzzer, HIGH); // buzzer를 True값으로 바꿔
58 + if(ac_x < 0)
59 + {
60 + analogWrite(LeftVib ,map(int(shock_level), 0, 15000, 0, 255));
61 + }
62 + else
63 + {
64 + analogWrite(RightVib ,map(int(shock_level), 0, 15000, 0, 255));
65 + }
63 } 66 }
64 67
65 // 충격 감지함수 68 // 충격 감지함수
66 void Shock_Sensing() 69 void Shock_Sensing()
67 { 70 {
68 State_Parameter = false; // 충격상태 초기화 71 State_Parameter = false; // 충격상태 초기화
69 - shock_level = 0; // 충격정도 초기화
70 //첫번째 센싱 72 //첫번째 센싱
71 for (int i=0; i < sum_count; i++) 73 for (int i=0; i < sum_count; i++)
72 { 74 {
...@@ -98,11 +100,10 @@ void Shock_Sensing() ...@@ -98,11 +100,10 @@ void Shock_Sensing()
98 deltha = deltha_x[0] + deltha_y[0] + deltha_z[0]; 100 deltha = deltha_x[0] + deltha_y[0] + deltha_z[0];
99 101
100 // deltha : 가속도 변화량 102 // deltha : 가속도 변화량
101 - if (deltha > Emergency_value){ 103 + if (deltha > Emergency_value)
102 - State_Parameter=true; 104 + {
103 - } 105 + State_Parameter=true;
104 - shock_level = deltha; // 추후 수정 예정 106 + }
105 -
106 // 충격상태체크 107 // 충격상태체크
107 if( State_Parameter == true ) 108 if( State_Parameter == true )
108 { 109 {
...@@ -110,30 +111,37 @@ void Shock_Sensing() ...@@ -110,30 +111,37 @@ void Shock_Sensing()
110 } 111 }
111 else 112 else
112 { 113 {
113 - digitalWrite(ShockLed , LOW); 114 + digitalWrite(ShockLed, LOW);
115 + digitalWrite(buzzer, LOW);
114 } 116 }
115 } 117 }
116 118
117 -// 진동함수 119 +// 누적 충격량 계산
118 -void vibrate() 120 +void ShockCal()
119 { 121 {
120 - analogWrite(LeftVib ,map(int(deltha), -15000, 15000, 0, 255)); 122 + Shock_Sensing();
121 - analogWrite(RightVib ,map(int(deltha), -15000, 15000, 0, 255)); 123 + if(State_Parameter == true)
124 + {
125 + shock_sum += deltha;
126 + }
127 + if(State_Parameter == false && shock_sum != 0)
128 + {
129 + shock_level = shock_sum; // 충격 누적값 전송
130 + shock_sum = 0; // 충격의 지속이 끝났으니 원래값인 0으로 초기화
131 + }
122 } 132 }
123 133
124 -
125 -
126 // Main 함수부 134 // Main 함수부
127 void setup() 135 void setup()
128 { 136 {
129 // 부속 하드웨어 제어 137 // 부속 하드웨어 제어
130 - pinMode(LeftVib,OUTPUT); 138 + pinMode(LeftVib,OUTPUT); // 핀모드사용, 왼쪽 진동자
131 - pinMode(RightVib,OUTPUT); 139 + pinMode(RightVib,OUTPUT); // 핀모드사용, 오른쪽 진동자
132 pinMode(ShockLed,OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔 140 pinMode(ShockLed,OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔
133 - pinMode(buzzer,OUTPUT); 141 + pinMode(buzzer,OUTPUT); // 핀모드사용, 버저
134 142
135 digitalWrite(ShockLed,LOW); // HIGH - LED on, LOW - LED off (DEFAULT : OFF) 143 digitalWrite(ShockLed,LOW); // HIGH - LED on, LOW - LED off (DEFAULT : OFF)
136 - digitalWrite(buzzer,LOW); 144 + digitalWrite(buzzer,LOW); // HIGH - buzzer on, buzzer - LED off (DEFAULT : OFF)
137 145
138 CoDrone.begin(115200); // 드론 플러그의 통신 개시 (115200bps) 146 CoDrone.begin(115200); // 드론 플러그의 통신 개시 (115200bps)
139 CoDrone.AutoConnect(NearbyDrone); // 가장 가까운 위치의 드론과 연결 147 CoDrone.AutoConnect(NearbyDrone); // 가장 가까운 위치의 드론과 연결
...@@ -156,16 +164,15 @@ void loop() ...@@ -156,16 +164,15 @@ void loop()
156 CoDrone.FlightEvent(Landing); // 드론을 착륙시킵니다. 164 CoDrone.FlightEvent(Landing); // 드론을 착륙시킵니다.
157 } 165 }
158 166
159 - Shock_Sensing(); 167 +
160 - vibrate();
161 -
162 -
163 if(PAIRING == true) // 연결(페어링)이 성공한 경우에만 실행 168 if(PAIRING == true) // 연결(페어링)이 성공한 경우에만 실행
164 { 169 {
170 +
171 + ShockCal();
165 YAW = -1 * CoDrone.AnalogScaleChange(analogRead(A3)); // 아날로그 3번 핀의 값을 YAW 값으로 사용합니다. - 좌우회전 172 YAW = -1 * CoDrone.AnalogScaleChange(analogRead(A3)); // 아날로그 3번 핀의 값을 YAW 값으로 사용합니다. - 좌우회전
166 THROTTLE = CoDrone.AnalogScaleChange(analogRead(A4)); // 아날로그 4번 핀의 값을 THROTTLE 값으로 사용합니다. - 승하강 173 THROTTLE = CoDrone.AnalogScaleChange(analogRead(A4)); // 아날로그 4번 핀의 값을 THROTTLE 값으로 사용합니다. - 승하강
167 ROLL = -1 * CoDrone.AnalogScaleChange(analogRead(A5)); // 아날로그 5번 핀의 값을 ROLL 값으로 사용합니다. - 좌우이동 174 ROLL = -1 * CoDrone.AnalogScaleChange(analogRead(A5)); // 아날로그 5번 핀의 값을 ROLL 값으로 사용합니다. - 좌우이동
168 PITCH = CoDrone.AnalogScaleChange(analogRead(A6)); // 아날로그 6번 핀의 값을 PITCH 값으로 사용합니다. - 전후진 175 PITCH = CoDrone.AnalogScaleChange(analogRead(A6)); // 아날로그 6번 핀의 값을 PITCH 값으로 사용합니다. - 전후진
169 - CoDrone.Control(SEND_INTERVAL); // 제어 신호를 보냅니다. 통신이 안정하게 가도록 시간을 두고 보냄 (최소 50ms) 176 + CoDrone.Control(1);
170 } 177 }
171 } 178 }
......