호버링 제어

1. PID 테스트

날개 4개 중에서 한 축인 2개의 모터만 돌려서 우선 균형을 잡을 수 있는 지 PID 테스트를 해보았습니다.

PID 제어란 목표인 입력값과 현재 상태인 결과 사이의 차이(Error)에 비례, 적분, 미분의 기법을 써서 구동부를 제어하는 방법입니다.

기본적인 PID 제어 방법과 코드는 아래와 같아요.

기초: PID 제어

#define Kp (2) // configurable value.

#define Ki (1) // configurable value.

#define Kd (1) // configurable value.

#define dt   0.01

double pid(int degree)

{

 static double P_err_prv; // Prev P error to get D_err.

 double P_err, I_err, D_err; // Basic Error values.

 P_err = SETPOINT - degree;

 I_err += P_err * dt;

 D_err = (P_err - P_err_prv) / dt;

 P_err_prv = P_err; // backup P_err.

 val_out = (Kp * P_err) + (Ki * I_err) + (Kd * D_err);

 return val_out; // Calculated.

}

균형을 잡는 지 저희 쿼드콥터로 테스트 하기 위해서 디버깅 환경을 다음과 같이 꾸몄습니다.

#include "SoftwareSerial.h"
#include "Servo.h"
 
/////////////////BLDC Motor/////////////////
#define MAX_THROTTLE 179
#define MIN_THROTTLE 0
#define MAX_PWM 100
#define MIN_PWM 20
#define MOTOR0 5  //motor pin number
#define MOTOR1 9  //motor pin number
#define MOTOR2 10  //motor pin number
#define MOTOR3 3 //motor pin number
 
Servo motor0;
Servo motor1;
Servo motor2;
Servo motor3;
int mspeed=0;
int pwm=0, Cpwm=0;
////////////////////////////////////////////
 
 
/////////////////IMU Sensor/////////////////
//IMU Sensor Serial
SoftwareSerial SensorSerial(2,4);//Rx Tx
//IMU Sensor variable
char data[40];
int i = 0;
double  roll, pitch, yaw;
////////////////////////////////////////////
 
 
/////////////////PID Conrtroll/////////////////
#define SETPOINT  0
#define INITfitpoint  30
#define INITKp  0.38
#define INITKi 0.000287
#define INITKd 33
#define MAXSPEED  3
#define MINSPEED  100
 
int fitpoint = INITfitpoint;
double Kp = INITKp, Ki = INITKi, Kd = INITKd;
double last_t;
////////////////////////////////////////////
 
 
//---------------------------------------------------------------------//
//
//                              funtions
//
//--------------------------------------------------------------------//
 
//esc조절 하기
void callibration(void)
{
  
  Serial.println("Calibrate ESC? 'y' or 'n'.");
    
  while (!Serial.available());
  char cali = Serial.read();
  if(cali == 'y'){  
    Serial.println("send max throttle");
    
    motor0.attach(MOTOR0);
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor0.write(MAX_THROTTLE);
    motor1.write(MAX_THROTTLE);
    motor2.write(MAX_THROTTLE);
    motor3.write(MAX_THROTTLE);
   
    delay(4000);
 
     Serial.println("send min throttle");
    motor0.attach(MOTOR0);
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor0.write(MIN_THROTTLE);
    motor1.write(MIN_THROTTLE);
    motor2.write(MIN_THROTTLE);
    motor3.write(MIN_THROTTLE);
 
    delay(2000);
    
    Serial.println("callibration complete");
    
  }else if(cali == 'n'){
    motor0.attach(MOTOR0);
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor0.write(MIN_THROTTLE);
    motor1.write(MIN_THROTTLE);
    motor2.write(MIN_THROTTLE);
    motor3.write(MIN_THROTTLE);
   
  }
  Serial.println("If you want Program keep going. Press any key.");
  while (!Serial.available());
}
 
