[드론] 모터 회전 구현하기(아두이노 IDE편)

모터 회전 구현하기

-Arduino IDE-

목표

프로세싱에서 받은 매개변수(게인) 및 throttle 값에 따라 모터 회전 속도를 구현하는 코딩

순서

1. 프로세싱에서 받은 값에 따라서 모터 속도를 구현

2. 프로세싱에서 매개변수 및 throttle 값을 변경할 수 있는 코드 작성(다음 포스팅에서 함)

---

소스코드

#include 

const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ; 

void setup() {
  initMPU6050(); //MPU-6050 센서에 대한 초기 설정 함수
  Serial.begin(115200); //Serial 통신 시작
  calibAccelGyro(); //센서 보정
  initDT(); //시간 간격에 대한 초기화 -> 현재 시각 저장
            //즉, 드론이 전원이 ON 되면 그떄부터 측정 시작!
  initMotorSpeed(); // 모터 스피드 초기화 -> 초기에는 0으로 설정
}

void loop() {
  readAccelGyro(); //가속도, 자이로 센서 값 읽어드림
  //   SendDataToProcessing(); //프로세싱으로 값 전달
  calcDT(); //측정 주기 시간 계산
  calcAccelYPR(); // 가속도 센서 처리 루틴
  calcGyroYPR(); //자이로 센서 처리 루틴
  calcFilteredYPR(); 
  calcYPRtoStdPID();      
  checkPIDChanged();
  calcMotorSpeed(); //calcYPRtoStdPID 함수를 통해 얻은 Roll, PItch, yaw 출ㄺ값을 이용해서 드론 모터 속도값 계산       
  updateMotorSpeed(); // 모터속도 수정

/*
  static int cnt;
  cnt++;
  if(cnt%2 == 0)
    SendDataToProcessing(); //위에 동일한 함수는 주석처리!
  //측정 주기 시간이 짝수(2ms 단위로 하기 위해서)이면 프로세싱으로 보낸다.
*/
}

void initMPU6050(){
  Wire.begin(); //I2C 통신 시작 아림
  Wire.beginTransmission(MPU_addr); //0x68번지 값을 가지는 MPU-6050과 I2C 통신
  Wire.write(0x6B);
  Wire.write(0); //잠자는 MPU-6050을 깨우고 있다.
  Wire.endTransmission(true); //I2C 버스 제어권에서 손 놓음
}

void readAccelGyro(){
  Wire.beginTransmission(MPU_addr); //0x68번지 값을 가지는 MPU-6050과 I2C 통신 시작
  Wire.write(0x3B); //0x3B번지에 저장
  Wire.endTransmission(false); //데이터 전송 후 재시작 메새지 전송(연결은 계속 지속)
  Wire.requestFrom(MPU_addr, 14, true); //0x68 번지에 0x3B 부터 48까지 총 14바이트 저장
  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 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;
extern float motorA_speed, motorB_speed, motorC_speed, motorD_speed;
/*
void SendDataToProcessing(){
  Serial.print(F("DEL : "));
  Serial.print(dt, DEC);
  Serial.print("/////////////////////////////");
  Serial.print(F("#RPY"));
  Serial.print(filtered_angle_y, 2);  //상보필터 적용한 Roll
  Serial.print(filtered_angle_x, 2);  //pitch
  Serial.print(filtered_angle_z, 2);  //yaw
  Serial.print("/////////////////////////////");
  Serial.print(F("#PID : "));
  Serial.print(roll_output, 2);
  Serial.print(pitch_output, 2);
  Serial.print(yaw_output, 2);
  Serial.print("/////////////////////////////");
  Serial.print(F("#A"));
  Serial.print(motorA_speed, 2);
  Serial.print(F("#B"));
  Serial.print(motorB_speed, 2);
  Serial.print(F("#C"));
  Serial.print(motorC_speed, 2);
  Serial.print(F("#D"));
  Serial.println(motorD_speed, 2);
}
*/
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);
  }
  baseAcX = sumAcX / 10; baseAcY = sumAcY / 10; baseAcZ = sumAcZ / 10;
  baseGyX = sumGyX / 10; baseGyY = sumGyY / 10; baseGyZ = sumGyZ / 10;
}

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; //millis()로 얻은 값은 밀리초 단위이니까!!!!
  t_prev = t_now;
}

