쿼드콥터프로젝트(16.1.12) - 시소테스트(Feat. PID control)

오늘의 목표 : 시소테스트를 통해 PID 제어를 완성

< 첫번째 시도 - 1월 8일 >

낮은 쓰로틀(30%이하)일 때에는 외란에 평형을 잘 맞추는 듯 했으나

점점 쓰로틀 값을 올려주니 한쪽으로 기울며 평형을 맞추지 못하게 되었다.

무엇이 원인일까 고민하다 여러 실험을 해보니 양 쪽의 모터에 동일한 PWM 값을 넣어줘도 회전속도가 차이가 난다는 것을 알게 되었다.

이를 어떻게 해결할까 고민했는데 멍청하게도 PID제어란 것이 이런 문제를 잡아주는 컨트롤이었다.

즉, PID 제어가 제대로 안되고 있다는 뜻이었다. (1월 8일날 시행)

<두번째 시도 - 1월 12일>

PID 제어에 대해 다시 공부하던 중 쿼드콥터에는 'PID 이중제어'를 사용한다는 사실을 찾았다.

단순히 error 값을 원하는 각도와의 각도 차이만이 아니라 거기에 더불어 이 각도 오차를 Sampling Time으로 나눠줘 

각속도로 변환시킨 후 PID 제어를 사용한다는 것이었다.

이를 적용시켜 코드를 변경시켜 주고 넣어보니 훨씬 안정적인 시스템이 되었다. 이제 Roll 평형을 맞추는 것을 해주는 단계로 넘어가야겠다.

+ 모터의 진동으로 인한 센서 오차를 줄이기 위해 LPF(Low Pass Filter)를 적용시켰다.

 ( 약간 흔들림이 적어졌다 )

* tau = 시정수로 0.01 정도로 설정

* dt = 센서 측정 시간

------------------------------------------------------------------------------------------------------------------------------------

[전체 코드]

#include "Wire.h"

#include "MPU6050.h"

#include "I2Cdev.h"

#include <Servo.h>

/* PID gain*/

#define Roll_Pgain_out 4.0

#define Roll_Pgain 0.12u

#define Roll_Dgain 0.03

#define Roll_Igain 0.3

double Roll_base=0;

double PIDdt;

#define Pitch_Pgain_out 4.0

#define Pitch_Pgain 0.1

#define Pitch_Dgain 0.03

#define Pitch_Igain 0.3

double Pitch_base=0;

double Roll_value = 0 ; double Roll_Ang_error = 0 ; double Roll_prev_error = 0; double Roll_prev_Ang_error=0;

double Roll_Pvalue, Roll_Dvalue, Roll_Ivalue = 0; double Roll_I_error=0; double Roll_Rate_error=0;

double Roll_prevtime=0;

double Pitch_value = 0 ; double Pitch_Ang_error = 0 ; double Pitch_prev_error = 0; double Pitch_prev_Ang_error=0;

double Pitch_Pvalue, Pitch_Dvalue, Pitch_Ivalue = 0; double Pitch_I_error=0;  double Pitch_Rate_error=0;

double Pitch_prevtime=0;

//ESC 핀 할당

#define ESC1 3  // 흰

#define ESC2 9  // 파

#define ESC3 10  // 빨

#define ESC4 11 // 노

#define MAX_SIGNAL 2000  // 최대 PWM 2000

#define MIN_SIGNAL 800  // 최소 PWM 800

//각 모터 할당

Servo motor1;  // 모터 1,3 x축 (Pitch)

Servo motor2;  // 모터 2,4 y축 (Roll)

Servo motor3;

Servo motor4;

double motor1_power;

double motor2_power;

double motor3_power;

double motor4_power;

#define CH1_int 0  // 인터럽트 핀 할당 (Throttle)

#define CH1_pin 2

#define dt1 20

#define valid_pulse_limit 2000 // [uS] Valid output high pulse time limit for RC controller

#define max_high_time 1895 // [uS] Maximum expected high time

#define min_high_time 1200 // [uS] Minimum expected high time

volatile unsigned long CH1_t = 0, CH1_delta = 0;

unsigned long t = 0;

double CH1;

double Throttle = 0;

//MPU6050에 필요한 값들

#define pi 3.141592

#define RADIANS_TO_DEGREES 180/3.141592

#define fs 131.0;

MPU6050 mpu;

int16_t ax, ay, az;

int16_t gx, gy, gz;

int16_t mx, my, mz;

double dt; 

//자이로(각속도)

double gyro_x, gyro_y;

//최종 가속도,자이로 각도

double accel_x, accel_y ;

double gyro_angle_x = 0, gyro_angle_y = 0;

//상보필터 거친 각도

double angle_x = 0, angle_y = 0, angle_z = 0;

// LPF 변수

double LPF_angle_x , LPF_angle_y , LPF_prevangle_x, LPF_prevangle_y =0 ;

double tau=0.01;

//자이로센서 바이어스값

double base_gx = 0, base_gy = 0, base_gz = 0;

unsigned long pre_msec = 0;

/*인터럽트 함수*/

void CH1_int_ISR()

{

  if ((micros() - CH1_t) < valid_pulse_limit) {

    CH1_delta = micros() - CH1_t;

  }

  CH1_t = micros();

}

//초기값 설정 함수

void calibrate() {

  int loop = 20;

  for (int i = 0; i < loop; i++)

  {

    mpu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

    base_gx += gx;

    base_gy += gy;

    base_gz += gz;

    delay(80);

  }

  base_gx /= loop;

  base_gy /= loop;

  base_gz /= loop;

}