//------------------------------------------------------------------------//
//센서값을 읽어서 roll,pitch,yaw 값을 알아낸다.
//IMU 센서에서 받아온 아스키코드 값을 잘라서 숫자로 저장하는 함수
//trans_ascii_to_num(저장할 버퍼 배열, 롤값 저장할 변수, 피치값 저장할 변수, 요값 저장할 변수)
void trans_ascii_to_num(char * SBuf, double * roll_ptr, double * pitch_ptr, double * yaw_ptr)
{  
  int value, value2;
  
  value=FindComma(SBuf);// ',' 위치(인덱스 값)을 반환?
  SBuf[value]='\0';// ','를 NULL(\0)값으로 바꾸어주어 하나의 스트링배열로 만들어주는듯?(NULL은 스트링의 마지막 값이니까)
  *roll_ptr=atof(SBuf); 
  //atof()는 스트링을 double형으로 바꾸어주는 함수, 소수점도 알아서 인식하고 바꾸어준다.
        
  value++;
  value2=FindComma(&SBuf[value]);
  SBuf[value+value2]='\0';
  *pitch_ptr=atof(&SBuf[value]);
    
  value=value+value2+1;
  value2=FindComma(&SBuf[value]);
  SBuf[value+value2]='\0';
  *yaw_ptr=atof(&SBuf[value]);
 
}
 
int FindComma(char * buf)
{
   int n;
   for(n=0;n<40;n++)
    {
     if(buf[n]==',') break;
    }
 
   return n;
}
 
//--------------------------------------------------------------------------------//
//각도 값을 받아서 PID제어값 출력
double pid(double degree)
{
    double val_out;
    static double p_err_prv, p_err, i_err, d_err;
 
    p_err = SETPOINT-degree;
    i_err += p_err;
    d_err = p_err - p_err_prv;
    
    p_err_prv = p_err;
    val_out = (Kp*p_err) + (Ki*i_err) + (Kd*d_err);
    
    Serial.print((Kp*p_err)); Serial.print(", ");Serial.print((Ki*i_err)); Serial.print(", ");Serial.print((Kd*d_err));Serial.print(", ");Serial.print(val_out); 
    return val_out;
}
 
//------------------------------------------------------------------------
//제어값을 받어서 모터 구동
void run_motors(int speed1, int speed2)
{
  pwm = speed1 + speed2; 
  Cpwm = speed1 - speed2;
  
  if(pwm>MAX_PWM)
  pwm = MAX_PWM;
  else if(pwm<MIN_PWM)
  pwm = MIN_PWM;
 
  if(Cpwm>MAX_PWM)
  Cpwm = MAX_PWM;
  else if(Cpwm<MIN_PWM)
  Cpwm = MIN_PWM;
 
  motor1.write(pwm); 
  motor3.write(Cpwm);
 
  Serial.print(", ");Serial.print(pwm);Serial.print(", ");Serial.print(Cpwm);Serial.println("");
}
 
//------------------------------------------------------------------------------//
//pid 계수의 값을 바꾼다.
void modify_pid_val(void)
{
   if(Serial.available()){
    char c = Serial.read();
    if(c=='r'){
      fitpoint += 1;
      Serial.print("fitpoint = ");Serial.println(fitpoint);
    }else if(c=='t'){
      fitpoint -= 1;
      Serial.print("fitpoint = ");Serial.println(fitpoint);
    }else if(c=='y'){
      fitpoint= INITfitpoint;
      Serial.print("fitpoint = ");Serial.println(fitpoint);
    }else if(c=='q'){
      Kp += 0.01;
      Serial.print("Kp = ");Serial.println(Kp);
    }else if(c=='w'){
      Kp -= 0.01;
      Serial.print("Kp = ");Serial.println(Kp);
    }else if(c=='e'){
      Kp= INITKp;
      Serial.print("Kp = ");Serial.println(Kp);
    }else if(c=='a'){
      Ki += 0.000001;
      Serial.print("Ki = ");Serial.println(Ki*10000);
    }else if(c=='s'){
      Ki -= 0.000001;
      Serial.print("Ki = ");Serial.println(Ki*10000);
    }else if(c=='d'){
      Ki = INITKi;
      Serial.print("Ki = ");Serial.println(Ki*10000);
    }else if(c=='z'){
      Kd += 1;
      Serial.print("Kd = ");Serial.println(Kd);
    }else if(c=='x'){
      Kd -= 1;
      Serial.print("Kd = ");Serial.println(Kd);
    }else if(c=='c'){
      Kd = INITKd;
      Serial.print("Kd = ");Serial.println(Kd);
    }else if(c=='p'){
      Serial.print("Kp = ");Serial.print(Kp);Serial.print(", Ki = ");Serial.print(Ki*10000);Serial.print(", Kd = ");Serial.println(Kd);   
    }
  } 
}
 
