[드론] 아두이노에서 PID출력을 프로세싱으로 보내기

아두이노 IDE에서 프로세싱으로 PID 출력 보내기

목표

Arduino IDE에서 PID 출력 값을 Processing IDE로 보내기

소스코드

/*
 *  목표 : 1. 프로세싱을 이용해서 GUI 형태의 그래프 형태로 PID 출력 값 살펴보기
 *         2. 프로세싱에서 제공하느 슬라이더를 이용해서 PID 매개변수(게인)를 조절  
 */
#include 
   
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
   
void setup() {
  initMPU6050(); //가속도 자이로 센서 값을 읽음
  Serial.begin(115200);
  calibAccelGyro(); //센서 보정 루틴
  initDT();// 시간 간격에 대한 초기화
}
   
void loop() {
  readAccelGyro();
  calcDT(); //시간 간격 계산
  calcAccelYPR(); //가속도 센서 처리 루틴
  calcGyroYPR(); //자이로 센서 처리 루틴 --> 가속도 센서의 값을 해석하기 위해 Roll, Pitch, Yaw에 대한 각도 구하는 함수
  calcFilteredYPR(); //상보필터 적용
  calcYPRtoStdPID();
 
  static int cnt;
  cnt++;
  if(cnt%2==0)
    SendDataToProcessing(); //Roll, Pitch, Yaw에 대한 각도 정보를 보냄
}
  
//MPU-6050초기화 루틴
void initMPU6050()
{
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
}
  
 //가속도 자이로 센서를 읽는 루틴
void readAccelGyro()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();
  Tmp = Wire.read() << 8 | Wire.read();
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();
}
  
float dt;
float accel_angle_x, accel_angle_y, accel_angle_z;
//float gyro_angle_x, gyro_angle_y, gyro_angle_z;
float filtered_angle_x, filtered_angle_y, filtered_angle_z;
float baseAcX, baseAcY, baseAcZ;
float baseGyX, baseGyY, baseGyZ;
extern float roll_output, pitch_output, yaw_output;
  
//센서 들의 기본값들의 평균을 내야하는 루틴(센서 보정 루틴)
void calibAccelGyro() 
{
  float sumAcX = 0, sumAcY = 0, sumAcZ = 0;
  float sumGyX = 0, sumGyY = 0, sumGyZ = 0;
  
  //가속도 자이로 센서를 읽어드림
  readAccelGyro();
 
  //읽어드렸으면 이제 읽어드린 값을 토대로 평균값을 구하면 됨
  for(int i=0; i<10; i++)
  {
    readAccelGyro();
    sumAcX += AcX; sumAcY += AcY; sumAcZ += AcZ;
    sumGyX += GyX; sumGyY += GyY; sumGyZ += GyZ;
    delay(100);//0.1초
  }
  //맨 처음 기본 센서 값들을 보여지고 그다음에 평균값을 구하는 함수
  baseAcX = sumAcX / 10;
  baseAcY = sumAcY / 10;
  baseAcZ = sumAcZ / 10;
  
  baseGyX = sumGyX / 10;
  baseGyY = sumGyY / 10;
  baseGyZ = sumGyZ / 10;
}
  
 //프로세싱 스케치로 각도 정보를 보내는 루틴
void SendDataToProcessing()
{
  Serial.write('a');
  Serial.write((int)roll_output));  
//processing에서 A문자를 받으면 roll 출력값을 처리(약속!) 
//즉, a, b, c 는 Roll, Pitch, Yaw 에 대한 헤더문자임
  Serial.write('b');
  Serial.write((int)pitch_output));
  Serial.write('c');
  Serial.write((int)yaw_output));
}
 
unsigned long t_now;
unsigned long t_prev;
 
void initDT(){
  t_prev = millis();
}
 
void calcDT(){
  t_now = millis();
  dt = (t_now - t_prev) / 1000.0;
  t_prev = t_now;
}
 
void calcAccelYPR() //가속도 센서 처리 루틴
{
  float accel_x, accel_y, accel_z; //x, y, z 축에 대한 각도 저장 변수
  float accel_xz, accel_yz;
  const float RADIANS_TO_DEGREES = 180 / 3.14159;
 
  accel_x = AcX - baseAcX;
  accel_y = AcY - baseAcY;
  accel_z = AcZ + (16384 - baseAcZ);
 
  accel_yz = sqrt(pow(accel_y, 2) + pow(accel_z, 2));
  accel_angle_y = atan(-accel_x / accel_yz)*RADIANS_TO_DEGREES;
 
  accel_xz = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
  accel_angle_y = atan(accel_y / accel_xz)*RADIANS_TO_DEGREES;
 
  accel_angle_z = 0;
}

