update alpha 1.3

 
 #include <CoDrone.h> // 코드론 라이브러리
-#include <math.h>
 #define ShockLed 12 // 충격 상태 확인을 위한 LED 핀번호
 #define buzzer 7 // 피에조 버저 핀번호
 #define LeftVib 27 // 왼쪽 진동자 핀번호
@@ -10,15 +9,10 @@
 long int ac_x, ac_y, ac_z;
 long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시
 const int mapping_value = 5000;
-const int Emergency_value = 10000; // 충격상태로 판단하는 값
-const int Emergency_angle = 25; // 충격상태로 판단하는 각도(기울기)
-const int Emergency_value2 = 1200;// 충격상태로 판단하는 값
-const int Emergency_angle2 = 9; // 충격상태로 판단하는 각도(기울기)
+const int Emergency_value = 300; // 충격상태로 판단하는 값
 boolean State_Parameter = false; // 충격상태 판단
-unsigned short int shock_level; // 충격정도 상,중,하   char로 받을 생각
+long int shock_level;
 const long int sum_count = 4; // 평균 내는 횟수
-const long interval = 50; // 충격을 감지후 다음 충격을 감지하는 최소 간극 시간 (ms)
-unsigned long previousMillis = 0; // 최초 시작시간
 long shock_sum = 0; // 누적 충격값
 
 //사용자지정 함수부
@@ -44,29 +38,37 @@ void accel_calculate()
 
   // Data SHIFT From CoDrone Accelerometer
   acceldata accel;
-  accel = CoDrone.getAccelerometer();
-  ac_x = accel.x;
-  ac_y = accel.y;
-  ac_z = accel.z;
+  accel = CoDrone.getAccelerometer(); // CoDrone에서 가속도 전달받음.
+  ac_x = accel.x; // 넘겨 받은 x축 raw data
+  ac_y = accel.y; // 넘겨 받은 y축 raw data
+  ac_z = accel.z; // 넘겨 받은 z축 raw data
   
   // 맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
-  normal_x = map(int(ac_x), -32768, 32767, -5000, 5000);
-  normal_y = map(int(ac_y), -32768, 32767, -5000, 5000);
-  normal_z = map(int(ac_z), -32768, 32767, -5000, 5000);
+  normal_x = map(int(ac_x), -16384, 16383, -1 * mapping_value, mapping_value);
+  normal_y = map(int(ac_y), -16384, 16383, -1 * mapping_value, mapping_value);
+  normal_z = map(int(ac_z), -16384, 16383, -1 * mapping_value, mapping_value);
 
 }
 
 // 충격상태함수
 void Emergency_state_()
 {
-  digitalWrite(ShockLed , HIGH); // 릴레이핀을 True값으로 바꿔 12번 핀의 LED를 ON시킨다.
+  digitalWrite(ShockLed , HIGH); // LED를 True값으로 바꿔 12번 핀의 LED를 ON시킨다.
+  digitalWrite(buzzer, HIGH); // buzzer를 True값으로 바꿔 
+  if(ac_x < 0)
+  {
+    analogWrite(LeftVib ,map(int(shock_level), 0, 15000, 0, 255));
+  }
+  else
+  {
+    analogWrite(RightVib ,map(int(shock_level), 0, 15000, 0, 255));
+  }
 }
 
 // 충격 감지함수
 void Shock_Sensing()
 {
   State_Parameter = false; // 충격상태 초기화
-  shock_level = 0; // 충격정도 초기화
   //첫번째 센싱
   for (int i=0; i < sum_count; i++)
   { 
@@ -98,11 +100,10 @@ void Shock_Sensing()
   deltha = deltha_x[0] + deltha_y[0] + deltha_z[0];
 
   // deltha : 가속도 변화량
-   if (deltha > Emergency_value){
-       State_Parameter=true;
-   }
-  shock_level = deltha; // 추후 수정 예정
- 
+  if (deltha > Emergency_value)
+  {
+      State_Parameter=true;
+  }
   // 충격상태체크
   if( State_Parameter == true )
   {
@@ -110,30 +111,37 @@ void Shock_Sensing()
   }
   else 
   {
-    digitalWrite(ShockLed , LOW);
+    digitalWrite(ShockLed, LOW);
+    digitalWrite(buzzer, LOW);
   }
 }
 
-// 진동함수
-void vibrate()
+// 누적 충격량 계산
+void ShockCal()
 {
-  analogWrite(LeftVib ,map(int(deltha), -15000, 15000, 0, 255));
-  analogWrite(RightVib ,map(int(deltha), -15000, 15000, 0, 255));
+  Shock_Sensing();
+  if(State_Parameter == true)
+    {
+      shock_sum += deltha;
+    }
+    if(State_Parameter == false && shock_sum != 0)
+    {
+      shock_level = shock_sum;  // 충격 누적값 전송
+      shock_sum = 0;  // 충격의 지속이 끝났으니 원래값인 0으로 초기화
+    }
 }
 
-
-
 // Main 함수부
 void setup() 
 {
   // 부속 하드웨어 제어
-  pinMode(LeftVib,OUTPUT);
-  pinMode(RightVib,OUTPUT);
+  pinMode(LeftVib,OUTPUT); // 핀모드사용, 왼쪽 진동자
+  pinMode(RightVib,OUTPUT); // 핀모드사용, 오른쪽 진동자
   pinMode(ShockLed,OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔
-  pinMode(buzzer,OUTPUT);
+  pinMode(buzzer,OUTPUT); // 핀모드사용, 버저
   
   digitalWrite(ShockLed,LOW); // HIGH - LED on, LOW - LED off   (DEFAULT : OFF)
-  digitalWrite(buzzer,LOW);
+  digitalWrite(buzzer,LOW); // HIGH - buzzer on, buzzer - LED off   (DEFAULT : OFF)
   
   CoDrone.begin(115200);  // 드론 플러그의 통신 개시 (115200bps)
   CoDrone.AutoConnect(NearbyDrone); // 가장 가까운 위치의 드론과 연결
@@ -156,16 +164,15 @@ void loop()
     CoDrone.FlightEvent(Landing);   // 드론을 착륙시킵니다.
   }
 
-  Shock_Sensing();
-  vibrate();
-  
-  
+    
   if(PAIRING == true) // 연결(페어링)이 성공한 경우에만 실행
   {
+    
+    ShockCal();
     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)
+    CoDrone.Control(1);
   }
 }