//---------------------------------------------------------------------//
//
//                                main
//
//--------------------------------------------------------------------//
void setup() {
  Serial.begin(115200); 
  SensorSerial.begin(57600); 
  callibration();
}
 
void loop() {
  if(SensorSerial.available()){
    data[i]= SensorSerial.read();
    i++;
   
    if(data[i-1] == 0x0A)
    data[i-1] = ',';
    
    if(data[i-1] == '*'){//데이터 다받음.
     i=0;
     
     trans_ascii_to_num(data, &roll, &pitch, &yaw);//센서값을 읽어서 roll,pitch,yaw 값을 알아낸다. 
     Serial.print(roll);Serial.print(", ");
     mspeed = pid(roll);//각도 값을 받아서 PID제어값 출력
     run_motors(fitpoint, mspeed);//제어값을 받어서 모터 구동
     
    }
   }
 
    modify_pid_val();
}
  
 

2. 호버링 제어

다음으로 위의 PID 코드를 수정하여 두 축의 균형을 잡을 수 있도록 소스를 수정 한 후 모터 4개를 모두 돌려서 테스트 하였습니다.

기체가 균형을 잡는지, 공중에 뜨는 지를 확인 하였는데 생각보다 안정적이지 않았습니다.

#include "SoftwareSerial.h"
#include "Servo.h"
 
/////////////////BLDC Motor/////////////////
#define MAX_THROTTLE 179
#define MIN_THROTTLE 0
#define MAX_PWM 100
#define MIN_PWM 20
#define MOTOR1 5  //motor pin number
#define MOTOR2 9  //motor pin number
#define MOTOR3 10  //motor pin number
#define MOTOR4 3 //motor pin number
 
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
////////////////////////////////////////////
 
 
/////////////////IMU Sensor/////////////////
//IMU Sensor Serial
SoftwareSerial SensorSerial(2,4);//Rx Tx
//IMU Sensor variable
typedef union {
  signed short value;
  unsigned char byte[2];
} S2BYTE;
 
S2BYTE data_word;
 
unsigned char SBuf[100];
int SCnt=0;
double  DegRoll, DegPitch, DegYaw;
////////////////////////////////////////////
 
 
/////////////////PID Conrtroll/////////////////
#define MINhover 0
#define INIThover  20
 
#define INITROLLKp 0.38
#define INITROLLKi 0.000287 
#define INITROLLKd 33
 
#define INITPITCHKp 0.29
#define INITPITCHKi 0.000287 
#define INITPITCHKd 33
 
#define INITYAWKp 0.29 //0.38
#define INITYAWKi 0.000287 
#define INITYAWKd 33
 
#define MAXSPEED  3
#define MINSPEED  100
 
#define INITROLLSETPOINT -1.532
#define INITPITCHSETPOINT -0.516
 
 
double roll_setpoint = INITROLLSETPOINT, pitch_setpoint = INITPITCHSETPOINT, yaw_setpoint = 0;
int hover = INIThover;
double roll_Kp = INITROLLKp, roll_Ki = INITROLLKi, roll_Kd = INITROLLKd;
double pitch_Kp = INITPITCHKp, pitch_Ki = INITPITCHKi, pitch_Kd = INITPITCHKd;
double yaw_Kp = INITYAWKp, yaw_Ki = INITYAWKi, yaw_Kd = INITYAWKd;
int rollControll, pitchControll, yawControll;
 
int initial = 0;
////////////////////////////////////////////
 
 
//---------------------------------------------------------------------//
//
//                              funtions
//
//--------------------------------------------------------------------//
 
//esc조절 하기
void callibration(void)
{
  
  Serial.println("Calibrate ESC? 'y' or 'n'.");
    
  while (!Serial.available());
  char cali = Serial.read();
  if(cali == 'y'){  
   Serial.println("send max throttle");
    
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor4.attach(MOTOR4);
    motor1.write(MAX_THROTTLE);
    motor2.write(MAX_THROTTLE);
    motor3.write(MAX_THROTTLE);
    motor4.write(MAX_THROTTLE);
   
    delay(5000);
 
    Serial.println("send min throttle");
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor4.attach(MOTOR4);
    motor1.write(MIN_THROTTLE);
    motor2.write(MIN_THROTTLE);
    motor3.write(MIN_THROTTLE);
    motor4.write(MIN_THROTTLE);
 
    delay(2000);
    
    Serial.println("callibration complete");
    
  }else if(cali == 'n'){
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor4.attach(MOTOR4);
    motor1.write(MIN_THROTTLE);
    motor2.write(MIN_THROTTLE);
    motor3.write(MIN_THROTTLE);
    motor4.write(MIN_THROTTLE);
   
  }
  Serial.println("If you want Program keep going. Press any key.");
  while (!Serial.available());
}
 