float gyro_x, gyro_y, gyro_z;

void calcGyroYPR()
{
  const float GYROXYZ_TO_DEGREES_PER_SEC = 131; // 각속도를 저장하는 변수

  gyro_x = (GyX - baseGyX) / GYROXYZ_TO_DEGREES_PER_SEC;
  gyro_y = (GyY - baseGyY) / GYROXYZ_TO_DEGREES_PER_SEC;
  gyro_z = (GyZ - baseGyZ) / GYROXYZ_TO_DEGREES_PER_SEC;

 // gyro_angle_x += gyro_x * dt;
 // gyro_angle_y += gyro_y * dt;
 // gyro_angle_z += gyro_z * dt;
 /*주석을 하는 이유
  * 이 부분은 자이로 센서 자체적으로 회전각을 구하는 부분,
  * 이제는 PID 제어를 통해 회전각을 구할것이다.
  */
}

void calcFilteredYPR() //상보필터
{
   const float ALPHA = 0.96;
   float tmp_angle_x, tmp_angle_y, tmp_angle_z; //임시 각도

   tmp_angle_x = filtered_angle_x + gyro_x * dt; // 이전 보정 각도 + 현재 자이로 센서를 이용해 얻은 각도(gyro_x * dt)
   tmp_angle_y = filtered_angle_y + gyro_y * dt;
   tmp_angle_z = filtered_angle_z+ gyro_z * dt;

   filtered_angle_x = ALPHA * tmp_angle_x + (1.0 - ALPHA) * accel_angle_x;
   filtered_angle_y = ALPHA * tmp_angle_y + (1.0 - ALPHA) * accel_angle_y;
   filtered_angle_z = tmp_angle_z;
}

//표준 PID 제어기
void stdPID(float& setpoint, float& input, float& prev_input, float& kp, float& ki, float& kd, float& iterm, float& output)
{
  float error;
  float dInput;
  float pterm, dterm;

  error = setpoint - input; 
  dInput = input - prev_input;
  prev_input = input;

  pterm = kp * error;
  iterm += ki * error * dt;
  dterm = -kd * (dInput / dt);

  output = pterm + iterm + dterm;
}

float roll_target_angle = 0.0;
float roll_prev_angle = 0.0;
float roll_kp = 1;
float roll_ki = 0;
float roll_kd = 0;
float roll_iterm;
float roll_output;

float pitch_target_angle = 0.0;
float pitch_prev_angle = 0.0;
float pitch_kp = 1;
float pitch_ki = 0;
float pitch_kd = 0;
float pitch_iterm;
float pitch_output;

float yaw_target_angle = 0.0;
float yaw_prev_angle = 0.0;
float yaw_kp = 1;
float yaw_ki = 0;
float yaw_kd = 0;
float yaw_iterm;
float yaw_output;

void calcYPRtoStdPID()
{
  stdPID(roll_target_angle, filtered_angle_y, roll_prev_angle, roll_kp, roll_ki, roll_kd, roll_iterm, roll_output);
  stdPID(pitch_target_angle, filtered_angle_x, pitch_target_angle, pitch_kp, pitch_ki, pitch_kd, pitch_iterm, pitch_output);
  stdPID(yaw_target_angle, filtered_angle_z, yaw_target_angle, yaw_kp, yaw_ki, yaw_kd, yaw_iterm, yaw_output);

  //PID 출력값(Roll, Pitch, Yaw)을 Processing IDE로 보내주는 부분
  //실제로 드론을 제어할때는 이 코드를 안씀 이 코드는 그냥 프로세싱에서 시각적으로 확인하기 위해 쓰는 코드임!!
 #define PIDOUTPUT_MAX 127
 #define PIDOUTPUT_MIN -128
  if(roll_output > PIDOUTPUT_MAX) roll_output = PIDOUTPUT_MAX;
  if(roll_output < PIDOUTPUT_MIN) roll_output = PIDOUTPUT_MIN;
  
  if(pitch_output > PIDOUTPUT_MAX) pitch_output = PIDOUTPUT_MAX;
  if(pitch_output < PIDOUTPUT_MIN) pitch_output = PIDOUTPUT_MIN;

  if(yaw_output > PIDOUTPUT_MAX) yaw_output = PIDOUTPUT_MAX;
  if(yaw_output < PIDOUTPUT_MIN) yaw_output = PIDOUTPUT_MIN;

  roll_output += 128;
  pitch_output += 128;
  yaw_output += 128;
}

