[드론] PID 출력값에 따라 드론 모터속도 계산하기

PID 출력값에 따라 모터속도 계산하기

목적

드론이 어느 방향으로 기우느냐에 따라서 모터 속도가 컨트롤 되면서 드론이 수평을 유지하도록 하는 것 입니다.

해햐할 부분

기존 PID 출력값을 프로세싱을 보냈는데 이제는 프로세싱으로 보내는 대신 PID출력값을 계산해서 모터로 보내어 속도를 제어하는 부분을 추가해야 한다.

---

소스코드

#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 되면 그떄부터 측정 시작!
}

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

  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); //Roll 출력값
  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',
};

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;
    }
  }
}
float throttle = 100.0; //모터 속도에 대한 기준 속도값을 임시로 100 설정
float motorA_speed, motorB_speed, motorC_speed, motorD_speed;

void calcMotorSpeed() {
  motorA_speed = 
    throttle + yaw_output + roll_output + pitch_output;
  motorB_speed = 
    throttle - yaw_output - roll_output + pitch_output;
  motorC_speed = 
    throttle + yaw_output - roll_output - pitch_output;
  motorD_speed = 
    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;
}

다운로드 파일

sketch_sep10c.ino

---

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

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

여기서 PID 출력값을 프로세싱으로 보내주는 부분을 삭제하면 됩니다.

---

설명#2 calcMotorSpeed() 함수 선언

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

loop 구문에 calcMotorSpeed 함수 선언

float throttle = 100.0; //모터 속도에 대한 기준 속도값을 임시로 100 설정
float motorA_speed, motorB_speed, motorC_speed, motorD_speed;

void calcMotorSpeed() {
  motorA_speed = 
    throttle + yaw_output + roll_output + pitch_output;
  motorB_speed = 
    throttle - yaw_output - roll_output + pitch_output;
  motorC_speed = 
    throttle + yaw_output - roll_output - pitch_output;
  motorD_speed = 
    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;
}

calcMotorSpeed 함수 정의

yaw_output, roll_output, pitch_output은 calcYPRtoStdPID() 함수를 통해 얻은 Roll, Pitch, Yaw의 출력값을 이용해서 드론이 기울어진 방향에 따라서 모터 속도의 값을 계산하는 부분입니다.

해당 드론의 모터 속도에 대한 알고리즘

motorA_speed = throttle + yaw_output + roll_output + pitch_output  motorB_speed = throttle - yaw_output - roll_output + pitch_output  motorC_speed = throttle + yaw_output - roll_output - pitch_output  motorD_speed = throttle - yaw_output + roll_output - pitch_output

---

설명#3 extern 타입 변수 선언

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); //Roll 출력값
  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);
}

extern 타입으로 선언해서 moroaA_speed와 같은 변수를 선언을 해주었습니다.

참고로 아래쪽에 보시게 되면

이렇게 float형으로 motorA_speed 변수를 선언을 해주었습니다. 근데 위에서 또 extern으로 선언을 해준 이유는 

컴파일러에게 이렇게 생긴 변수(morotA_speed)가 어딘가에 존재를 한다는것을 알릴때 사용됩니다.

드론을 어떻게 기우느냐에 따라서 모터 속도가 달라지는것을 확인할 수 있습니다.