Showing
1 changed file
with
45 additions
and
38 deletions
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 | } | ... | ... |
-
Please register or login to post a comment