2021-1-capstone-design2 / 2014104077

Go to a project
Toggle navigation Toggle navigation pinning
Wonseok Kwak
ce850d9d 1 parent fb9eefd1

캡스톤디자인1 관련 레거시 코드

Inline Side-by-side
Showing 3 changed files with 726 additions and 0 deletions
1 +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
2 +// for both classes must be in the include path of your project
3 +// 전처리부
4 +#include <SoftwareSerial.h> // 0,1번핀 제외하고 Serial 통신을 하기 위해 선언
5 +#include "I2Cdev.h"
6 +#include "MPU6050.h"
7 +
8 +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
9 + #include "Wire.h"
10 +#endif
11 +
12 +#define mpu_add 0x68
13 +#define Pin_Relay 13
14 +#define rxPin 7
15 +#define txPin 6
16 +
17 +SoftwareSerial mySerial(6, 7);
18 +
19 +MPU6050 mpu;
20 +
21 +
22 +// 변수 및 상수부
23 +long ac_x, ac_y, ac_z, gy_x, gy_y, gy_z; //acc, gyro data (acc, gyro 계산 수식)
24 +double angle = 0, deg; // angle, deg data (각도계산)
25 +double dgy_x; // double type acc data
26 +long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시
27 +long int angle_value;
28 +const int mapping_value = 5000;
29 +const int Emergency_value = 10000; // 충격상태로 판단하는 값
30 +const int Emergency_angle = 25; // 충격상태로 판단하는 각도(기울기)
31 +const int Emergency_value2 = 1200;// 충격상태로 판단하는 값
32 +const int Emergency_angle2 = 9; // 충격상태로 판단하는 각도(기울기)
33 +boolean State_Parameter = false; // 충격상태 판단
34 +unsigned short int shock_level; // 충격정도 상,중,하 char로 받을 생각
35 +const long int sum_count = 4; // 평균 내는 횟수
36 +const long interval = 50; // 충격을 감지후 다음 충격을 감지하는 최소 간극 시간 (ms)
37 +unsigned long previousMillis = 0; // 최초 시작시간
38 +long shock_sum = 0; // 누적 충격값
39 +
40 +
41 +/* 사용자지정함수부
42 +*/
43 +// 모듈초기화
44 +void mpu6050_init(){
45 + Wire.begin(); //I2C통신 시작
46 + Wire.beginTransmission(mpu_add) ; // 0x68(주소) 찾아가기
47 + Wire.write(0x6B) ; // 0x6B의 레지스트를 0으로 함으로써 Sleep에서 깨움
48 + Wire.write(0); // set to ZERO(WAKE UP THE MPU6050)
49 + Wire.endTransmission(true) ;
50 +}
51 +
52 +// 연산에 필요한 변수들을 초기화
53 +void value_init(){
54 + normal_x = 0;
55 + normal_y = 0;
56 + normal_z = 0;
57 +
58 + for(int i = 0; i < 3; i++){
59 + deltha_x[i] = 0;
60 + deltha_y[i] = 0;
61 + deltha_z[i] = 0;
62 + angle = 0;
63 + angle_value = 0;
64 + }
65 +}
66 +
67 +// 가속도 연산 함수
68 +void accel_calculate() {
69 + ac_x = 0;
70 + ac_y = 0;
71 + ac_z = 0;
72 + normal_x = 0;
73 + normal_y = 0;
74 + normal_z = 0;
75 +
76 + Wire.beginTransmission(mpu_add) ; // 번지수 찾기
77 + Wire.write(0x3B) ; // 가속도 데이터 보내달라고 컨트롤 신호 보내기
78 + Wire.endTransmission(false) ; // 기달리고,
79 + Wire.requestFrom(mpu_add, 6, true) ; // 데이터를 받아 처리
80 +
81 + // Data SHIFT
82 + ac_x = Wire.read() << 8 | Wire.read() ;
83 + ac_y = Wire.read() << 8 | Wire.read() ;
84 + ac_z = Wire.read() << 8 | Wire.read() ;
85 +
86 + //맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
87 + normal_x = map(int(ac_x), -16384, 16384, -5000, mapping_value);
88 + normal_y = map(int(ac_y), -16384, 16384, -5000, mapping_value);
89 + normal_z = map(int(ac_z), -16384, 16384, -5000, mapping_value);
90 +
91 + //normal_z = map(int(ac_z), -16384, 16384, -1000, mapping_value);
92 +
93 + //각도계산 deg -> 각도
94 + deg = atan2(ac_x, ac_z) * 180 / PI ; //rad to deg
95 + dgy_x = gy_y / 131. ; //16-bit data to 250 deg/sec
96 + angle = (0.95 * (angle + (dgy_x * 0.001))) + (0.05 * deg) ;
97 +}
98 +
99 +// 충격상태함수
100 +void Emergency_state_(){
101 + digitalWrite(Pin_Relay , HIGH); // 릴레이핀을 True값으로 바꿔 13번 핀의 LED를 ON시킨다.
102 + Serial.print("SHOCK level : ");
103 + Serial.println(shock_level); // Serial 모니터로 확인
104 + Serial.print(" x: ");
105 + Serial.print(deltha_x[0]);
106 + Serial.print(" y: ");
107 + Serial.print(deltha_y[0]);
108 + Serial.print(" z: ");
109 + Serial.println(deltha_z[0]);
110 +
111 + /*Serial.print(" deltha_1 : "); Serial.print(deltha_x[1]);
112 + Serial.print(" deltha_2 : "); Serial.println(deltha_x[2]);*/
113 +}
114 +
115 +void Shock_Sensing(){
116 + State_Parameter = false; // 충격상태 초기화
117 + shock_level = 0; // 충격정도 초기화
118 + //첫번째 센싱
119 + for (int i=0; i < sum_count; i++){
120 + accel_calculate();
121 + deltha_x[1] = deltha_x[1]+(normal_x);
122 + deltha_y[1] = deltha_y[1]+(normal_y);
123 + deltha_z[1] = deltha_z[1]+(normal_z);
124 + angle_value = angle_value + angle;
125 + }
126 + deltha_x[1] = int(deltha_x[1]/sum_count);
127 + deltha_y[1] = int(deltha_y[1]/sum_count);
128 + deltha_z[1] = int(deltha_z[1]/sum_count);
129 +
130 + //두번째 센싱
131 + for (int i=0; i < sum_count; i++){
132 + accel_calculate();
133 + deltha_x[2] = deltha_x[2]+(normal_x);
134 + deltha_y[2] = deltha_y[2]+(normal_y);
135 + deltha_z[2] = deltha_z[2]+(normal_z);
136 + angle_value = angle_value + angle;
137 + }
138 + deltha_x[2] = int(deltha_x[2]/sum_count);
139 + deltha_y[2] = int(deltha_y[2]/sum_count);
140 + deltha_z[2] = int(deltha_z[2]/sum_count);
141 +
142 + //3축 변화량 비교 - 가속도 변화량, 각도 평균 값
143 + deltha_x[0] = abs(deltha_x[1]-deltha_x[2]);
144 + deltha_y[0] = abs(deltha_y[1]-deltha_y[2]);
145 + deltha_z[0] = abs(deltha_z[1]-deltha_z[2]);
146 + deltha = deltha_x[0] + deltha_y[0] + deltha_z[0];
147 + angle_value = abs(int(angle_value/(sum_count)));
148 +
149 + // deltha : 가속도 변화량
150 + // angle_value : 각도 값(현재 각도 값)
151 + if (deltha > Emergency_value){
152 + State_Parameter=true;
153 + }
154 +
155 + shock_level = deltha; // 추후 수정 예정
156 +
157 +/*
158 + if(deltha > 하){
159 + shock_level = 'a';
160 + if(deltha > 중){
161 + shock_level = 'b';
162 + if(deltha > 상){
163 + shock_level = 'c';
164 + }
165 + }
166 + }
167 +
168 +*/
169 +
170 +
171 + /*if (angle_value > Emergency_angle){
172 + State_Parameter=true;
173 + }
174 + if ((deltha > Emergency_value2)&&(angle_value > Emergency_angle2)){
175 + State_Parameter=true;
176 + }*/
177 +
178 + // 충격상태체크
179 + if( State_Parameter == true ){
180 + Emergency_state_();
181 + }
182 + else {
183 + digitalWrite(Pin_Relay , LOW);
184 + }
185 +}
186 +
187 +// BLUETOOTH TRANSACTION
188 +
189 +/*main 함수부*/
190 +void setup() {
191 + Serial.begin(9600); // 시리얼 속도 설정
192 + mySerial.begin(9600); // 블루투스 통신 시작
193 + Serial.println("Initializing I2C devices...");
194 + // bluetooth.begin(9600); // 블루투스 속도 설정
195 + Serial.println("Initializing Bluetooth devices...");
196 +
197 + mpu6050_init(); // 가속도 센서 초기화
198 + Serial.println("Testing device connections...");
199 + Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
200 +
201 + pinMode(Pin_Relay, OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔
202 + digitalWrite(Pin_Relay , LOW); // HIGH - LED on, LOW - LED off (DEFAULT : OFF)
203 +}
204 +
205 +char char_maker(long x){ // 백그라운드 앱에 전송할 충격을 구분해서 char값으로 반환하는 함수
206 + if(x<=15000){
207 + return 'L';
208 + }
209 + else if(x<=25000){
210 + return 'M';
211 + }
212 + else{ // 세게치면 40000까지 누적값이 반환되는 일이 생기는데 흔하게 일어나지는 않음
213 + return 'H';
214 + }
215 +}
216 +
217 +
218 +void loop() {
219 + value_init(); //가속도-각도 관련 초기값 선언
220 + // softwareSerial.print("test");
221 + //---------dump--------------------do not erase yet-
222 + //unsigned long currentMillis = 0;
223 + //previousMillis = currentMillis;
224 + //if(currentMillis - previousMillis >= interval){
225 + //if(currentMillis - previousMillis >= interval){
226 + //--------------------------------------------------
227 +
228 +
229 +
230 +
231 +
232 +
233 + Shock_Sensing();
234 + if(State_Parameter == true){
235 + shock_sum += deltha;
236 + // currentMillis = millis();
237 + }
238 + if(State_Parameter == false && shock_sum != 0){
239 + if (Serial.available()){ // 블루투스로 충격정도 전송 (shock_maker 라는 char 리턴함수를 통해 진동강도 전송)
240 + mySerial.write(char(char_maker(shock_sum)));
241 + Serial.println(char_maker(shock_sum));
242 + Serial.println("send complete");
243 + }
244 + //bluetooth.write(char_maker(shock_sum));
245 + Serial.print("누적 충격값 : ");
246 + Serial.println(shock_sum); // 충격 누적값 전송
247 + shock_sum = 0; // 충격의 지속이 끝났으니 원래값인 0으로 초기화
248 + }
249 + if (mySerial.available()){ // 안드로이드 쪽에서 값을 아두이노로 가져오는 코드, 필요없을거 같아서 주석처리
250 + Serial.write(mySerial.read());
251 + }
252 +}
...\ No newline at end of file ...\ No newline at end of file
1 +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
2 +// for both classes must be in the include path of your project
3 +// 전처리부
4 +#include <SoftwareSerial.h> // 0,1번핀 제외하고 Serial 통신을 하기 위해 선언
5 +#include "I2Cdev.h"
6 +#include "MPU6050.h"
7 +
8 +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
9 + #include "Wire.h"
10 +#endif
11 +
12 +#define mpu_add 0x68
13 +#define Pin_Relay 13
14 +<<<<<<< Updated upstream
15 +#define rxPin 7
16 +#define txPin 6
17 +
18 +SoftwareSerial mySerial(6, 7);
19 +
20 +=======
21 +<<<<<<< Updated upstream
22 +#define BT_RXD 7
23 +#define BT_TXD 6
24 +=======
25 +#define BT_RXD 4
26 +#define BT_TXD 5
27 +>>>>>>> Stashed changes
28 +>>>>>>> Stashed changes
29 +MPU6050 mpu;
30 +
31 +
32 +// 변수 및 상수부
33 +long ac_x, ac_y, ac_z, gy_x, gy_y, gy_z; //acc, gyro data (acc, gyro 계산 수식)
34 +double angle = 0, deg; // angle, deg data (각도계산)
35 +double dgy_x; // double type acc data
36 +long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시
37 +long int angle_value;
38 +const int mapping_value = 5000;
39 +const int Emergency_value = 10000; // 충격상태로 판단하는 값
40 +const int Emergency_angle = 25; // 충격상태로 판단하는 각도(기울기)
41 +const int Emergency_value2 = 1200;// 충격상태로 판단하는 값
42 +const int Emergency_angle2 = 9; // 충격상태로 판단하는 각도(기울기)
43 +boolean State_Parameter = false; // 충격상태 판단
44 +unsigned short int shock_level; // 충격정도 상,중,하 char로 받을 생각
45 +const long int sum_count = 4; // 평균 내는 횟수
46 +const long interval = 50; // 충격을 감지후 다음 충격을 감지하는 최소 간극 시간 (ms)
47 +unsigned long previousMillis = 0; // 최초 시작시간
48 +long shock_sum = 0; // 누적 충격값
49 +
50 +
51 +/* 사용자지정함수부
52 +*/
53 +// 모듈초기화
54 +void mpu6050_init(){
55 + Wire.begin(); //I2C통신 시작
56 + Wire.beginTransmission(mpu_add) ; // 0x68(주소) 찾아가기
57 + Wire.write(0x6B) ; // 0x6B의 레지스트를 0으로 함으로써 Sleep에서 깨움
58 + Wire.write(0); // set to ZERO(WAKE UP THE MPU6050)
59 + Wire.endTransmission(true) ;
60 +}
61 +
62 +// 연산에 필요한 변수들을 초기화
63 +void value_init(){
64 + normal_x = 0;
65 + normal_y = 0;
66 + normal_z = 0;
67 +
68 + for(int i = 0; i < 3; i++){
69 + deltha_x[i] = 0;
70 + deltha_y[i] = 0;
71 + deltha_z[i] = 0;
72 + angle = 0;
73 + angle_value = 0;
74 + }
75 +}
76 +
77 +// 가속도 연산 함수
78 +void accel_calculate() {
79 + ac_x = 0;
80 + ac_y = 0;
81 + ac_z = 0;
82 + normal_x = 0;
83 + normal_y = 0;
84 + normal_z = 0;
85 +
86 + Wire.beginTransmission(mpu_add) ; // 번지수 찾기
87 + Wire.write(0x3B) ; // 가속도 데이터 보내달라고 컨트롤 신호 보내기
88 + Wire.endTransmission(false) ; // 기달리고,
89 + Wire.requestFrom(mpu_add, 6, true) ; // 데이터를 받아 처리
90 +
91 + // Data SHIFT
92 + ac_x = Wire.read() << 8 | Wire.read() ;
93 + ac_y = Wire.read() << 8 | Wire.read() ;
94 + ac_z = Wire.read() << 8 | Wire.read() ;
95 +
96 + //맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
97 + normal_x = map(int(ac_x), -16384, 16384, -5000, mapping_value);
98 + normal_y = map(int(ac_y), -16384, 16384, -5000, mapping_value);
99 + normal_z = map(int(ac_z), -16384, 16384, -5000, mapping_value);
100 +
101 + //normal_z = map(int(ac_z), -16384, 16384, -1000, mapping_value);
102 +
103 + //각도계산 deg -> 각도
104 + deg = atan2(ac_x, ac_z) * 180 / PI ; //rad to deg
105 + dgy_x = gy_y / 131. ; //16-bit data to 250 deg/sec
106 + angle = (0.95 * (angle + (dgy_x * 0.001))) + (0.05 * deg) ;
107 +}
108 +
109 +// 충격상태함수
110 +void Emergency_state_(){
111 + digitalWrite(Pin_Relay , HIGH); // 릴레이핀을 True값으로 바꿔 13번 핀의 LED를 ON시킨다.
112 + Serial.print("SHOCK level : ");
113 + Serial.println(shock_level); // Serial 모니터로 확인
114 + Serial.print(" x: ");
115 + Serial.print(deltha_x[0]);
116 + Serial.print(" y: ");
117 + Serial.print(deltha_y[0]);
118 + Serial.print(" z: ");
119 + Serial.println(deltha_z[0]);
120 +
121 + /*Serial.print(" deltha_1 : "); Serial.print(deltha_x[1]);
122 + Serial.print(" deltha_2 : "); Serial.println(deltha_x[2]);*/
123 +}
124 +
125 +void Shock_Sensing(){
126 + State_Parameter = false; // 충격상태 초기화
127 + shock_level = 0; // 충격정도 초기화
128 + //첫번째 센싱
129 + for (int i=0; i < sum_count; i++){
130 + accel_calculate();
131 + deltha_x[1] = deltha_x[1]+(normal_x);
132 + deltha_y[1] = deltha_y[1]+(normal_y);
133 + deltha_z[1] = deltha_z[1]+(normal_z);
134 + angle_value = angle_value + angle;
135 + }
136 + deltha_x[1] = int(deltha_x[1]/sum_count);
137 + deltha_y[1] = int(deltha_y[1]/sum_count);
138 + deltha_z[1] = int(deltha_z[1]/sum_count);
139 +
140 + //두번째 센싱
141 + for (int i=0; i < sum_count; i++){
142 + accel_calculate();
143 + deltha_x[2] = deltha_x[2]+(normal_x);
144 + deltha_y[2] = deltha_y[2]+(normal_y);
145 + deltha_z[2] = deltha_z[2]+(normal_z);
146 + angle_value = angle_value + angle;
147 + }
148 + deltha_x[2] = int(deltha_x[2]/sum_count);
149 + deltha_y[2] = int(deltha_y[2]/sum_count);
150 + deltha_z[2] = int(deltha_z[2]/sum_count);
151 +
152 + //3축 변화량 비교 - 가속도 변화량, 각도 평균 값
153 + deltha_x[0] = abs(deltha_x[1]-deltha_x[2]);
154 + deltha_y[0] = abs(deltha_y[1]-deltha_y[2]);
155 + deltha_z[0] = abs(deltha_z[1]-deltha_z[2]);
156 + deltha = deltha_x[0] + deltha_y[0] + deltha_z[0];
157 + angle_value = abs(int(angle_value/(sum_count)));
158 +
159 + // deltha : 가속도 변화량
160 + // angle_value : 각도 값(현재 각도 값)
161 + if (deltha > Emergency_value){
162 + State_Parameter=true;
163 + }
164 +
165 + shock_level = deltha; // 추후 수정 예정
166 +
167 +/*
168 + if(deltha > 하){
169 + shock_level = 'a';
170 + if(deltha > 중){
171 + shock_level = 'b';
172 + if(deltha > 상){
173 + shock_level = 'c';
174 + }
175 + }
176 + }
177 +
178 +*/
179 +
180 +
181 + /*if (angle_value > Emergency_angle){
182 + State_Parameter=true;
183 + }
184 + if ((deltha > Emergency_value2)&&(angle_value > Emergency_angle2)){
185 + State_Parameter=true;
186 + }*/
187 +
188 + // 충격상태체크
189 + if( State_Parameter == true ){
190 + Emergency_state_();
191 + }
192 + else {
193 + digitalWrite(Pin_Relay , LOW);
194 + }
195 +}
196 +
197 +// BLUETOOTH TRANSACTION
198 +
199 +/*main 함수부*/
200 +void setup() {
201 + Serial.begin(9600); // 시리얼 속도 설정
202 + mySerial.begin(9600); // 블루투스 통신 시작
203 + Serial.println("Initializing I2C devices...");
204 + // bluetooth.begin(9600); // 블루투스 속도 설정
205 + Serial.println("Initializing Bluetooth devices...");
206 +
207 + mpu6050_init(); // 가속도 센서 초기화
208 + Serial.println("Testing device connections...");
209 + Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
210 +
211 + pinMode(Pin_Relay, OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔
212 + digitalWrite(Pin_Relay , LOW); // HIGH - LED on, LOW - LED off (DEFAULT : OFF)
213 +}
214 +
215 +char char_maker(long x){ // 백그라운드 앱에 전송할 충격을 구분해서 char값으로 반환하는 함수
216 + if(x<=15000){
217 + return 'L';
218 + }
219 + else if(x<=25000){
220 + return 'M';
221 + }
222 + else{ // 세게치면 40000까지 누적값이 반환되는 일이 생기는데 흔하게 일어나지는 않음
223 + return 'H';
224 + }
225 +}
226 +
227 +
228 +void loop() {
229 + value_init(); //가속도-각도 관련 초기값 선언
230 + // softwareSerial.print("test");
231 + //---------dump--------------------do not erase yet-
232 + //unsigned long currentMillis = 0;
233 + //previousMillis = currentMillis;
234 + //if(currentMillis - previousMillis >= interval){
235 + //if(currentMillis - previousMillis >= interval){
236 + //--------------------------------------------------
237 +
238 +
239 +
240 +
241 +
242 +
243 + Shock_Sensing();
244 + if(State_Parameter == true){
245 +<<<<<<< Updated upstream
246 + shock_sum += deltha;
247 + // currentMillis = millis();
248 + }
249 + if(State_Parameter == false && shock_sum != 0){
250 + if (Serial.available()){ // 블루투스로 충격정도 전송 (shock_maker 라는 char 리턴함수를 통해 진동강도 전송)
251 + mySerial.write(char(char_maker(shock_sum)));
252 + Serial.println(char_maker(shock_sum));
253 + Serial.println("send complete");
254 + }
255 + //bluetooth.write(char_maker(shock_sum));
256 + Serial.print("누적 충격값 : ");
257 + Serial.println(shock_sum); // 충격 누적값 전송
258 + shock_sum = 0; // 충격의 지속이 끝났으니 원래값인 0으로 초기화
259 + }
260 +<<<<<<< Updated upstream
261 + if (mySerial.available()){ // 안드로이드 쪽에서 값을 아두이노로 가져오는 코드, 필요없을거 같아서 주석처리
262 + Serial.write(mySerial.read());
263 + }
264 +=======
265 +=======
266 + bluetooth.print("SHOCK level : ");
267 + bluetooth.println(shock_level); // 블루투스로 충격정도 전송
268 + previousMillis = currentMillis;
269 + }
270 + }
271 +>>>>>>> Stashed changes
272 +>>>>>>> Stashed changes
273 +}
1 +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
2 +// for both classes must be in the include path of your project
3 +// 전처리부
4 +#include <SoftwareSerial.h>
5 +#include "I2Cdev.h"
6 +#include "MPU6050.h"
7 +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
8 + #include "Wire.h"
9 +#endif
10 +#define mpu_add 0x68
11 +#define Pin_Relay 13
12 +#define BT_RXD 7
13 +#define BT_TXD 6
14 +MPU6050 mpu;
15 +// 변수 및 상수부
16 +long ac_x, ac_y, ac_z, gy_x, gy_y, gy_z; //acc, gyro data (acc, gyro 계산 수식)
17 +double angle = 0, deg; // angle, deg data (각도계산)
18 +double dgy_x; // double type acc data
19 +long int normal_x, normal_y, normal_z, deltha_x[3], deltha_y[3], deltha_z[3], deltha; // 노말라이즈(정규화 데이터), 가속도 변화량 표시
20 +long int angle_value;
21 +const int mapping_value = 5000;
22 +const int Emergency_value = 10000; // 충격상태로 판단하는 값
23 +const int Emergency_angle = 25; // 충격상태로 판단하는 각도(기울기)
24 +const int Emergency_value2 = 1200;// 충격상태로 판단하는 값
25 +const int Emergency_angle2 = 9; // 충격상태로 판단하는 각도(기울기)
26 +boolean State_Parameter = false; // 충격상태 판단
27 +unsigned short int shock_level; // 충격정도 상,중,하 char로 받을 생각
28 +const long int sum_count = 4; // 평균 내는 횟수
29 +const long interval = 50; // 충격을 감지후 다음 충격을 감지하는 최소 간극 시간 (ms)
30 +unsigned long previousMillis = 0; // 최초 시작시간
31 +long shock_sum = 0; // 누적 충격값
32 +
33 +/* 사용자지정함수부
34 +*/
35 +// 모듈초기화
36 +void mpu6050_init(){
37 + Wire.begin(); //I2C통신 시작
38 + Wire.beginTransmission(mpu_add) ; // 0x68(주소) 찾아가기
39 + Wire.write(0x6B) ; // 0x6B의 레지스트를 0으로 함으로써 Sleep에서 깨움
40 + Wire.write(0); // set to ZERO(WAKE UP THE MPU6050)
41 + Wire.endTransmission(true) ;
42 +}
43 +// 연산에 필요한 변수들을 초기화
44 +void value_init(){
45 + normal_x = 0;
46 + normal_y = 0;
47 + normal_z = 0;
48 +
49 + for(int i = 0; i < 3; i++){
50 + deltha_x[i] = 0;
51 + deltha_y[i] = 0;
52 + deltha_z[i] = 0;
53 + angle = 0;
54 + angle_value = 0;
55 + }
56 +}
57 +// 가속도 연산 함수
58 +void accel_calculate() {
59 + ac_x = 0;
60 + ac_y = 0;
61 + ac_z = 0;
62 + normal_x = 0;
63 + normal_y = 0;
64 + normal_z = 0;
65 + Wire.beginTransmission(mpu_add) ; // 번지수 찾기
66 + Wire.write(0x3B) ; // 가속도 데이터 보내달라고 컨트롤 신호 보내기
67 + Wire.endTransmission(false) ; // 기달리고,
68 + Wire.requestFrom(mpu_add, 6, true) ; // 데이터를 받아 처리
69 + // Data SHIFT
70 + ac_x = Wire.read() << 8 | Wire.read() ;
71 + ac_y = Wire.read() << 8 | Wire.read() ;
72 + ac_z = Wire.read() << 8 | Wire.read() ;
73 +
74 + //맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
75 + normal_x = map(int(ac_x), -16384, 16384, -5000, mapping_value);
76 + normal_y = map(int(ac_y), -16384, 16384, -5000, mapping_value);
77 + normal_z = map(int(ac_z), -16384, 16384, -5000, mapping_value);
78 + //normal_z = map(int(ac_z), -16384, 16384, -1000, mapping_value);
79 +
80 + //각도계산 deg -> 각도
81 + deg = atan2(ac_x, ac_z) * 180 / PI ; //rad to deg
82 + dgy_x = gy_y / 131. ; //16-bit data to 250 deg/sec
83 + angle = (0.95 * (angle + (dgy_x * 0.001))) + (0.05 * deg) ;
84 +}
85 +// 충격상태함수
86 +void Emergency_state_(){
87 + digitalWrite(Pin_Relay , HIGH); // 릴레이핀을 True값으로 바꿔 13번 핀의 LED를 ON시킨다.
88 + Serial.print("SHOCK level : ");
89 + Serial.println(shock_level); // Serial 모니터로 확인
90 + Serial.print(" x: ");
91 + Serial.print(deltha_x[0]);
92 + Serial.print(" y: ");
93 + Serial.print(deltha_y[0]);
94 + Serial.print(" z: ");
95 + Serial.println(deltha_z[0]);
96 + /*Serial.print(" deltha_1 : "); Serial.print(deltha_x[1]);
97 + Serial.print(" deltha_2 : "); Serial.println(deltha_x[2]);*/
98 +}
99 +void Shock_Sensing(){
100 + State_Parameter = false; // 충격상태 초기화
101 + shock_level = 0; // 충격정도 초기화
102 + //첫번째 센싱
103 + for (int i=0; i < sum_count; i++){
104 + accel_calculate();
105 + deltha_x[1] = deltha_x[1]+(normal_x);
106 + deltha_y[1] = deltha_y[1]+(normal_y);
107 + deltha_z[1] = deltha_z[1]+(normal_z);
108 + angle_value = angle_value + angle;
109 + }
110 + deltha_x[1] = int(deltha_x[1]/sum_count);
111 + deltha_y[1] = int(deltha_y[1]/sum_count);
112 + deltha_z[1] = int(deltha_z[1]/sum_count);
113 + //두번째 센싱
114 + for (int i=0; i < sum_count; i++){
115 + accel_calculate();
116 + deltha_x[2] = deltha_x[2]+(normal_x);
117 + deltha_y[2] = deltha_y[2]+(normal_y);
118 + deltha_z[2] = deltha_z[2]+(normal_z);
119 + angle_value = angle_value + angle;
120 + }
121 + deltha_x[2] = int(deltha_x[2]/sum_count);
122 + deltha_y[2] = int(deltha_y[2]/sum_count);
123 + deltha_z[2] = int(deltha_z[2]/sum_count);
124 +
125 + //3축 변화량 비교 - 가속도 변화량, 각도 평균 값
126 + deltha_x[0] = abs(deltha_x[1]-deltha_x[2]);
127 + deltha_y[0] = abs(deltha_y[1]-deltha_y[2]);
128 + deltha_z[0] = abs(deltha_z[1]-deltha_z[2]);
129 + deltha = deltha_x[0] + deltha_y[0] + deltha_z[0];
130 + angle_value = abs(int(angle_value/(sum_count)));
131 +
132 + // deltha : 가속도 변화량
133 + // angle_value : 각도 값(현재 각도 값)
134 + if (deltha > Emergency_value){
135 + State_Parameter=true;
136 + }
137 + shock_level = deltha; // 추후 수정 예정
138 +/*
139 + if(deltha > 하){
140 + shock_level = 'a';
141 + if(deltha > 중){
142 + shock_level = 'b';
143 + if(deltha > 상){
144 + shock_level = 'c';
145 + }
146 + }
147 + }
148 +*/
149 +
150 + /*if (angle_value > Emergency_angle){
151 + State_Parameter=true;
152 + }
153 + if ((deltha > Emergency_value2)&&(angle_value > Emergency_angle2)){
154 + State_Parameter=true;
155 + }*/
156 +
157 + // 충격상태체크
158 + if( State_Parameter == true ){
159 + Emergency_state_();
160 + }
161 + else {
162 + digitalWrite(Pin_Relay , LOW);
163 + }
164 +}
165 +// BLUETOOTH TRANSACTION
166 +SoftwareSerial bluetooth(BT_RXD, BT_TXD); // 블루투스 모듈
167 +/*main 함수부*/
168 +void setup() {
169 + Serial.begin(9600); // 시리얼 속도 설정
170 + Serial.println("Initializing I2C devices...");
171 + bluetooth.begin(9600); // 블루투스 속도 설정
172 + Serial.println("Initializing Bluetooth devices...");
173 +
174 + mpu6050_init(); // 가속도 센서 초기화
175 + Serial.println("Testing device connections...");
176 + Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
177 +
178 + pinMode(Pin_Relay, OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔
179 + digitalWrite(Pin_Relay , LOW); // HIGH - LED on, LOW - LED off (DEFAULT : OFF)
180 +}
181 +void loop() {
182 + value_init(); //가속도-각도 관련 초기값 선언
183 + //---------dump--------------------do not erase yet-
184 + //unsigned long currentMillis = 0;
185 + //previousMillis = currentMillis;
186 + //if(currentMillis - previousMillis >= interval){
187 + //if(currentMillis - previousMillis >= interval){
188 + //--------------------------------------------------
189 +
190 + Shock_Sensing();
191 + if(State_Parameter == true){
192 + shock_sum += deltha;
193 + //currentMillis = millis();
194 + }
195 + if(State_Parameter == false && shock_sum != 0){
196 + //bluetooth.write(char_maker(shock_sum)); // 블루투스로 충격정도 전송 (shock_maker 라는 char 리턴함수를 통해 진동강도 전송)
197 + Serial.print("누적 충격값 : ");
198 + Serial.println(shock_sum); // 충격 누적값 전송
199 + shock_sum = 0; // 충격의 지속이 끝났으니 원래값인 0으로 초기화
200 + }
201 +}