롤 제어

쿼드 콥터는 X자형이냐 +자형이냐에 따라 제어 방법이 다릅니다.

저는 +자형 쿼드 콥터를 만들기 때문에 롤 제어는 다음과 같이 합니다.

롤 제어 방법은 한 축의 두 개의 모터는 균형을 규지하면서 나머지 다른 축의 모터의 기울기를 통하여 비행하는 기울기를 결정합니다.

제가 테스트한 동영상의 방법은 일단 무선 조종기가 없었기 때문에 아두이노 IDE에서 시리얼 디버깅 창을 띄워놓고

한 쪽 축의 목표 기울기 값을 컴퓨터를 통하여 아두이노에 전송하여 기울기 방향을 정하였습니다.

동영상을 자세히 보면 기체가 한 쪽 방향으로 기울인 뒤 그 방향으로 움직이는 것을 볼 수 있어요.

#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
 
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
  
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
////////////////////////////////////////////
 
 
/////////////////IMU Sensor/////////////////
#define DATANUM 12 //6
#define ACC_X_OFFSET 0.18
#define ACC_Y_OFFSET 0.01
#define ACC_Z_OFFSET -0.77
//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;
double Acc_x, Acc_y, Acc_z;
////////////////////////////////////////////
 
 
/////////////////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;
double roll_p, roll_i, roll_d, roll_val;
double pitch_p, pitch_i, pitch_d, pitch_val;
double yaw_p, yaw_i, yaw_d, yaw_val;
int rollControll, pitchControll, yawControll;
 
int initial = 0;
////////////////////////////////////////////
 
/////////////////Serial Monitor/////////////////
int Serial_print_sig=1;
int roll_print = 1, pitch_print =0, yaw_print=0, hover_print=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==DATANUM+2)  // 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<DATANUM;n++) chk+=sdata[n]; 
  if(chk != get_data_word(&sdata[DATANUM])) 
  {
    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.;
 
  Acc_x = get_data_word(&sdata[6]) / 100. + ACC_X_OFFSET;
  Acc_y = get_data_word(&sdata[8]) / 100. + ACC_Y_OFFSET;
  Acc_z = get_data_word(&sdata[10]) / 100. + ACC_Z_OFFSET;
 
  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);
 
    roll_p = roll_Kp*roll_p_err;
    roll_i = roll_Ki*roll_i_err;
    roll_d = roll_Kd*roll_d_err;
    roll_val = 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);
    
    pitch_p = pitch_Kp*pitch_p_err;
    pitch_i = pitch_Ki*pitch_i_err;
    pitch_d = pitch_Kd*pitch_d_err;
    pitch_val = 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);
    
    yaw_p = yaw_Kp*yaw_p_err;
    yaw_i = yaw_Ki*yaw_i_err;
    yaw_d = yaw_Kd*yaw_d_err;
    yaw_val = val_out;
    
    return val_out;
}
//------------------------------------------------------------------------
//제어값을 받어서 모터 구동
void run_motors(int throttle, int rollOffset, int pitchOffset, int yawOffset)
{
  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);
}
 
void limit_max_min(int pwm)
{
  if(pwm>MAX_PWM)
  pwm = MAX_PWM;
  else if(pwm<MIN_PWM)
  pwm = MIN_PWM;
}
//-------------------------------------------------------------------------------//
void Serial_Monitor(void)
{
  if(Serial_print_sig ==1 ){
    if(roll_print == 1){
      Serial.print("Roll=> ");Serial.print(DegRoll);Serial.print(", ");Serial.print(roll_p); Serial.print(", ");Serial.print(roll_i); Serial.print(", ");Serial.print(roll_d);Serial.print(", ");Serial.print(roll_val);Serial.print(", ");Serial.print(motor2Power);Serial.print(", ");Serial.println(motor4Power);
    }else if(pitch_print == 1){
      Serial.print("Pitch=> ");Serial.print(DegPitch);Serial.print(", ");Serial.print(pitch_p); Serial.print(", ");Serial.print(pitch_i); Serial.print(", ");Serial.print(pitch_d);Serial.print(", ");Serial.print(pitch_val);Serial.print(", ");Serial.print(motor1Power);Serial.print(", ");Serial.println(motor3Power);
    }else if(yaw_print == 1){
      Serial.print("Yaw=> ");Serial.print(DegYaw);Serial.print(", ");Serial.print(yaw_p); Serial.print(", ");Serial.print(yaw_i); Serial.print(", ");Serial.print(yaw_d);Serial.print(", ");Serial.println(yaw_val); 
    }
  } 
 
  if(Serial_print_sig == 0){
    if(hover_print == 1){
      Serial.print("hover = ");Serial.println(hover);
      hover_print =0;
    }else if(roll_print == 1){
      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);
      roll_print = 0;
    }else if(pitch_print == 1){
      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);
      pitch_print = 0;
    }else if(yaw_print == 1){
      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);
      yaw_print = 0;
    }
  }
}
 
//------------------------------------------------------------------------------//
void from_computer(void)
{
    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);
    }else if(c=='5'){
      roll_setpoint += 0.1;
      Serial.print("roll_setpoint = ");Serial.println(roll_setpoint);
    }else if(c=='6'){
      roll_setpoint -= 0.1;
      Serial.print("roll_setpoint = ");Serial.println(roll_setpoint);
    }else if(c=='7'){
      roll_setpoint = INITROLLSETPOINT;
      Serial.print("roll_setpoint = ");Serial.println(roll_setpoint);
    }
 
    
    
    //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);
    }
 
    else if(c=='p'){
      Serial_print_sig = !(Serial_print_sig);
    }else if(c=='/'){
      roll_print = !(roll_print);
    }else if(c=='*'){
      pitch_print = !(pitch_print);
    }else if(c=='-'){
      yaw_print = !(yaw_print);
    }else if(c=='+'){
      hover_print = !(hover_print);
    }
    
}
 
//---------------------------------------------------------------------//
//
//                                main
//
//--------------------------------------------------------------------//
void setup() {
  Serial.begin(115200); 
  SensorSerial.begin(57600); 
  callibration();
}
 
void loop() {
 
  
  if(SensorSerial.available()){ //데이터 받기.
   if(recieve_data()==1)  //데이터 다 받음?
   {   
     if(trans_data_to_degree(SBuf)==1) //센서값. 
     {
      if(initial == 0){
      yaw_setpoint = DegYaw;
      initial = 1;
      //Serial.println(yaw_setpoint);
      }
             
       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); //MOTOR
 
       //Serial_Monitor(); //Serial.print.
       Serial.print(Acc_x);Serial.print(", ");Serial.print(Acc_y);Serial.print(", ");Serial.println(Acc_y);
      }
    } 
  }
 
   if(Serial.available()){ //통신입력?
    from_computer(); //입력받기.
   }
}