[드론] 프로세싱에서 받은 매개변수를 아두이노 드론에 적용하기

전달 받은 매개변수를 아두이노에 적용시키기

목표 

프로세싱에서 시리얼 통신을 이용해서 시리얼 포트롤 통해 보낸 매개변수를 아두이노에 적용을 시켜서 GUI 그래프 차트상으로 매개변수 변경함에 따라서 차트 변화 확인하기

소스코드

/*
 *  목표 : 프로세싱에서 보낸 매개변수를 아두이노에서 받아서 드론으로 구동시키기
 *  기술 : 프로세싱에서 시리얼 통신으로 시리얼 포트를 통해 보내준 9개의 매개변수 값들의 설정을 적용시키기
 */
#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(); //표준 PId 제어를 기반으로 한 알고리즘 적용 함수
  checkPIDChanged(); // PID 매개변수 값 변경값 설정 함수
 
  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;
}

enum
{
  IDLE = 'I',
  ROLL_KP = 'a', ROLL_KI = 'b', ROLL_KD = 'c',
  PITCH_KP = 'd', PITCH_KI = 'e', PITCH_KD = 'f',
  YAW_KP = 'g', YAW_KI = 'h', YAW_KD = 'i',
};

int inputState = IDLE;
//사용자 입력 상태를 저장하기 위한 변수(초기값은 IDLE : 사용자 입력 값이 없는 상태)

void checkPIDChanged()
{
  int inputData; //프로세싱에서 입력값이 들오온 데이터 저장

  if(Serial.available() > 0)
  {
    inputData = Serial.read();
    if(inputState == IDLE) //inputState에 현자 사용자 입력 값이 없는 상태라면...
    {
      switch(inputData)
      {
        case ROLL_KP: inputState = ROLL_KP; break;
        case ROLL_KI: inputState = ROLL_KI; break;
        case ROLL_KD: inputState = ROLL_KD; break;
        case PITCH_KP: inputState = PITCH_KP; break;
        case PITCH_KI: inputState = PITCH_KI; break;
        case PITCH_KD: inputState = PITCH_KD; break;
        case YAW_KP: inputState = YAW_KP; break;
        case YAW_KI: inputState = YAW_KI; break;
        case YAW_KD: inputState = YAW_KD; break;
        default: break;
      }
    }
    else //inputState에 현재 사용자 값이 있는 상태라면...
    {
      switch(inputState)
      {
        case ROLL_KP: roll_kp = inputData+0.2; break;
        case ROLL_KI: roll_ki = inputData*0.05; break;
        case ROLL_KD: roll_kd = inputData / 10.0; break;
        case PITCH_KP: pitch_kp = inputData*0.2; break;
        case PITCH_KI: pitch_ki = inputData*0.05; break;
        case PITCH_KD: pitch_kd = inputData / 10.0; break;
        case YAW_KP: yaw_kp = inputData*0.2; break;
        case YAW_KI: yaw_ki = inputData*0.05; break;
        case YAW_KD: yaw_kd = inputData / 10.0; break;
      }
      inputState = IDLE;
    }
  }
}

다운로드 파일

_15ypr2stdpid02.ino

---

설명#1 loop구문에 PID 매개변수 변경 값 설정 함수 추가하기

void loop() {
  readAccelGyro();
  calcDT(); //시간 간격 계산
  calcAccelYPR(); //가속도 센서 처리 루틴
  calcGyroYPR(); 
//자이로 센서 처리 루틴 --> 가속도 센서의 값을 해석하기 위해 Roll, Pitch, Yaw에 대한 각도 구하는 함수
  calcFilteredYPR(); //상보필터 적용
  calcYPRtoStdPID(); //표준 PId 제어를 기반으로 한 알고리즘 적용 함수
  checkPIDChanged(); // PID 매개변수 값 변경값 설정 함수
}

딱 한번만 함수를 호출하는게 아니라 계속 드론이 작동하고 있는 동안에 호출을 해야하니까 LOOP 구문에 선언을 한 것

---

설명#2 열거형 함수(enum) 선언

