PID Controller

　　去年暑假無意間看到有paper用基因演算法去自動調整PID控制參數，感覺上非常實用也很有趣不過工作很忙一直沒時間研究，這幾天又想趁有空閒來實現那個paper，又突然看到Arduino PID Library的作者的Blog解說，發現跟一般普通理想化的PID公式不同，因此讀完了以下參考前八篇（Arduino PID Library）以及第九篇（補充說明），跟著每一篇實作code與推導公式收益良多！

PID.h


#ifndef __PID__
#define __PID__

const bool DIRECT = true;
const bool REVERSE = false;

const bool ON = true;
const bool OFF = false;

class PID
{
   public:
      PID(double*,double*,double*,double,double,double,unsigned long,bool,double,double,double);
      void Computing();
      void SetTunings(double,double,double);
      void SetSampleTime(unsigned long);
      void SetOutputLimits(double,double);
      void SetOnOff(bool);
      void SetDirection(bool);

   private:
      double Kp,Ki,Kd;
      double *input,*output;
      double *setpoint;

      double integral;
      double last_input;

      double min,max;

      unsigned long last_time;
      unsigned long sample_time;

      bool on_off;
      bool direction;
};

#endif


PID.cpp


#if ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#endif

#include "PID.h"

PID::PID(double *_input,double *_output,double *_setpoint,
      double _Kp,double _Ki,double _Kd,
      unsigned long _sample_time = 100000,
      bool _on_off = OFF,double _direction = DIRECT,
      double _min = 0,double _max = 255)
{
   input = _input;
   output = _output;
   setpoint = _setpoint;

   sample_time = _sample_time;

   on_off = _on_off;

   SetOutputLimits(_min,_max);

   SetDirection(_direction);

   SetTunings(_Kp,_Ki,_Kd);

   last_time = (micros() - sample_time);
}

void PID::Computing()
{
   if(on_off == OFF){
      return ;
   }

   unsigned long now = micros();

   if((now - last_time) >= sample_time){
      double error = (*setpoint - *input);

      integral += (Ki * error);
      if(integral > max){
integral = max;
      }
      else if(integral < min){
integral = min;
      }

      double diff_input = (*input - last_input);

      *output = (Kp * error) + integral - (Kd * diff_input);

      if(*output > max){
*output = max;
      }
      else if(*output < min){
*output = min;
      }

      last_input = *input;
      last_time = now;
   }
}

void PID::SetTunings(double _Kp,double _Ki,double _Kd)
{
   if((_Kp < 0) || (_Ki < 0) || (_Kd < 0)){
      return ;
   }

   double SampleTimeInSec = ((double)sample_time) / 1000000.0f;

   Kp = _Kp;
   Ki = _Ki * SampleTimeInSec;
   Kd = _Kd / SampleTimeInSec;

   if(direction != REVERSE){
      Kp = (0 - Kp);
      Ki = (0 - Ki);
      Kd = (0 - Kd);
   }
}

void PID::SetSampleTime(unsigned long _sample_time)
{
   if(_sample_time > 0){
      double ratio = ((double)_sample_time / (double)sample_time);

      Ki *= ratio;
      Kd /= ratio;

      sample_time = _sample_time;
   }
}

void PID::SetOutputLimits(double _min,double _max)
{
   if(_min > _max){
      return ;
   }

   min = _min;
   max = _max;

   if(on_off == ON){
      if(*output > max){
*output = max;
      }
      else if(*output < min){
*output = min;
      }

      if(integral > max){
integral = max;
      }
      else if(integral < min){
integral = min;
      }
   }
}

void PID::SetOnOff(bool _on_off)
{
   bool new_state = (_on_off == ON);

   if(new_state == (!on_off)){
      integral = *output;
      last_input = *input;

      if(integral > max){
integral = max;
      }
      else if(integral < min){
integral = min;
      }
   }

   on_off = new_state;
}

void PID::SetDirection(bool _direction)
{
  if((on_off == ON) && (_direction != direction)){
      Kp = (0 - Kp);
      Ki = (0 - Ki);
      Kd = (0 - Kd);
   }

   direction = _direction;
}



Download

參考：

　　第一篇是基本的PID公式，第二篇將採樣時間固定而消去幾次除法運算（Kalman Filter等等的微積分時間也可以這樣簡化計算），第三篇說明如何做Anti-Derivative Kick，第四篇說明如何消去中途修改PID參數的落差，第五篇說明如何做Anti-Reset Windup，第六篇其實就是說中途中止計算要記得直接跳出不要更改狀態，第七篇接續前一篇如果要恢復記得要回到上次計算的狀態，第八篇說明計算結果方向（正負號），例如Input大於Setpoint就增加Output（正向），或者Input小於Setpoint就增加Output（反向），第九篇算是補充說明積分項計算時間點的差異。

1. Improving the Beginner’s PID – Introduction
2. Improving the Beginner’s PID – Sample Time
3. Improving the Beginner’s PID – Derivative Kick
4. Improving the Beginner’s PID: Tuning Changes
5. Improving the Beginner’s PID: Reset Windup
6. Improving the Beginner’s PID: On/Off
7. Improving the Beginner’s PID: Initialization
8. Improving the Beginner’s PID: Direction
9. PID: When Should I Compute the Integral Term?