/* mpu6050 센싱 함수*/

void sensor() {

  //단위시간 변화량

  dt = (millis() - pre_msec) / 1000.0;

  pre_msec = millis();

  mpu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

  //가속도값 아크탄젠트->각도변환

  accel_x = atan(ay / sqrt(pow(ax, 2) + pow(az, 2))) * RADIANS_TO_DEGREES;

  accel_y = atan(-1 * ax / sqrt(pow(ay, 2) + pow(az, 2))) * RADIANS_TO_DEGREES;

  //자이로 -32766~+32766을 실제 250degree/s로 변환

  //앞에서 계산한 오차의 평균값을 빼줌

  gyro_x = (gx - base_gx) / fs;

  gyro_y = (gy - base_gy) / fs;

  //변화량을 적분

  gyro_angle_x = angle_x + dt * gyro_x;

  gyro_angle_y = angle_y + dt * gyro_y;

  //상보필터

  angle_x = 0.95 * gyro_angle_x + 0.05 * accel_x;

  angle_y = 0.95 * gyro_angle_y + 0.05 * accel_y;

  // LPF

  LPF_angle_x = (tau*LPF_prevangle_x + dt*angle_x)/(tau+dt) ;

  LPF_angle_y = (tau*LPF_prevangle_y + dt*angle_y)/(tau+dt) ;

  LPF_prevangle_x = LPF_angle_x;

  LPF_prevangle_y = LPF_angle_y ;

  Serial.print("angle_x:");

  Serial.print(angle_x);

  Serial.print("   ");

  Serial.print("angle_y:");

  Serial.print(angle_y);

  Serial.print("   ");

  Serial.print("LPF_angle_x:");

  Serial.print(LPF_angle_x);

  Serial.print("   ");

  Serial.print("LPF_angle_y:");

  Serial.println(LPF_angle_y);

}

void PID_control_Roll(){

  double Roll_currenttime=millis();

  PIDdt=(Roll_currenttime-Roll_prevtime)/1000;

  Roll_Ang_error = Roll_base -  angle_x ;

  Roll_Rate_error= Roll_Ang_error * Roll_Pgain_out - (Roll_Ang_error - Roll_prev_Ang_error) / PIDdt  ;

  Roll_I_error += Roll_Rate_error* PIDdt;

  Roll_Ivalue = Roll_Igain * Roll_I_error;

  Roll_Dvalue = Roll_Dgain * (Roll_Rate_error - Roll_prev_error) / PIDdt  ;

  Roll_value = Roll_Pvalue + Roll_Ivalue + Roll_Dvalue;

  Roll_prev_error = Roll_Rate_error;

  Roll_prevtime=Roll_currenttime;

  Roll_prev_Ang_error = Roll_Ang_error;

}

void PID_control_Pitch(){

  double Pitch_currenttime=millis();

  PIDdt=(Pitch_currenttime-Pitch_prevtime)/1000;

  Pitch_Ang_error = Pitch_base -  angle_y ;

  Pitch_Rate_error= Pitch_Ang_error * Pitch_Pgain_out - (Pitch_Ang_error - Pitch_prev_Ang_error) / PIDdt  ;

  Pitch_I_error += Pitch_Rate_error* PIDdt;

  Pitch_Ivalue = Pitch_Igain * Pitch_I_error;

  Pitch_Dvalue = (Pitch_Rate_error - Pitch_prev_error) / PIDdt * Pitch_Dgain ;

  Pitch_value =  Pitch_Pvalue + Pitch_Ivalue + Pitch_Dvalue;

  Pitch_prev_error = Pitch_Rate_error;

  Pitch_prevtime=Pitch_currenttime;

  Pitch_prev_Ang_error = Pitch_Ang_error;

}

void setup() {

  Serial.begin(9600);

  pinMode(CH1_pin, INPUT);

  attachInterrupt(CH1_int, CH1_int_ISR, CHANGE);

  motor1.attach(ESC1); // 각 모터들 연결

  motor2.attach(ESC2);

  motor3.attach(ESC3);

  motor4.attach(ESC4);

  // mpu 데이터관련 setting

  Wire.begin();

  mpu.initialize();

  calibrate();

  sensor();

  Roll_prev_error=0;

  Pitch_prev_error=0;

}

void loop() {

  CH1 = ((double)CH1_delta - (double)min_high_time) * 255 / (max_high_time - min_high_time);

  if (CH1 > 255) {

    CH1 = 255;

  }

  Throttle = 800 + ((CH1 - 13) / 242) * 1200; // 조종기로부터 받는 Throttle 값

  sensor(); // 센싱

  //PID 제어

   PID_control_Roll();

   PID_control_Pitch();

  

  motor1_power = Throttle - 0.6*Pitch_value;

  motor3_power = Throttle + 0.6*Pitch_value;

  motor2_power = Throttle + 0.6 * Roll_value;

  motor4_power = Throttle - 0.6 * Roll_value; 

  if(Throttle<851){ motor1_power=800; motor2_power=800; motor3_power=800; motor4_power=800;}

  

  motor1.write((int)motor1_power);

  motor3.write((int)motor3_power);

  

  motor2.write((int)motor2_power);

  motor4.write((int)motor4_power);

}

------------------------------------------------------------------------------------------------------------------------------------

다음 목표 : Roll 축 PID 제어 성공 + 비행시도