//------------------------------------------------------------------------//
//센서값을 읽어서 roll,pitch,yaw 값을 알아낸다.
int recieve_data(void)
{
    static int step_=0, check_all_recieve=0;
    unsigned char data1byte;
    int n;
 
    data1byte = SensorSerial.read();
    
    switch(step_)
    {
        case 0:
           if(data1byte==0x55) {step_=1, check_all_recieve=0;}
        break;
 
        case 1:
           if(data1byte==0x55) { step_=2;  SCnt=0;}
           else step_=0;
        break;
 
        case 2:
           SBuf[SCnt++] = (unsigned char)data1byte;
           if(SCnt==8)  // roll(2byte),pitch(2byte),yaw(2byte),checksum(2byte) 
             {   
                step_=0;
                SCnt=0;
                check_all_recieve = 1;
             }
        break;
 
        default:
        break;
     }
   
    return check_all_recieve;
}
 
static int trans_data_to_degree(unsigned char *sdata)  // sdata : roll(2)+pitch(2)+yaw(2)+chk(2)
{
  int n;
  unsigned short chk=0;
 
  ///////////// checksum /////////////////
  chk=0x55+0x55;
  for(n=0;n<6;n++) chk+=sdata[n]; 
  if(chk != get_data_word(&sdata[6])) 
  {
    Serial.println("checksum error!");
    return 0; //   
  }
  ////////////////////////////////////////
 
  DegRoll  = get_data_word(&sdata[0]) / 100.;
  DegPitch = get_data_word(&sdata[2]) / 100. * -1;
  DegYaw   = get_data_word(&sdata[4]) / 100.;
 
  return 1;
}
 
static signed short get_data_word(unsigned char *dat)
{
  // little endian..
  data_word.byte[0]=dat[1];  
  data_word.byte[1]=dat[0];  
 
  return data_word.value;
}
 
//--------------------------------------------------------------------------------//
//각도 값을 받아서 PID제어값 출력
double roll_pid(double setpoint, double degree)
{
    double val_out;
    static double roll_p_err_prv, roll_p_err, roll_i_err, roll_d_err;
 
    roll_p_err = setpoint-degree;
    roll_i_err += roll_p_err;
    roll_d_err = roll_p_err - roll_p_err_prv;
    
    roll_p_err_prv = roll_p_err;
    val_out = (roll_Kp*roll_p_err) + (roll_Ki*roll_i_err) + (roll_Kd*roll_d_err);
    
    Serial.print((roll_Kp* roll_p_err)); Serial.print(", ");Serial.print((roll_Ki* roll_i_err)); Serial.print(", ");Serial.print((roll_Kd* roll_d_err));Serial.print(", ");Serial.print(val_out); 
    return val_out;
}
 
double pitch_pid(double setpoint, double degree)
{
    double val_out;
    static double pitch_p_err_prv, pitch_p_err, pitch_i_err, pitch_d_err;
 
    pitch_p_err = setpoint-degree;
    pitch_i_err += pitch_p_err;
    pitch_d_err = pitch_p_err - pitch_p_err_prv;
    
    pitch_p_err_prv = pitch_p_err;
    val_out = (pitch_Kp*pitch_p_err) + (pitch_Ki*pitch_i_err) + (pitch_Kd*pitch_d_err);
    
    //Serial.print((pitch_Kp* pitch_p_err)); Serial.print(", ");Serial.print((pitch_Ki* pitch_i_err)); Serial.print(", ");Serial.print((pitch_Kd* pitch_d_err));Serial.print(", ");Serial.print(val_out); 
    return val_out;
}
 
