Revision 0:eeb41e25a490, committed 2016-02-01

Comitter:

porizou

Date:

Mon Feb 01 06:14:20 2016 +0000

Commit message:

add incompletedifferential

Changed in this revision

diff -r 000000000000 -r eeb41e25a490 PIDcontroller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDcontroller.cpp	Mon Feb 01 06:14:20 2016 +0000
@@ -0,0 +1,135 @@
+#include "PIDcontroller.h"
+
+ PID::PID(float Kp, float Ki, float Kd, float tSample){
+
+    tSample_ = tSample;
+
+    Kp_ = Kp; Ki_ = Ki; Kd_ = Kd;
+    
+    setPoint_             = 0.0;
+    processVariable_      = 0.0;
+    controllerOutput_     = 0.0;
+    prevControllerOutput_ = 0.0;
+
+    accError_ = 0.0;
+    Error_    = 0.0;
+    prevError_= 0.0;
+    Bias_     = 0.0;
+
+    usingFeedForward = 0;
+    usingIncompleteDifferential = 0;
+
+    prevDiffOut = 0.0;
+    u_ = 0.0;
+ }
+
+ void PID::setInputLimits(float inMin, float inMax){
+
+    if(inMin >= inMax) return;
+     
+    inMin_  = inMin;
+    inMax_  = inMax;
+    inSpan_ = inMax - inMin;
+ }
+
+ void PID::setOutputLimits(float outMin, float outMax){
+
+    if(outMin >= outMax) return;
+
+    outMin_  = outMin;
+    outMax_  = outMax;
+    outSpan_ = outMax - outMin;
+ }
+
+ void PID::setBias(float Bias) {
+    Bias_ = Bias;
+    usingFeedForward = 1;
+ }
+
+ void PID::setIncompleteDifferential(float u) {
+    u_ = u;
+    usingIncompleteDifferential = 1;
+ }
+
+ void PID::setSetPoint(float sp){
+    setPoint_ = sp;
+ }
+
+ void PID::setProcessValue(float pv){
+    processVariable_ = pv;
+ }
+
+void PID::setGain(float Kp, float Ki, float Kd) {
+    Kp_ = Kp; Ki_ = Ki; Kd_ = Kd;
+}
+
+ float PID::compute(){
+
+    //現在値と目標値の値を0~100%の範囲に置き換える
+    float scaledPV = scaledParcent((processVariable_ - inMin_) / inSpan_);
+
+    float scaledSP = scaledParcent((setPoint_ - inMin_) / inSpan_);
+
+    //偏差の計算
+    Error_= scaledSP - scaledPV;
+
+    //アンチワインドアップ
+    if (!(prevControllerOutput_ >= 1 && Error_ > 0) && !(prevControllerOutput_ <= 0 && Error_ < 0)) {
+        accError_ += (Error_ + prevError_) / 2.0 * tSample_; //偏差の積分値の計算
+    }
+
+    //偏差の微分値の計算(不完全微分が有効な場合,偏差の不完全微分値を計算)
+    float diffError = usingIncompleteDifferential ? calcIncompleteDifferential() : (Error_ - prevError_) / tSample_;
+    
+    //フィードフォワード制御が有効な場合,バイアス値の計算
+    float scaledBias = usingFeedForward ? (Bias_ - outMin_) / outSpan_ : 0.0;
+
+    //PIDの計算
+    controllerOutput_ = scaledParcent(scaledBias + Kp_ * Error_ + Ki_ * accError_ + Kd_ * diffError); 
+
+    //出力の値,偏差の値を更新
+    prevControllerOutput_ = controllerOutput_;
+    prevError_ = Error_;
+    //微分項の値を更新
+    prevDiffOut = Kd_ * diffError;
+    //PIDの出力を実際の値に変換して返す
+    return ((controllerOutput_ * outSpan_) + outMin_);
+
+ }
+
+ float PID::scaledParcent(float value) {
+
+    if(value > 1.0) {
+        return 1.0;
+    } else if(value < 0.0) {
+        return 0.0;
+    }
+    else return value;
+ }
+
+ /**
+ * 不完全微分の式
+ *
+ * 伝達関数
+ * Yn = (Td*s / (1 + k*Td*s)) * E
+ *
+ * 差分の式
+ * yn = (k*Td / (Δt + k*Td)) * yn-1 + (Td / (Δt + k*Td))*(en - en-1)
+ *
+ * Δt   制御周期
+ * k    定数
+ * Td   微分ゲイン
+ * yn   現在の出力
+ * yn-1 前回の出力
+ * en   現在の偏差
+ * en-1 前回の偏差
+ * 
+ * 参考文献 http://www.nikko-pb.co.jp/products/k_data/P12_13.pdf
+ */
+
+ float PID::calcIncompleteDifferential(void) {
+
+    float k = 1 / (tSample_ + u_ * Kd_);
+
+    return (k * u_ * prevDiffOut) + (k * (Error_ - prevError_));
+ }

diff -r 000000000000 -r eeb41e25a490 PIDcontroller.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDcontroller.h	Mon Feb 01 06:14:20 2016 +0000
@@ -0,0 +1,81 @@
+#ifndef PIDcontroller_H
+#define PIDcontroller_H
+
+ #include "mbed.h"
+ 
+ class PID {
+
+ public:
+
+    /**
+    * Kp 比例ゲイン
+    * Ki 積分ゲイン
+    * Kd 微分ゲイン
+    * tSample 制御周期
+    */
+    PID(float Kp, float Ki, float Kd, float tSample);
+
+    /**
+    * 入力値の範囲を設定
+    *
+    * InMin 入力の最小値 --> 0% 
+    * InMax 入力の最大値 --> 100%
+    */
+    void setInputLimits(float inMin, float inMax);
+
+    /**
+    * 出力値の範囲を設定
+    *
+    * InMin 出力の最小値 --> 0% 
+    * InMax 出力の最大値 --> 100%
+    */
+    void setOutputLimits(float outMin, float outMax);
+
+    void setSetPoint(float sp); //目標の設定
+ 
+    void setProcessValue(float pv); //現在値の設定
+    
+    void setGain(float Kp, float Ki, float Kd); //PIDゲインの設定
+
+    void setBias(float Bias); //フィードフォワード制御有効　バイアス値の設定
+
+    void setIncompleteDifferential(float u); //不完全微分(実用微分)有効,不完全微分係数の設定
+
+    float compute(void); //PIDの計算　戻り値はoutMinからoutMaxの範囲
+    float scaledParcent(float value);
+
+    float calcIncompleteDifferential(void);
+
+ private:
+    bool usingFeedForward; //フィードフォワード制御有効フラグ
+    bool usingIncompleteDifferential; //不完全微分有効フラグ
+
+    //PIDゲイン
+    float Kp_, Ki_, Kd_;
+ 
+    float setPoint_; //目標値
+
+    float     controllerOutput_; //出力値(0.0~1.0)
+    float prevControllerOutput_; //前回の出力値(0.0~1.0)
+
+    float  inMin_,  inMax_,  inSpan_; //入力の最小値,最大値,範囲
+    float outMin_, outMax_, outSpan_; //出力の最小値,最大値,範囲
+
+    float  accError_; //偏差の積分値
+
+    float     Error_; //現在の偏差
+    float prevError_; //前回の偏差
+    
+    float tSample_; //制御周期
+
+    float Bias_; //フィードフォワード制御バイアス値
+
+    float processVariable_; //入力値
+
+    float prevDiffOut; //前回の微分項の出力
+
+    float u_; //不完全微分係数
+
+ };
+
+ #endif /* PIDcontroller_H */