6_Project/Zigner
ZIGNER Main Code 250116
Mi:sAng
2025. 1. 16. 23:57
//패치 내용
//250115 : MPU9250 기능 추가
//250116 : 배터리 잔여용량 체크 기능, 서보제어 기능, 데이터 분리 기능
//서보동작이 정밀하지 못하니 수정 요망
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU9250.h"
#include <Servo.h>
#include <SoftwareSerial.h>
Servo ESC; // create servo object to control the ESC
SoftwareSerial HC12(2, 3); // 아두이노 2번을 HC-12 TX Pin에 연결, 3번을 HC-12 RX Pin에 연결.
MPU9250 accelgyro;
I2Cdev I2C_M;
Servo servo1;
Servo servo2;
Servo servo3;
Servo camServo;
//1. Setup 변수
int setPin=13; // SET 단자 D13
int batteryPin=0; //배터리 측정 단자 A0
int servo1Pin =6;
int servo2Pin =9;
int servo3Pin =10;
int camServoPin =11;
bool safetySeq=false;
bool safetyCom=false;
bool safetyBet=false;
bool safetyJoyCom=false;
bool safetySv=false;
bool safetyMt=false;
bool safetyCam=false;
bool safetyJoy=false;
bool flyRdy =false;
//2. MPU9250 변수
uint8_t buffer_m[6];
int16_t ax, ay, az;//Accelation
int16_t gx, gy, gz;//Gyro
int16_t mx, my, mz;//Compass
float heading; //N극에서 몇 도 회전해 있는가를 구함
float tiltheading;//The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:
float Axyz[3];//Accelation
float Gxyz[3];//Gyro
float Mxyz[3];//Compass
#define sample_num_mdate 5000 //sample 데이터 수집할 횟수
volatile float mx_sample[3]; //샘플 넣을 배열. mx_sample[]은 0:min, 1:max, 2:sample 이다.
volatile float my_sample[3]; //샘플 넣을 배열. my_sample[]은 0:min, 1:max, 2:sample 이다.
volatile float mz_sample[3]; //샘플 넣을 배열. mz_sample[]은 0:min, 1:max, 2:sample 이다.
static float mx_centre = 0; //Calibration 값 (설정값 아니다. 아래서 구할 것이다.) Mxyz_init_calibrated ()에서 구한다.
static float my_centre = 0; //Calibration 값 (설정값 아니다. 아래서 구할 것이다.) Mxyz_init_calibrated ()에서 구한다.
static float mz_centre = 0; //Calibration 값 (설정값 아니다. 아래서 구할 것이다.) Mxyz_init_calibrated ()에서 구한다.
volatile int mx_max = 0; //샘플 데이터 중에서 최대값
volatile int my_max = 0; //샘플 데이터 중에서 최대값
volatile int mz_max = 0; //샘플 데이터 중에서 최대값
volatile int mx_min = 0; //샘플 데이터 중에서 최소값
volatile int my_min = 0; //샘플 데이터 중에서 최소값
volatile int mz_min = 0; //샘플 데이터 중에서 최소값
float temperature;
float pressure;
float atm;
float altitude;
//3. MPU9250 함수
void getHeading(void); //N극에서 몇 도 회전해 있는가를 구함
void getTiltHeading(void); //The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:
void Mxyz_init_calibrated(); //Calibration 값 구하고 시리얼 출력하는 함수
void get_calibration_Data(); //calibration값 (mx_centre)구하는 함수
void get_one_sample_date_mxyz(); //Compass 데이터 1번 받고 샘플로 삼는다.
void getAccel_Data(void); //Accel 데이터 받기
void getGyro_Data(void);//Gyro 데이터 받기
void getCompass_Data(void);//Compass 데이터 받기
void getCompassDate_calibrated (); //Compass Calibrated 데이터 받기
//4. Control 변수
int motorSpeed=0;;// 80~150에서 동작한다.
int servo1Stat=2;//1:상, 2:중 3:하
int servo2Stat=2;//1:상, 2:중 3:하
int servo3Stat=2;//1:상, 2:중 3:하
int camServoStat=0; //0~180도 가능
void setup() {
//0.
pinMode(setPin,OUTPUT);
digitalWrite(setPin,HIGH);
delay(100);
Wire.begin();
Serial.begin(115200); // 통신속도 115200 bps
HC12.begin(115200); // 시리얼포트 ↔ HC12
servo1.attach(6);
servo2.attach(9);
servo3.attach(10);
camServo.attach(11);
//1.자이로 센서 캘리브레이션 세팅
Serial.println("[ZIGNER] Initializing I2C devices...");
HC12.println("[ZIGNER] Initializing I2C devices...");
accelgyro.initialize();
Serial.println("[ZIGNER] Testing device connections...");
HC12.println("[ZIGNER] Testing device connections...");
Serial.print("[ZIGNER] ");
Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
HC12.print("[ZIGNER] " );
HC12.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
delay(1000);
Serial.println(" ");
HC12.println(" ");
//2.MPU9250 Calibration
Serial.println("[ZIGNER] Initializing Calibration...");
HC12.println("[ZIGNER] Initializing Calibration...");
Mxyz_init_calibrated (); //초기에 Calibration 값 구하고 동작해야한다.
//3.ESC Attach
Serial.println("[ZIGNER] ESC Attach...");
HC12.println("[ZIGNER] ESC Attach...");
ESC.attach(5);
ESC.write(70);
Serial.println("[ZIGNER] COM SET HIGH...");
HC12.println("[ZIGNER] COM SET HIGH...");
digitalWrite(setPin,HIGH);
delay(3000);
//4. Setup Done
Serial.println("[ZIGNER] Setup Done");
Serial.println("[ZIGNER] COM OPEN");
HC12.println("[ZIGNER] Setup Done");
HC12.println("[ZIGNER] COM OPEN");
}
void loop() {
//큰 틀
//Safety Squence
// if(!safetySeq){ //safety Seq 시작
// if(!safetyCom){//통신체크
// }
// else if(!safetyBet){//배터리 체크
// }
// else if (!safetyJoyCom){//조이스틱 연결체크
// }
// else if (!safetySv){//서보체크
// }
// else if(!safetyMt){//모터체크
// }
// else if(!safetyCam){//카메라 서보 체크
// }
// else if(!safetyJoy){//조이스틱 체크
// }
// }
//Flying Sequence
if(!flyRdy){//OK+FLY 받으면 조작 시작
//1.MPU9250 데이터 읽기
getAccel_Data();
getGyro_Data();
getCompassDate_calibrated(); // compass data has been calibrated here
getHeading(); //before we use this function we should run 'getCompassDate_calibrated()' frist, so that we can get calibrated data ,then we can get correct angle .
getTiltHeading();
// Serial.println("calibration parameter: ");
// Serial.print(mx_centre);
// Serial.print(" ");
// Serial.print(my_centre);
// Serial.print(" ");
// Serial.println(mz_centre);
// Serial.println(" ");
// Serial.println("Acceleration(g) of X,Y,Z:");
// Serial.print(Axyz[0]);
// Serial.print(",");
// Serial.print(Axyz[1]);
// Serial.print(",");
// Serial.println(Axyz[2]);
// Serial.println("Gyro(degress/s) of X,Y,Z:");
// Serial.print(Gxyz[0]);
// Serial.print(",");
// Serial.print(Gxyz[1]);
// Serial.print(",");
// Serial.println(Gxyz[2]);
// Serial.println("Compass Value of X,Y,Z:");
// Serial.print(Mxyz[0]);
// Serial.print(",");
// Serial.print(Mxyz[1]);
// Serial.print(",");
// Serial.println(Mxyz[2]);
// Serial.println("The clockwise angle between the magnetic north and X-Axis:");
// Serial.print(heading);
// Serial.println(" ");
// Serial.println("The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:");
// Serial.println(tiltheading);
// Serial.println(" ");
// Serial.println();
//2.배터리값 읽기
float batteryVal=analogRead(batteryPin);
batteryVal = map(batteryVal,0,1023,0,5);
batteryVal = batteryVal*3;
//3.데이터 프로토콜
//ZIGNER 프로토콜 : Zax,ay,az,gx,gy,gz,E
//ZIGNER 프로토콜 : Gmx,my,mz,배터리(V),heading,tiltheading,E
String dataProtocol="Z,";
dataProtocol=dataProtocol+String(Axyz[0])+",";
dataProtocol=dataProtocol+String(Axyz[1])+",";
dataProtocol=dataProtocol+String(Axyz[2])+",";
dataProtocol=dataProtocol+String(Gxyz[0])+",";
dataProtocol=dataProtocol+String(Gxyz[1])+",";
dataProtocol=dataProtocol+String(Gxyz[2])+",E";
String dataProtocol2="G,";
dataProtocol2=dataProtocol2+String(Mxyz[0])+",";
dataProtocol2=dataProtocol2+String(Mxyz[1])+",";
dataProtocol2=dataProtocol2+String(Mxyz[2])+",";
dataProtocol2=dataProtocol2+String(batteryVal)+","; //배터리
dataProtocol2=dataProtocol2+String(heading)+",";
dataProtocol2=dataProtocol2+String(tiltheading)+",E";//End 표시
delay(20);
//4.PC에서 데이터 받고 데이터 보내기
while (HC12.available()) { //HC12모듈이 받은 데이터가 있을 경우 시리얼모니터로 출력
//-1.PC에서 값 받기
String input = HC12.readString();
Serial.print("[PC] ");
Serial.println(input);
//-2.조이스틱 값인 경우
if(input.charAt(0)=='P'){ //조이스틱 값 프로토콜
//P모터값,서보1,서보2,서보3(1,2,3),카메라서보1(0~180),E
Serial.print("[PC] ");
Serial.println(input);
//-1.MPU값을 보낸다.
Serial.println("Data [ZIGNER] >> [PC]");
HC12.println(dataProtocol);
Serial.println(dataProtocol);
HC12.println(dataProtocol2);
Serial.println(dataProtocol2);
//-2.조이스틱 값을 분리한다.
dataSeparate(input);
//-3.모터와 서보를 동작한다.
ESC.write(motorSpeed);
Serial.print("[Motor Speed] ");
Serial.println(motorSpeed);
HC12.print("[Motor Speed] ");
HC12.println(motorSpeed);
moveWing(servo1Stat, servo2Stat, servo3Stat);
Serial.print("[Servo Stat] ");
Serial.print("servo1:");
Serial.print(servo1Stat);
Serial.print(", servo2:");
Serial.print(servo2Stat);
Serial.print(", servo3:");
Serial.println(servo3Stat);
HC12.print("[Servo Stat]");
HC12.print(servo1Stat);
HC12.print(servo2Stat);
HC12.println(servo3Stat);
moveCam(camServoStat);
Serial.print("[Cam Servo Angle] ");
Serial.println(camServoStat);
HC12.print("[Cam Servo Angle] ");
HC12.println(camServoStat);
delay(200);
}
//-3.COM SET HIGH 명령어
else if(input =="OPEN+COM"){//통신 SET HIGH 설정 명령
digitalWrite(setPin,HIGH);
HC12.println("[ZIGNER] SET+HIGH");
Serial.print("[PC] ");
Serial.println(input);
delay(20);
}
//-4. 통신 답변 바람 명령어
else if(input =="OK+COM"){ //통신 답신바라는 명령
HC12.println("[ZIGNER] COM+OK");
Serial.print("[PC] ");
Serial.println(input);
delay(20);
}
}
//5.무인기 내부 시리얼 모니터
// 시리얼모니터로 수신(입력)데이터가 있을 경우 HC12를 통해 데이터를 발송
// while (Serial.available()) {
// String input = Serial.readString();
// Serial.print("[ZIGNER] : ");
// Serial.println(input);
// if(input =="SET+HIGH"){
// digitalWrite(setPin,HIGH);
// delay(20);
// }
// else if(input =="SET+LOW"){
// digitalWrite(setPin,LOW);
// delay(20);
// }
// else{
// HC12.println(input);
// }
// }
}
}
void moveWing(int sv1,int sv2, int sv3){
switch(sv1){
case 1:
servo1.write(0);
break;
case 2:
servo1.write(90);
break;
case 3:
servo1.write(180);
}
switch(sv2){
case 1:
servo2.write(0);
break;
case 2:
servo2.write(90);
break;
case 3:
servo2.write(180);
break;
}
switch(sv3){
case 1:
servo3.write(0);
break;
case 2:
servo3.write(90);
break;
case 3:
servo3.write(180);
break;
}
}
void moveCam(int num){
camServo.write(num);
}
void dataSeparate(String input2){
// int motorSpeed=0;;// 80~150에서 동작한다.
// int servo1Stat=2;//1:상, 2:중 3:하
// int servo2Stat=2;//1:상, 2:중 3:하
// int servo3Stat=2;//1:상, 2:중 3:하
// int camServoStat=0; //0~180도 가능
//P180,2,2,2,100,EP,180,2,2,2,100,E
// 첫 번째 P와 첫 번째 E 사이의 문자열 추출
int startIndex = input2.indexOf('P');
int endIndex = input2.indexOf('E', startIndex);
if (startIndex != -1 && endIndex != -1) {
String segment = input2.substring(startIndex + 1, endIndex); // P와 E 사이 추출
String tokens[10];
int tokenIndex = 0;
while (segment.length() > 0) {
int index = segment.indexOf(',');
if (index == -1) { // 더 이상 ','가 없는 경우
tokens[tokenIndex++] = segment;
break;
} else {
tokens[tokenIndex++] = segment.substring(0, index);
segment = segment.substring(index + 1);
}
}
// 결과 출력
Serial.println("Parsed tokens between first P and first E:");
for (int i = 0; i < tokenIndex; i++) {
Serial.print("Token ");
Serial.print(i + 1);
Serial.print(": ");
Serial.println(tokens[i]);
}
motorSpeed=tokens[1].toInt();// 80~150에서 동작한다.
servo1Stat=tokens[2].toInt();//1:상, 2:중 3:하
servo2Stat=tokens[3].toInt();;//1:상, 2:중 3:하
servo3Stat=tokens[4].toInt();;//1:상, 2:중 3:하
camServoStat=tokens[5].toInt();//0~180도 가능
Serial.println("[Separated Data]");
Serial.print(" Motor:");
Serial.print(motorSpeed);
Serial.print(", Servo1:");
Serial.print(servo1Stat);
Serial.print(", Servo2:");
Serial.print(servo2Stat);
Serial.print(", Servo3:");
Serial.print(servo3Stat);
Serial.print(", CamServo:");
Serial.println(camServoStat);
}
else {
Serial.println("[Error]: Could not find both P and E in the [Control Data].");
}
}
void getHeading(void) //N극에서 몇 도 회전해 있는가를 구함
{//before we use this function we should run 'getCompassDate_calibrated()' frist,
//so that we can get calibrated data ,then we can get correct angle .
heading = 180 * atan2(Mxyz[1], Mxyz[0]) / PI;
if (heading < 0) heading += 360;
}
void getTiltHeading(void) //The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:
{
float pitch = asin(-Axyz[0]);
float roll = asin(Axyz[1] / cos(pitch));
float xh = Mxyz[0] * cos(pitch) + Mxyz[2] * sin(pitch);
float yh = Mxyz[0] * sin(roll) * sin(pitch) + Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
float zh = -Mxyz[0] * cos(roll) * sin(pitch) + Mxyz[1] * sin(roll) + Mxyz[2] * cos(roll) * cos(pitch);
tiltheading = 180 * atan2(yh, xh) / PI;
if (yh < 0) tiltheading += 360;
}
void Mxyz_init_calibrated () //Calibration 값 구하고 시리얼 출력하는 함수
{
Serial.println(F("Before using 9DOF,we need to calibrate the compass frist,It will takes about 2 minutes."));
Serial.print(" ");
Serial.println(F("During calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
Serial.print(" ");
Serial.println(F("If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
HC12.println(F("[ZIGNER] Before using 9DOF,we need to calibrate the compass frist,It will takes about 2 minutes."));
HC12.println("[ZIGNER] ");
HC12.println(F("[ZIGNER] During calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
HC12.println("[ZIGNER] ");
HC12.println(F("[ZIGNER] If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
String str="";
while(str!="ready"){
if(HC12.available()){
str = HC12.readString();
Serial.print("[PC] ");
Serial.print(str);
Serial.println("");
}
if(Serial.find("ready")){
break;
}
}
//while (!Serial.find("ready"));
Serial.println(" ");
Serial.println("[ZIGNER] ready");
Serial.println("[ZIGNER] Sample starting......");
Serial.println("[ZIGNER] waiting ......");
HC12.println(" ");
HC12.println("[ZIGNER] ready");
HC12.println("[ZIGNER] Sample starting......");
HC12.println("[ZIGNER] waiting ......");
get_calibration_Data ();
Serial.println(" ");
Serial.println("compass calibration parameter ");
Serial.print(mx_centre);
Serial.print(" ");
Serial.print(my_centre);
Serial.print(" ");
Serial.println(mz_centre);
Serial.println(" ");
HC12.println("[ZIGNER] ");
HC12.println("[ZIGNER] compass calibration parameter ");
HC12.print(mx_centre);
HC12.print(" ");
HC12.print(my_centre);
HC12.print(" ");
HC12.println(mz_centre);
HC12.println(" ");
}
void get_calibration_Data () //calibration값 (mx_centre)구하는 함수
{ HC12.print("[5%]");
for (int i = 0; i < sample_num_mdate; i++)
{
if(i%100 ==0){HC12.print("#");}
get_one_sample_date_mxyz();//샘플로 Compass Data받고
//샘플을 sample_num_mdate 만큼 받아서 최대값과 최소값을 구한다.
//mx_sample[]은 0:min, 1:max, 2:sample 이다.
if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; //find max value
if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];
if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; //find min value
if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];
}
mx_max = mx_sample[1];
my_max = my_sample[1];
mz_max = mz_sample[1];
mx_min = mx_sample[0];
my_min = my_sample[0];
mz_min = mz_sample[0];
mx_centre = (mx_max + mx_min) / 2; //최대값과 최소값으로 중앙값 구함
my_centre = (my_max + my_min) / 2;
mz_centre = (mz_max + mz_min) / 2;
HC12.println("");
}
void get_one_sample_date_mxyz()//Compass 데이터 1번 받고 샘플로 삼는다.
{
getCompass_Data();
mx_sample[2] = Mxyz[0];
my_sample[2] = Mxyz[1];
mz_sample[2] = Mxyz[2];
}
void getAccel_Data(void) //Accel 데이터 받기
{
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
Axyz[0] = (double) ax / 16384;
Axyz[1] = (double) ay / 16384;
Axyz[2] = (double) az / 16384;
}
void getGyro_Data(void) //Gyro 데이터 받기
{
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
Gxyz[0] = (double) gx * 250 / 32768;
Gxyz[1] = (double) gy * 250 / 32768;
Gxyz[2] = (double) gz * 250 / 32768;
}
void getCompass_Data(void) //Compass 데이터 받기
{
I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
delay(10);
I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);
mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;
Mxyz[0] = (double) mx * 1200 / 4096;
Mxyz[1] = (double) my * 1200 / 4096;
Mxyz[2] = (double) mz * 1200 / 4096;
}
void getCompassDate_calibrated () //Compass Calibrated 데이터 받기
{
getCompass_Data();
Mxyz[0] = Mxyz[0] - mx_centre;
Mxyz[1] = Mxyz[1] - my_centre;
Mxyz[2] = Mxyz[2] - mz_centre;
}