Wonseok Kwak

update alpha 1.1

#include <CoDrone.h> // 코드론 라이브러리
#define ShockLed 12 // 충격 상태 확인을 위한 LED 핀번호
#define buzzer 7 // 피에조 버저 핀번호
#define LeftVib 27 // 왼쪽 진동자 핀번호
#define RightVib 28 // 오른쪽 진동자 핀번호
......@@ -55,9 +56,9 @@ void accel_calculate()
*/
// Data SHIFT From CoDrone Accelerometer
ac_x = CoDrone.getAccelerometer().x;
ac_y = CoDrone.getAccelerometer().y;
ac_z = CoDrone.getAccelerometer().z;
ac_x = CoDrone.ImuAccX / 2;
ac_y = CoDrone.ImuAccY / 2;
ac_z = CoDrone.ImuAccZ / 2;
//맵핑화 시킨 것 - 즉 10000으로 맵핑시킴
normal_x = map(int(ac_x), -16384, 16384, -5000, mapping_value);
......@@ -76,17 +77,6 @@ void accel_calculate()
void Emergency_state_()
{
digitalWrite(ShockLed , HIGH); // 릴레이핀을 True값으로 바꿔 13번 핀의 LED를 ON시킨다.
Serial.print("SHOCK level : ");
Serial.println(shock_level); // Serial 모니터로 확인
Serial.print(" x: ");
Serial.print(deltha_x[0]);
Serial.print(" y: ");
Serial.print(deltha_y[0]);
Serial.print(" z: ");
Serial.println(deltha_z[0]);
/*Serial.print(" deltha_1 : "); Serial.print(deltha_x[1]);
Serial.print(" deltha_2 : "); Serial.println(deltha_x[2]);*/
}
// 충격 감지함수
......@@ -168,6 +158,9 @@ void Shock_Sensing()
// 진동함수
void vibrate()
{
// if 충격값이 왼쪽인지 오른쪽인지 대소비교로 판단
// 판단되면 그위치에 나눈것만큼의 진동을 나눠서 크기별로 부여
// analogWrite();
}
......@@ -176,10 +169,15 @@ void vibrate()
// Main 함수부
void setup()
{
// 부속 하드웨어 제어
pinMode(LeftVib,OUTPUT);
pinMode(RightVib,OUTPUT);
pinMode(ShockLed,OUTPUT); // 핀모드사용, led로 육안확인 가능하게끔
pinMode(buzzer,OUTPUT);
digitalWrite(ShockLed,LOW); // HIGH - LED on, LOW - LED off (DEFAULT : OFF)
digitalWrite(buzzer,LOW);
CoDrone.begin(115200); // 드론 플러그의 통신 개시 (115200bps)
CoDrone.AutoConnect(NearbyDrone); // 가장 가까운 위치의 드론과 연결
CoDrone.DroneModeChange(Flight); // 드론을 플라이트 모드로 설정
......@@ -192,47 +190,35 @@ void loop()
byte bt1 = digitalRead(11); // ■ □ □ □ □ □ □ 밑면 적외선 센서를 입력으로 사용
byte bt4 = digitalRead(14); // □ □ □ ■ □ □ □ 밑면 적외선 센서를 입력으로 사용
byte bt8 = digitalRead(18); // □ □ □ □ □ □ ■ 밑면 적외선 센서를 입력으로 사용
if (bt1 && !bt4 && !bt8) // 밑면 센서 가장 끝 11번 센서에 손을 대면 실행합니다.
{
CoDrone.FlightEvent(Stop); // 드론을 정지시킵니다.
CoDrone.FlightEvent(Stop); // 드론을 정지시킵니다.
}
if (!bt1 && !bt4 && bt8) // 밑면 센서 가장 끝 18번 센서에 손을 대면 실행합니다.
if (!bt1 && !bt4 && bt8) // 밑면 센서 가장 끝 18번 센서에 손을 대면 실행합니다.
{
CoDrone.FlightEvent(Landing); // 드론을 착륙시킵니다.
CoDrone.FlightEvent(Landing); // 드론을 착륙시킵니다.
}
Shock_Sensing();
if(State_Parameter == true){
shock_sum += deltha;
//currentMillis = millis();
}
if(State_Parameter == true)
{
shock_sum += deltha;
//currentMillis = millis();
}
if(State_Parameter == false && shock_sum != 0){
if(State_Parameter == false && shock_sum != 0)
{
//bluetooth.write(char_maker(shock_sum)); // 블루투스로 충격정도 전송 (shock_maker 라는 char 리턴함수를 통해 진동강도 전송)
Serial.print("누적 충격값 : ");
Serial.println(shock_sum); // 충격 누적값 전송
shock_sum = 0; // 충격의 지속이 끝났으니 원래값인 0으로 초기화
}
// 연결(페어링)이 성공한 경우에만 실행
if(PAIRING == true)
if(PAIRING == true) // 연결(페어링)이 성공한 경우에만 실행
{
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)
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)
}
}
......