void calcAccelYPR(){
  float accel_x, accel_y, accel_z; //가속도 센서의 최종적인 보정값!!!
  float accel_xz, accel_yz;
  const float RADIANS_TO_DEGREES = 180/3.14159;

  accel_x = AcX - baseAcX; // 가속도(직선) X축에 대한 현재 값 - 가속도 센서의 평균값
  accel_y = AcY - baseAcY;
  accel_z = AcZ + (16384 - baseAcZ);

  //직석 +X축이 기울어진 각도 구함
  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_x = 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_SED = 131;
                                  //131 값은 1초동안 1도 돌때 자이로 값이 131이다.

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

}

void calcFilteredYPR(){
  const float ALPHA = 0.96;
  float tmp_angle_x, tmp_angle_y, tmp_angle_z;  //이전 필터 각도(prev)

  tmp_angle_x = filtered_angle_x + gyro_x * dt; //자이로 각도 = 각속도 x 센서 입력 주기
                                                //각속도 = 단위시간당 회전한 각도 -> 회전한 각도 / 단위시간
  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; //곡선 +Z 축은 자이로 센서만 이용해서 나타내고 있음(가속도 센서는 안함)
  //그래서 위에(calcAccelYPR) 보게 되면 accel_angle_z 는 0이다.
}

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);
}

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',
  THROTTLE = 'T',
};

int inputState = IDLE;
//사용자 입력 상태를 저장하기 위한 변수(초기값은 IDLE : 사용자 입력 값이 없는 상태)
extern float throttle; //throttle 변수를 checkPIDChanged 함수에게 알려주기 위해 사용
 
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;
        case THROTTLE: inputState = THROTTLE; 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;
        case THROTTLE: throttle = inputData;  break;
      }
      inputState = IDLE;
    }
  }
}
float throttle = 0;
float motorA_speed, motorB_speed, motorC_speed, motorD_speed;

void calcMotorSpeed() {
  motorA_speed = (throttle == 0) ? 0:
    throttle + yaw_output + roll_output + pitch_output;
  motorB_speed = (throttle == 0) ? 0:
    throttle - yaw_output - roll_output + pitch_output;
  motorC_speed = (throttle == 0) ? 0:
    throttle + yaw_output - roll_output - pitch_output;
  motorD_speed = (throttle == 0) ? 0:
    throttle - yaw_output + roll_output - pitch_output;
    
  if(motorA_speed < 0) motorA_speed = 0; 
  if(motorA_speed > 255) motorA_speed = 255;
  if(motorB_speed < 0) motorB_speed = 0; 
  if(motorB_speed > 255) motorB_speed = 255;
  if(motorC_speed < 0) motorC_speed = 0; 
  if(motorC_speed > 255) motorC_speed = 255;
  if(motorD_speed < 0) motorD_speed = 0; 
  if(motorD_speed > 255) motorD_speed = 255;
}

#define THROTTLE_MAX 255
#define THROTTLE_MIN 0

int motorA_pin = 6;
int motorB_pin = 10;
int motorC_pin = 9;
int motorD_pin = 5;

void initMotorSpeed(){
  analogWrite(motorA_pin,THROTTLE_MIN);
  analogWrite(motorB_pin,THROTTLE_MIN);
  analogWrite(motorC_pin,THROTTLE_MIN);
  analogWrite(motorD_pin,THROTTLE_MIN);
}

void updateMotorSpeed(){
  analogWrite(motorA_pin,motorA_speed);
  analogWrite(motorB_pin,motorB_speed);
  analogWrite(motorC_pin,motorC_speed);
  analogWrite(motorD_pin,motorD_speed);
}