enum
{
  IDLE = 'I',
  ROLL_KP = 'a', ROLL_KI = 'b', ROLL_KD = 'c',
  PITCH_KP = 'd', PITCH_KI = 'e', PITCH_KD = 'f',
  YAW_KP = 'g', YAW_KI = 'h', YAW_KD = 'i',
};

enum이라는 C언어 내용인 열거형 함수가 나왔습니다. 열거형 함수는 이전 포스팅에서도 설명을 한번 한적이 있습니다.

이전 포스팅 : [드론]프로세싱과 ESP8266을 이용해서 드론 모터속도 조절하기

여기 Microsoft Developer 홈페이제에 아주 잘 나와있네요^^ 

참고 : Enum에 대한 설명

여기는 즉,  프로세싱에서는 Char형태로 우선 전송이 되고 그 다음에 int형으로 변환된 값들이 보내지게 됩니다.

프로세싱 예제

public void roll_kp(float kp)
{
  println("roll kp : " + kp);
  Comport.write('a');
  Comport.write((int)((kp + 0.01) / 0.2));
}

이렇게 write 함수를 사용해서 말이지요

여기서는 'a'라는 문자를 보내주었으니 열거형 함수에서는 a가 들어오면 ROLL_KP라는 변수로 사용을 하겠다 라는 뜻 입니다.

---

설명#3 checkPIDchanged() 함수 설정하기

int inputState = IDLE;
//사용자 입력 상태를 저장하기 위한 변수(초기값은 IDLE : 사용자 입력 값이 없는 상태)

void checkPIDChanged()
{
  int inputData; //프로세싱에서 입력값이 들오온 데이터 저장

  if(Serial.available() > 0)
  {
    inputData = Serial.read();
    if(inputState == IDLE) //inputState에 현자 사용자 입력 값이 없는 상태라면...
    {
      switch(inputData)
      {
        case ROLL_KP: inputState = ROLL_KP; break;
        case ROLL_KI: inputState = ROLL_KI; break;
        case ROLL_KD: inputState = ROLL_KD; break;
        case PITCH_KP: inputState = PITCH_KP; break;
        case PITCH_KI: inputState = PITCH_KI; break;
        case PITCH_KD: inputState = PITCH_KD; break;
        case YAW_KP: inputState = YAW_KP; break;
        case YAW_KI: inputState = YAW_KI; break;
        case YAW_KD: inputState = YAW_KD; break;
        default: break;
      }
    }
    else //inputState에 현재 사용자 값이 있는 상태라면...
    {
      switch(inputState)
      {
        case ROLL_KP: roll_kp = inputData+0.2; break;
        case ROLL_KI: roll_ki = inputData*0.05; break;
        case ROLL_KD: roll_kd = inputData / 10.0; break;
        case PITCH_KP: pitch_kp = inputData*0.2; break;
        case PITCH_KI: pitch_ki = inputData*0.05; break;
        case PITCH_KD: pitch_kd = inputData / 10.0; break;
        case YAW_KP: yaw_kp = inputData*0.2; break;
        case YAW_KI: yaw_ki = inputData*0.05; break;
        case YAW_KD: yaw_kd = inputData / 10.0; break;
      }
      inputState = IDLE;
    }
  }
}

우선적으로 짚고 넘어가야할 부분이 계속 반복해서 IDLE라는 놈이 사용되고 있습니다. 그래서 잠깐 알아보고 넘어갈게요

설명 속 설명#1 IDLE란?

사용자 입력 값이 없는 상태를 의미(약속)

즉, inputState 변수는 사용자 입력 상태를 저장하기 위한 변수입니다. 알고리즘을 그려보면 아시겠지만

Serial 통신으로 어떠한 값이 들어온다면 inputData 변수에 시리얼 통신으로 값을 읽어와 저장하고 그다음에 if문과 else 구문을 실행시킴니다. 만약 inputState가 IDLE라면 현자 사용자 입력 값이 없는 상태이므로 inputData값에 따라 inputState에 매개변수를 저장시켜주어야 합니다. 이런 식으로 else문도 해석을 하시면 됩니다.

그래서 결과를 보게 된다면 프로세싱에서 매개변수를 변경시키면 그에 따라서 GUI상 곡선 그래프가 변경이 되는것을 확인하실 수 있습니다.