다운로드 파일

_15ypr2stdpid02.ino

---

설명#1 calcYPRtoStdPID() 함수 수정하기

void calcYPRtoStdPID()
{
  stdPID(roll_target_angle, filtered_angle_y, roll_prev_angle, roll_kp, roll_ki, roll_kd, roll_iterm, roll_output);
  stdPID(pitch_target_angle, filtered_angle_x, pitch_target_angle, pitch_kp, pitch_ki, pitch_kd, pitch_iterm, pitch_output);
  stdPID(yaw_target_angle, filtered_angle_z, yaw_target_angle, yaw_kp, yaw_ki, yaw_kd, yaw_iterm, yaw_output);

  //PID 출력값(Roll, Pitch, Yaw)을 Processing IDE로 보내주는 부분
  //실제로 드론을 제어할때는 이 코드를 안씀 이 코드는 그냥 프로세싱에서 시각적으로 확인하기 위해 쓰는 코드임!!
 #define PIDOUTPUT_MAX 127
 #define PIDOUTPUT_MIN -128
  if(roll_output > PIDOUTPUT_MAX) roll_output = PIDOUTPUT_MAX;
  if(roll_output < PIDOUTPUT_MIN) roll_output = PIDOUTPUT_MIN;
  
  if(pitch_output > PIDOUTPUT_MAX) pitch_output = PIDOUTPUT_MAX;
  if(pitch_output < PIDOUTPUT_MIN) pitch_output = PIDOUTPUT_MIN;

  if(yaw_output > PIDOUTPUT_MAX) yaw_output = PIDOUTPUT_MAX;
  if(yaw_output < PIDOUTPUT_MIN) yaw_output = PIDOUTPUT_MIN;

  roll_output += 128;
  pitch_output += 128;
  yaw_output += 128;
}

추가해준 부분은 우리가 실제 PID 제어로 드론 모터를 구동하기 전에 시각적으로 PID제어를 확인하기 위해서 프로세싱으로 Arduino IDE에서 제공받은 PID 출력 값을 Processing으로 보내주는 그러한 코드입니다.

※ 실제 드론을 제어할때는 이번에 추가한 코드는 사용하지 않습니다.

Q. 왜 상한값(PIDOUTPUT_MAX)과 하한값(PIDOUTPUT_MIN)을 설정해 주어서 출력 범위를 정해주었는가?

→ 드론에서 사용되는 모터는 analogWrite 함수를 이용하는데 analogWrite를 이용해 제어가 가능한 PWM 값의 범위가 0 ~ 255입니다. 따라서 이렇게 상한값과 하한값을 지정을 해주어서 값의 범위를 0 ~ 255로 해준 것 입니다.

참고 : [드론] 드론용 모터 소개 및 테스트

---

설명#2  프로세싱으로 보내는 함수(SendDataToProcessing) 수정하기

void SendDataToProcessing()
{
  Serial.write('a');
  Serial.write((int)roll_output)); 
 //processing에서 A문자를 받으면 roll 출력값을 처리(약속!)
 //즉, a, b, c 는 Roll, Pitch, Yaw 에 대한 헤더문자임
  Serial.write('b');
  Serial.write((int)pitch_output));
  Serial.write('c');
  Serial.write((int)yaw_output));
}

Serial.Write 함수를 이용해서 데이터들을 보내주고 있습니다 위 내용은 a, b, c,라는 문자를 프로세싱에서 받게 되면 그 다음 내용인 roll_output, pitch_output, yaw_output을 주는 내용입니다.

즉, a, b, c는 Roll, Pitch, Yaw에 대한 매크로 문자(?) 라고 생각하시면 될거 같습니다.

다음 포스팅에서는 아두이노에서 보내준 PID 출력값을 바탕으로 프로세싱에서 받아서 시각적을 보여지는 포스팅에 대해 설명을 하도록 하겠습니다.