다운로드 파일

sketch_sep10c.ino

---

#1 sendDatatoProcessing을 주석처리 or 삭제한다.

모터 속도에 대한건 프로세싱에서 보내는 값에 따라서 직접 눈으로 볼거기 때문에 그냥 삭제해줘요

+

이부분도 삭제를 해줍니다.

그 이유는, dt(센서 입력 주기)값에 변동이 생기기 때문입니다. 그렇게 되면 드론이 불안정해 지게 됩니다.

#2 initMotorSpeed 함수 선언

setup() 함수에 선언을 해주게 됩니다.

이 함수수의 역할은 초기 모터와 연결되어 있는 각 핀의 모터의 속도를 0으로 초기화 해줍니다.

#3 updateMotorSpeed 함수 선언

계산된 모터의 속도를 실질적으로 모터에다가 analogWrite 함수를 이용해서 전달해 주게 됩니다. 따라서 모터는 전달 받은 값에 따라서 회전을 하게 됩니다.

#4 enum 정의 부분에 THROTTLE = 'T' 를 추가

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',
  THROTTLE = 'T',
};

이 부분은 THROTTLE에 대한 값을 정의해주는 부분 입니다. 

다음 포스팅에서 보시면 알겠지만 프로세싱에서 

이 부분이 있습니다. 

comPort.write('T') 이부분이 T라는 값을 시리얼 통신을 이용해서 보내겠다는 뜻 인데 현재 comPort는 시리얼 통신의 객체입니다. 따라서 프로세싱에서 T라는 값이 보내지게 되면 아두이노 IDE에서는 T라고 그대로 받는데 이게 무슨놈인지 보니까 enum 정의부분에서 THROTTLE라고 선언을 해준것을 보게 됩니다. 

그리고 checkPIDChanged() 함수에서 프로세싱에서 들어온 값을 처리하게 되고 그 근거로 calcMotorSpeed() 함수에서 모터 속도를 설정하게 되며 updateMotorSpeed() 함수에서 모터 속도를 analogWrite 함수를 이용해서 출력하게 되는 원리 입니다.

#5 checkPIDChanged 함수 수정

int inputState = IDLE;
//사용자 입력 상태를 저장하기 위한 변수(초기값은 IDLE : 사용자 입력 값이 없는 상태)
extern float throttle; //throttle 변수를 checkPIDChanged 함수에게 알려주기 위해 사용
 
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;
        case THROTTLE: inputState = THROTTLE; 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;
        case THROTTLE: throttle = inputData;  break;
      }
      inputState = IDLE;
    }
  }
}

함수 수정이랄거 없이 그냥 THROTTLE에 대한 내용만 추가해주는 것 입니다. 

("[드론] 프로세싱에서 받은 매개변수를 아두이노 드론에 적용하기" 편에서도 간단하게 설명이 되어있습니다)

그래서 이전 포스팅에서는 간단하게 설명이 되어있는데 이번 포스팅에서 어떻게 흘러가는지 흐름과 추가적인 자세한 설명을 해보도록 하겠습니다^^

checkPIDChanged 함수 설명

1. extern으로 변수를 먼저 선언

extern float throttle; 부분을 주석처리 하고 컴파일을 돌려보면 오류가 발생되게 됩니다.

여기서 이 부분의 역할을 checkPIDChanged 함수에다가 "나(throttle) 여기있어~" 라고 알려주기 위해 사용이 되는 것 입니다. 

생각을 더 해보겠습니다. extern은 이러한 변수가 어딘가에 있다고 설명을 해주는 역할이라고 했습니다. 그러면 함수를 선언할때 checkPIDChanged 함수 이후에 선언을 해준다면 무용지물입니다. 똑같이 Error가 날 것입니다. 따라서 checkPIDChanged 함수가 선언되기 이전 어디에선가 extern으로 선언을 해준다면 error가 안나고 정상적으로 동작이 됩니다.

2. checkPIDChanged 흐름도