double yaw_pid(double setpoint, double degree)
{
    double val_out;
    static double yaw_p_err_prv, yaw_p_err, yaw_i_err, yaw_d_err;
 
    yaw_p_err = setpoint-degree;
    yaw_i_err += yaw_p_err;
    yaw_d_err = yaw_p_err - yaw_p_err_prv;
    
    yaw_p_err_prv = yaw_p_err;
    val_out = (yaw_Kp*yaw_p_err) + (yaw_Ki*yaw_i_err) + (yaw_Kd*yaw_d_err);
    
    //Serial.print((yaw_Kp*yaw_p_err)); Serial.print(", ");Serial.print((yaw_Ki*yaw_i_err)); Serial.print(", ");Serial.print((yaw_Kd*yaw_d_err));Serial.print(", ");Serial.print(val_out); 
    return val_out;
}
//------------------------------------------------------------------------
//제어값을 받어서 모터 구동
void run_motors(int throttle, int rollOffset, int pitchOffset, int yawOffset)
{
  int motor1Power, motor3Power; // Motors on the X axis
  //=>; changing their speeds will affect the pitch
  int motor2Power, motor4Power; // Motors on the Y axis
  //=>; changing their speeds will affect the roll
 
  motor1Power = throttle + pitchOffset - yawOffset;
  motor2Power = throttle + rollOffset + yawOffset;
  motor3Power = throttle - pitchOffset  - yawOffset;
  motor4Power = throttle - rollOffset + yawOffset;
  
  limit_max_min(motor1Power);
  limit_max_min(motor2Power);
  limit_max_min(motor3Power);
  limit_max_min(motor4Power);
 
  motor1.write(motor1Power);motor3.write(motor3Power);
  motor2.write(motor2Power);motor4.write(motor4Power);
 
  //Serial.print(", ");Serial.print(motor1Power);Serial.print(", ");Serial.print(motor3Power);
  Serial.print(", ");Serial.print(motor2Power);Serial.print(", ");Serial.print(motor4Power);
}
 
void limit_max_min(int pwm)
{
  if(pwm>MAX_PWM)
  pwm = MAX_PWM;
  else if(pwm<MIN_PWM)
  pwm = MIN_PWM;
}
//------------------------------------------------------------------------------//
//pid 계수의 값을 바꾼다.
void modify_pid_val(void)
{
   if(Serial.available()){
    
    char c = Serial.read();
    //hover debuging
    if(c=='1'){
      hover += 1;
      Serial.print("hover = ");Serial.println(hover);
    }else if(c=='2'){
      hover -= 1;
      Serial.print("hover = ");Serial.println(hover);
    }else if(c=='3'){
      hover= INIThover;
      Serial.print("hover = ");Serial.println(hover);
    }else if(c=='4'){
      hover= MINhover;
      Serial.print("hover = ");Serial.println(hover);
    }
    
    
    //roll debuging
    else if(c=='q'){
      roll_Kp += 0.01;
      Serial.print("roll_Kp = ");Serial.println(roll_Kp);
    }else if(c=='w'){
      roll_Kp -= 0.01;
      Serial.print("roll_Kp = ");Serial.println(roll_Kp);
    }else if(c=='e'){
      roll_Kp= INITROLLKp;
      Serial.print("roll_Kp = ");Serial.println(roll_Kp);
    }else if(c=='a'){
      roll_Ki += 0.000001;
      Serial.print("roll_Ki = ");Serial.println(roll_Ki*10000);
    }else if(c=='s'){
      roll_Ki -= 0.000001;
      Serial.print("roll_Ki = ");Serial.println(roll_Ki*10000);
    }else if(c=='d'){
      roll_Ki = INITROLLKi;
      Serial.print("roll_Ki = ");Serial.println(roll_Ki*10000);
    }else if(c=='z'){
      roll_Kd += 1;
      Serial.print("roll_Kd = ");Serial.println(roll_Kd);
    }else if(c=='x'){
      roll_Kd -= 1;
      Serial.print("roll_Kd = ");Serial.println(roll_Kd);
    }else if(c=='c'){
      roll_Kd = INITROLLKd;
      Serial.print("roll_Kd = ");Serial.println(roll_Kd);
    }
    
    
    
    //pitch debuging
    else if(c=='r'){
      pitch_Kp += 0.01;
      Serial.print("pitch_Kp = ");Serial.println(pitch_Kp);
    }else if(c=='t'){
      pitch_Kp -= 0.01;
      Serial.print("pitch_Kp = ");Serial.println(pitch_Kp);
    }else if(c=='y'){
      pitch_Kp= INITPITCHKp;
      Serial.print("pitch_Kp = ");Serial.println(pitch_Kp);
    }else if(c=='f'){
      pitch_Ki += 0.000001;
      Serial.print("pitch_Ki = ");Serial.println(pitch_Ki*10000);
    }else if(c=='g'){
      pitch_Ki -= 0.000001;
      Serial.print("pitch_Ki = ");Serial.println(pitch_Ki*10000);
    }else if(c=='h'){
      pitch_Ki = INITPITCHKi;
      Serial.print("pitch_Ki = ");Serial.println(pitch_Ki*10000);
    }else if(c=='v'){
      pitch_Kd += 1;
      Serial.print("pitch_Kd = ");Serial.println(pitch_Kd);
    }else if(c=='b'){
      pitch_Kd -= 1;
      Serial.print("pitch_Kd = ");Serial.println(pitch_Kd);
    }else if(c=='n'){
      pitch_Kd = INITPITCHKd;
      Serial.print("pitch_Kd = ");Serial.println(pitch_Kd);
    }
 
    //yaw debuging
    else if(c=='u'){
      yaw_Kp += 0.01;
      Serial.print("yaw_Kp = ");Serial.println(yaw_Kp);
    }else if(c=='i'){
      yaw_Kp -= 0.01;
      Serial.print("yaw_Kp = ");Serial.println(yaw_Kp);
    }else if(c=='o'){
      yaw_Kp= INITYAWKp;
      Serial.print("yaw_Kp = ");Serial.println(yaw_Kp);
    }else if(c=='j'){
      yaw_Ki += 0.000001;
      Serial.print("yaw_Ki = ");Serial.println(yaw_Ki*10000);
    }else if(c=='k'){
      yaw_Ki -= 0.000001;
      Serial.print("yaw_Ki = ");Serial.println(yaw_Ki*10000);
    }else if(c=='l'){
      yaw_Ki = INITYAWKi;
      Serial.print("yaw_Ki = ");Serial.println(yaw_Ki*10000);
    }else if(c=='m'){
      yaw_Kd += 1;
      Serial.print("yaw_Kd = ");Serial.println(yaw_Kd);
    }else if(c==','){
      yaw_Kd -= 1;
      Serial.print("yaw_Kd = ");Serial.println(yaw_Kd);
    }else if(c=='.'){
      yaw_Kd = INITYAWKd;
      Serial.print("yaw_Kd = ");Serial.println(yaw_Kd);
    }
    
    
  } 
}
 
