[Arduino] SBOT1 아두이노 세그웨이 밸런싱로봇 제작(Feat. MPU6050, PID제어) - (2)

---

http://scipia.co.kr/cms/blog/227
이 키트를 조립하기 전, 공부를 하려 합니다.
(해당 포스팅의 모든 출처는 싸이피아 임을 명시합니다.)

---

이번에 공부한 내용

PID 제어와 관련된 알고리즘

---

PID 제어란?

PID는 Proportional(비례), Integral(적분), Derivative(미분)의 약자로서 산업 제어시스템에서 널리 사용되는 제어 루프 피드백 메카니즘

원하는 설정값과 실제 측정값의 차이를 오차를 지속적으로 계산하고 P,I,D 알고리즘으로 제어하여 원하는 설정값을 유지하게 합니다.

비례(P) 제어를 기본으로 하고 여기에 적분(I)과 미분(D) 제어를 통하여 누적오차를 줄이고 응답속도를 높이며 최대한 안정된 상태로 만들어 줍니다.

Set point : 로봇이 지면에서 평형을 유지하는 상태의 값
e : 설정값과 실제 측정값과의 오차
Kp, Ki, Kd : 제어 파라미터
-> 이 값을 수정해 제어기가 최적으로 동작할 수 있도록 하는 제어기 튜닝작업이 필요
-> 제어기 튜닝을 통해 최적의 Kp, Ki, Kp 값을 찾아내야 한다.

즉, 오차 값을 줄이기 위해 조절 가능한 최적의 값을 찾아야 한다는 뜻.

---

공개된 아두이노 코드로 보는 PID 제어 예시

튜닝값 수정하는 방법

1. Kp 조정 :
   Kp값이 너무 작으면 -> 로봇이 쉽게 넘어짐
   Kp값이 너무 크면 -> 로봇이 앞뒤로 심하게 흔들림
   => 로봇이 앞뒤로 조금씩 흔들리는 상태가 최적의 상태

2. Kd 조정 :
   => 로봇이 안정을 유지하는 동안 진동을 감소, 손으로 밀어도 로봇이 바로 복귀되게 함

3. Ki 조정 :
   Kp와 Kd가 설정되더라도 안정된 상태로 도달하는 동안 진동함.
   => 최적의 Ki 값 : 로봇이 안정되는 데 걸리는 시간을 단축시킬 수 있음

---

라이브러리로 이해하는 알고리즘

1. PID 제어 라이브러리

• For an ultra-detailed explanation of why the code is the way it is, please visit:
  http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

• For function documentation see: http://playground.arduino.cc/Code/PIDLibrary

  /*working variables*/
  unsigned long lastTime;
  double Input, Output, Setpoint;
  double errSum, lastErr;
  double kp, ki, kd;
  void Compute()
  {
    /*How long since we last calculated*/
    unsigned long now = millis();
    double timeChange = (double)(now - lastTime);
   
    /*Compute all the working error variables*/
    double error = Setpoint - Input;
    errSum += (error * timeChange);
    double dErr = (error - lastErr) / timeChange;
   
    /*Compute PID Output*/
    Output = kp * error + ki * errSum + kd * dErr;
   
    /*Remember some variables for next time*/
    lastErr = error;
    lastTime = now;
  }
  void SetTunings(double Kp, double Ki, double Kd)
  {
    kp = Kp;
    ki = Ki;
    kd = Kd;
  }

// 아두이노 밸런싱로봇 프로그램 (참조 및 출처 : circuitdigest.com)

#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

 
// 아래의 4개의 값을 본인의 로봇에 맞게 튜닝합니다.
/*********파라메터 튜닝 시작*********/
double setpoint= 180; //로봇이 지면에서 평형을 유지하는 상태의 값입니다.
//다음은 PID 제어기의 Kp, Ki, Kd 파라메타를 설정합니다. 아래의 순서대로 설정합니다.
double Kp = 21; 
double Kd = 0.8; 
double Ki = 140; 
/******파라메터 튜닝 끝*********/

double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

 

volatile bool mpuInterrupt = false;     // MPU6050의 인터럽트 발생유무 확인
void dmpDataReady()
{
    mpuInterrupt = true;
}

void setup() {
  Serial.begin(115200);

    // MPU6050 초기화
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // MPU6050 통신확인
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // DMP 초기화
    devStatus = mpu.dmpInitialize();

    
    // 기본 옵셋값 설정
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1688); 

    // 정상동작하는 경우
    if (devStatus == 0)
    {
        // DMP 가동
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // 아두이노 인터럽트 설정
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // DMP 사용가능 상태 설정
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // 패킷사이즈 가져오기
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        //PID 설정
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    else
    {
        // 오류시
        // 1 = 초기 메모리 에러
        // 2 = DMP 설정 오류
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    //모터 출력핀 초기화
    pinMode (6, OUTPUT);
    pinMode (9, OUTPUT);
    pinMode (10, OUTPUT);
    pinMode (11, OUTPUT);

   //모터 동작 OFF
    analogWrite(6,LOW);
    analogWrite(9,LOW);
    analogWrite(10,LOW);
    analogWrite(11,LOW);
}

 

void loop() {
 
    // 오류시 작업중지
    if (!dmpReady) return;

    // MPU 인터럽트나 패킷 대기
    while (!mpuInterrupt && fifoCount < packetSize)
    {
        //MPU6050 데이터가 없는 경우 PID 계산
        pid.Compute();   
        
        //시리얼 모니터로 현재 상태 출력
        Serial.print(input); Serial.print(" =>"); Serial.println(output);
               
        if (input>150 && input<200){//로봇이 기울어지는 경우(각도 범위내에서만)
          
        if (output>0) //앞으로 기울어지는 경우
        Forward(); //전진
        else if (output<0) //뒤로 기울어지는 경우
        Reverse(); //후진
        }
        else //로봇이 기울어지지 않은 경우
        Stop(); //모터 정지
        
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & 0x02)
    {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q
        mpu.dmpGetGravity(&gravity, &q); //get value for gravity
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr

        input = ypr[1] * 180/M_PI + 180;

   }
}

void Forward() //전진
{
    analogWrite(6,output);
    analogWrite(9,0);
    analogWrite(10,output);
    analogWrite(11,0);
    Serial.print("F"); 
}

void Reverse() //후진
{
    analogWrite(6,0);
    analogWrite(9,output*-1);
    analogWrite(10,0);
    analogWrite(11,output*-1); 
    Serial.print("R");
}

void Stop() //정지
{
    analogWrite(6,0);
    analogWrite(9,0);
    analogWrite(10,0);
    analogWrite(11,0); 
    Serial.print("S");
}