update alpha 1.2

Wonseok Kwak
Commit 9688c8f90070842734cb43538894c96d025241e7 9688c8f9 1 parent c8338d94
Showing 1 changed file with 22 additions and 75 deletions
source/DroneControl/capstonedesign2/capstonedesign2.ino
--- a/source/DroneControl/capstonedesign2/capstonedesign2.ino
View file @9688c8f
+++ b/source/DroneControl/capstonedesign2/capstonedesign2.ino
View file @9688c8f
-#include <CoDrone.h> // 코드론 라이브러리
+#include <CoDrone.h> // 코드론 라이브러리
+#include <math.h>
 #define ShockLed 12 // 충격 상태 확인을 위한 LED 핀번호
 #define buzzer 7 // 피에조 버저 핀번호
 #define LeftVib 27 // 왼쪽 진동자 핀번호
 #define RightVib 28 // 오른쪽 진동자 핀번호
 // 변수 및 상수부
-long ac_x, ac_y, ac_z, gy_x, gy_y, gy_z; //acc, gyro data (acc, gyro 계산 수식)
+long int ac_x, ac_y, ac_z;
-double angle = 0, deg; // angle, deg data (각도계산)
-double dgy_x; // double type acc data
 long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시
-long int angle_value;
 const int mapping_value = 5000;
 const int Emergency_value = 10000; // 충격상태로 판단하는 값
 const int Emergency_angle = 25; // 충격상태로 판단하는 각도(기울기)
@@ -30,12 +28,11 @@ void value_init()
   normal_y = 0; 
   normal_z = 0;
-  for(int i = 0; i < 3; i++){ 
+  for(int i = 0; i < 3; i++)
+  { 
      deltha_x[i] = 0; 
      deltha_y[i] = 0; 
      deltha_z[i] = 0; 
-     angle = 0; 
-     angle_value = 0;
   }
 }
 // 가속도 연산 함수
@@ -44,39 +41,25 @@ void accel_calculate()
   ac_x = 0; 
   ac_y = 0; 
   ac_z = 0;
-  normal_x = 0; 
-  normal_y = 0; 
-  normal_z = 0;
-  /* MPU 6050 전용
-  Wire.beginTransmission(mpu_add) ; // 번지수 찾기
-  Wire.write(0x3B) ; // 가속도 데이터 보내달라고 컨트롤 신호 보내기
-  Wire.endTransmission(false) ; // 기달리고,
-  Wire.requestFrom(mpu_add, 6, true) ; // 데이터를 받아 처리 
-  */
-  
   // Data SHIFT From CoDrone Accelerometer
-  ac_x = CoDrone.ImuAccX / 2;
+  acceldata accel;
-  ac_y = CoDrone.ImuAccY / 2;
+  accel = CoDrone.getAccelerometer();
-  ac_z = CoDrone.ImuAccZ / 2;
+  ac_x = accel.x;
- 
+  ac_y = accel.y;
-  //맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
+  ac_z = accel.z;
-  normal_x = map(int(ac_x), -16384, 16384, -5000, mapping_value);
+  
-  normal_y = map(int(ac_y), -16384, 16384, -5000, mapping_value);
+  // 맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
-  normal_z = map(int(ac_z), -16384, 16384, -5000, mapping_value);
+  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_z = map(int(ac_z), -16384, 16384, -1000, mapping_value);
- 
-  //각도계산 deg -> 각도
-  deg = atan2(ac_x, ac_z) * 180 / PI ; //rad to deg
-  dgy_x = gy_y / 131. ; //16-bit data to 250 deg/sec
-  angle = (0.95 * (angle + (dgy_x * 0.001))) + (0.05 * deg) ;
 }
 // 충격상태함수
 void Emergency_state_()
 {
-  digitalWrite(ShockLed , HIGH); // 릴레이핀을 True값으로 바꿔 13번 핀의 LED를 ON시킨다.
+  digitalWrite(ShockLed , HIGH); // 릴레이핀을 True값으로 바꿔 12번 핀의 LED를 ON시킨다.
 }
 // 충격 감지함수
@@ -85,12 +68,12 @@ void Shock_Sensing()
   State_Parameter = false; // 충격상태 초기화
   shock_level = 0; // 충격정도 초기화
   //첫번째 센싱
-  for (int i=0; i < sum_count; i++){ 
+  for (int i=0; i < sum_count; i++)
+  { 
     accel_calculate();
     deltha_x[1] = deltha_x[1]+(normal_x); 
     deltha_y[1] = deltha_y[1]+(normal_y); 
     deltha_z[1] = deltha_z[1]+(normal_z); 
-    angle_value = angle_value + angle;
   }
   deltha_x[1] = int(deltha_x[1]/sum_count); 
   deltha_y[1] = int(deltha_y[1]/sum_count); 
@@ -103,7 +86,6 @@ void Shock_Sensing()
     deltha_x[2] = deltha_x[2]+(normal_x); 
     deltha_y[2] = deltha_y[2]+(normal_y); 
     deltha_z[2] = deltha_z[2]+(normal_z); 
-    angle_value = angle_value + angle;
   }
   deltha_x[2] = int(deltha_x[2]/sum_count); 
   deltha_y[2] = int(deltha_y[2]/sum_count); 
@@ -114,35 +96,12 @@ void Shock_Sensing()
   deltha_y[0] = abs(deltha_y[1]-deltha_y[2]); 
   deltha_z[0] = abs(deltha_z[1]-deltha_z[2]);
   deltha = deltha_x[0] + deltha_y[0] + deltha_z[0];
-  angle_value = abs(int(angle_value/(sum_count)));
+
- 
   // deltha : 가속도 변화량
-  // angle_value : 각도 값(현재 각도 값)
    if (deltha > Emergency_value){
        State_Parameter=true;
    }
   shock_level = deltha; // 추후 수정 예정
-
-  /*
-  if(deltha > 하){
-      shock_level = 'a';
-      if(deltha > 중){
-        shock_level = 'b';
-        if(deltha > 상){
-          shock_level = 'c';
-        }
-      }
-   }
-
-  */
-
-   
-  /*if (angle_value > Emergency_angle){
-    State_Parameter=true;
-    }
-  if ((deltha > Emergency_value2)&&(angle_value > Emergency_angle2)){
-    State_Parameter=true;
-    }*/
   // 충격상태체크
   if( State_Parameter == true )
@@ -158,10 +117,8 @@ void Shock_Sensing()
 // 진동함수
 void vibrate()
 {
-  // if 충격값이 왼쪽인지 오른쪽인지 대소비교로 판단
+  analogWrite(LeftVib ,map(int(deltha), -15000, 15000, 0, 255));
-  // 판단되면 그위치에 나눈것만큼의 진동을 나눠서 크기별로 부여
+  analogWrite(RightVib ,map(int(deltha), -15000, 15000, 0, 255));
-  // analogWrite();
-  
 }
@@ -200,17 +157,7 @@ void loop()
   }
   Shock_Sensing();
-  if(State_Parameter == true)
+  vibrate();
-  {
-      shock_sum += deltha;
-      //currentMillis = millis();
-  }
- 
-    if(State_Parameter == false && shock_sum != 0)
-    {
-      //bluetooth.write(char_maker(shock_sum)); // 블루투스로 충격정도 전송 (shock_maker 라는 char 리턴함수를 통해 진동강도 전송) 
-      shock_sum = 0;  // 충격의 지속이 끝났으니 원래값인 0으로 초기화
-    }
   if(PAIRING == true) // 연결(페어링)이 성공한 경우에만 실행