//---------------------------------------------------------------------//
//
//                                main
//
//--------------------------------------------------------------------//
void setup() {
  Serial.begin(115200); 
  SensorSerial.begin(57600); 
  callibration();
}
 
void loop() {
 
  
  if(SensorSerial.available()){
   if(recieve_data()==1)  //roll(2byte),pitch(2byte),yaw(2byte),checksum(2byte) 모든 데이터 받았음
    {   
     if(trans_data_to_degree(SBuf)==1) //센서값을 읽어서 roll,pitch,yaw 값을 알아낸다. 
     {
      if(initial == 0){
      yaw_setpoint = DegYaw;
      initial = 1;
      //Serial.println(yaw_setpoint);
      }
 
       Serial.print(DegRoll);Serial.print(", ");
       //Serial.print(DegPitch);Serial.print(", ");
       //Serial.print(DegYaw); Serial.print(", ");
             
       rollControll = roll_pid(roll_setpoint, DegRoll);//각도 값을 받아서 PID제어값 출력
       pitchControll = pitch_pid(pitch_setpoint, DegPitch);
       yawControll = yaw_pid(yaw_setpoint, DegYaw);
    
       run_motors(hover, rollControll, pitchControll, yawControll);//제어값을 받어서 모터 구동
       Serial.print(", hover = ");Serial.print(hover);
       
       Serial.print(", roll_Kp = ");Serial.print(roll_Kp);Serial.print(", roll_Ki = ");Serial.print(roll_Ki*10000);Serial.print(", roll_Kd = ");Serial.println(roll_Kd);
 
       //Serial.print(", pitch_Kp = ");Serial.print(pitch_Kp);Serial.print(", pitch_Ki = ");Serial.print(pitch_Ki*10000);Serial.print(", pitch_Kd = ");Serial.println(pitch_Kd);
 
       //Serial.print(", yaw_Kp = ");Serial.print(yaw_Kp);Serial.print(", yaw_Ki = ");Serial.print(yaw_Ki*10000);Serial.print(", yaw_Kd = ");Serial.println(yaw_Kd);
      }
    } 
  }
 
 
    modify_pid_val();
}