PIDcontroller.cpp

00001 #include "PIDcontroller.h"
00002 
00003  PID::PID(float Kp, float Ki, float Kd, float tSample){
00004 
00005     tSample_ = tSample;
00006 
00007     Kp_ = Kp; Ki_ = Ki; Kd_ = Kd;
00008     
00009     setPoint_             = 0.0;
00010     processVariable_      = 0.0;
00011     controllerOutput_     = 0.0;
00012     prevControllerOutput_ = 0.0;
00013 
00014     accError_ = 0.0;
00015     Error_    = 0.0;
00016     prevError_= 0.0;
00017     Bias_     = 0.0;
00018 
00019     usingFeedForward = 0;
00020     usingIncompleteDifferential = 0;
00021 
00022     prevDiffOut = 0.0;
00023     u_ = 0.0;
00024  }
00025 
00026  void PID::setInputLimits(float inMin, float inMax){
00027 
00028     if(inMin >= inMax) return;
00029      
00030     inMin_  = inMin;
00031     inMax_  = inMax;
00032     inSpan_ = inMax - inMin;
00033  }
00034 
00035  void PID::setOutputLimits(float outMin, float outMax){
00036 
00037     if(outMin >= outMax) return;
00038 
00039     outMin_  = outMin;
00040     outMax_  = outMax;
00041     outSpan_ = outMax - outMin;
00042  }
00043 
00044  void PID::setBias(float Bias) {
00045     Bias_ = Bias;
00046     usingFeedForward = 1;
00047  }
00048 
00049  void PID::setIncompleteDifferential(float u) {
00050     u_ = u;
00051     usingIncompleteDifferential = 1;
00052  }
00053 
00054  void PID::setSetPoint(float sp){
00055     setPoint_ = sp;
00056  }
00057 
00058  void PID::setProcessValue(float pv){
00059     processVariable_ = pv;
00060  }
00061 
00062 void PID::setGain(float Kp, float Ki, float Kd) {
00063     Kp_ = Kp; Ki_ = Ki; Kd_ = Kd;
00064 }
00065 
00066  float PID::compute(){
00067 
00068     //現在値と目標値の値を0~100%の範囲に置き換える
00069     float scaledPV = scaledParcent((processVariable_ - inMin_) / inSpan_);
00070 
00071     float scaledSP = scaledParcent((setPoint_ - inMin_) / inSpan_);
00072 
00073     //偏差の計算
00074     Error_= scaledSP - scaledPV;
00075 
00076     //アンチワインドアップ
00077     if (!(prevControllerOutput_ >= 1 && Error_ > 0) && !(prevControllerOutput_ <= 0 && Error_ < 0)) {
00078         accError_ += (Error_ + prevError_) / 2.0 * tSample_; //偏差の積分値の計算
00079     }
00080 
00081     //偏差の微分値の計算(不完全微分が有効な場合,偏差の不完全微分値を計算)
00082     float diffError = usingIncompleteDifferential ? calcIncompleteDifferential() : (Error_ - prevError_) / tSample_;
00083     
00084     //フィードフォワード制御が有効な場合,バイアス値の計算
00085     float scaledBias = usingFeedForward ? (Bias_ - outMin_) / outSpan_ : 0.0;
00086 
00087     //PIDの計算
00088     controllerOutput_ = scaledParcent(scaledBias + Kp_ * Error_ + Ki_ * accError_ + Kd_ * diffError); 
00089 
00090     //出力の値,偏差の値を更新
00091     prevControllerOutput_ = controllerOutput_;
00092     prevError_ = Error_;
00093     //微分項の値を更新
00094     prevDiffOut = Kd_ * diffError;
00095     //PIDの出力を実際の値に変換して返す
00096     return ((controllerOutput_ * outSpan_) + outMin_);
00097 
00098  }
00099 
00100  float PID::scaledParcent(float value) {
00101 
00102     if(value > 1.0) {
00103         return 1.0;
00104     } else if(value < 0.0) {
00105         return 0.0;
00106     }
00107     else return value;
00108  }
00109  
00110  void PID::resetIntError(){
00111     accError_ = 0.0;
00112  }
00113 
00114  /**
00115  * 不完全微分の式
00116  *
00117  * 伝達関数
00118  * Yn = (Td*s / (1 + k*Td*s)) * E
00119  *
00120  * 差分の式
00121  * yn = (k*Td / (Δt + k*Td)) * yn-1 + (Td / (Δt + k*Td))*(en - en-1)
00122  *
00123  * Δt   制御周期
00124  * k    定数
00125  * Td   微分ゲイン
00126  * yn   現在の出力
00127  * yn-1 前回の出力
00128  * en   現在の偏差
00129  * en-1 前回の偏差
00130  * 
00131  * 参考文献 http://www.nikko-pb.co.jp/products/k_data/P12_13.pdf
00132  */
00133 
00134  float PID::calcIncompleteDifferential(void) {
00135 
00136     float k = 1 / (tSample_ + u_ * Kd_);
00137 
00138     return (k * u_ * prevDiffOut) + (k * (Error_ - prevError_));
00139  }