Hiroki Mori / PID

mbedのPIDライブラリを改良したものです 主な変更 ・PIDゲインを独立に調整可能 ・実行中に入力値と出力値の範囲を変更する機能の削除 ・微分制御に不完全微分を追加 ・PIDゲインを設定する機能を追加 ・各種ゲッターを削除

Dependents:   Nucleo_MPU_9250_ test_master_kourobo2018 Nucleo_MPU_9250_

PIDcontroller.cpp@0:eeb41e25a490, 2016-02-01 (annotated)

Committer:
porizou
Date:
Mon Feb 01 06:14:20 2016 +0000
Revision:
0:eeb41e25a490
add incompletedifferential

Who changed what in which revision?

UserRevisionLine numberNew contents of line
porizou 0:eeb41e25a490 1 #include "PIDcontroller.h"
porizou 0:eeb41e25a490 2
porizou 0:eeb41e25a490 3 PID::PID(float Kp, float Ki, float Kd, float tSample){
porizou 0:eeb41e25a490 4
porizou 0:eeb41e25a490 5 tSample_ = tSample;
porizou 0:eeb41e25a490 6
porizou 0:eeb41e25a490 7 Kp_ = Kp; Ki_ = Ki; Kd_ = Kd;
porizou 0:eeb41e25a490 8
porizou 0:eeb41e25a490 9 setPoint_ = 0.0;
porizou 0:eeb41e25a490 10 processVariable_ = 0.0;
porizou 0:eeb41e25a490 11 controllerOutput_ = 0.0;
porizou 0:eeb41e25a490 12 prevControllerOutput_ = 0.0;
porizou 0:eeb41e25a490 13
porizou 0:eeb41e25a490 14 accError_ = 0.0;
porizou 0:eeb41e25a490 15 Error_ = 0.0;
porizou 0:eeb41e25a490 16 prevError_= 0.0;
porizou 0:eeb41e25a490 17 Bias_ = 0.0;
porizou 0:eeb41e25a490 18
porizou 0:eeb41e25a490 19 usingFeedForward = 0;
porizou 0:eeb41e25a490 20 usingIncompleteDifferential = 0;
porizou 0:eeb41e25a490 21
porizou 0:eeb41e25a490 22 prevDiffOut = 0.0;
porizou 0:eeb41e25a490 23 u_ = 0.0;
porizou 0:eeb41e25a490 24 }
porizou 0:eeb41e25a490 25
porizou 0:eeb41e25a490 26 void PID::setInputLimits(float inMin, float inMax){
porizou 0:eeb41e25a490 27
porizou 0:eeb41e25a490 28 if(inMin >= inMax) return;
porizou 0:eeb41e25a490 29
porizou 0:eeb41e25a490 30 inMin_ = inMin;
porizou 0:eeb41e25a490 31 inMax_ = inMax;
porizou 0:eeb41e25a490 32 inSpan_ = inMax - inMin;
porizou 0:eeb41e25a490 33 }
porizou 0:eeb41e25a490 34
porizou 0:eeb41e25a490 35 void PID::setOutputLimits(float outMin, float outMax){
porizou 0:eeb41e25a490 36
porizou 0:eeb41e25a490 37 if(outMin >= outMax) return;
porizou 0:eeb41e25a490 38
porizou 0:eeb41e25a490 39 outMin_ = outMin;
porizou 0:eeb41e25a490 40 outMax_ = outMax;
porizou 0:eeb41e25a490 41 outSpan_ = outMax - outMin;
porizou 0:eeb41e25a490 42 }
porizou 0:eeb41e25a490 43
porizou 0:eeb41e25a490 44 void PID::setBias(float Bias) {
porizou 0:eeb41e25a490 45 Bias_ = Bias;
porizou 0:eeb41e25a490 46 usingFeedForward = 1;
porizou 0:eeb41e25a490 47 }
porizou 0:eeb41e25a490 48
porizou 0:eeb41e25a490 49 void PID::setIncompleteDifferential(float u) {
porizou 0:eeb41e25a490 50 u_ = u;
porizou 0:eeb41e25a490 51 usingIncompleteDifferential = 1;
porizou 0:eeb41e25a490 52 }
porizou 0:eeb41e25a490 53
porizou 0:eeb41e25a490 54 void PID::setSetPoint(float sp){
porizou 0:eeb41e25a490 55 setPoint_ = sp;
porizou 0:eeb41e25a490 56 }
porizou 0:eeb41e25a490 57
porizou 0:eeb41e25a490 58 void PID::setProcessValue(float pv){
porizou 0:eeb41e25a490 59 processVariable_ = pv;
porizou 0:eeb41e25a490 60 }
porizou 0:eeb41e25a490 61
porizou 0:eeb41e25a490 62 void PID::setGain(float Kp, float Ki, float Kd) {
porizou 0:eeb41e25a490 63 Kp_ = Kp; Ki_ = Ki; Kd_ = Kd;
porizou 0:eeb41e25a490 64 }
porizou 0:eeb41e25a490 65
porizou 0:eeb41e25a490 66 float PID::compute(){
porizou 0:eeb41e25a490 67
porizou 0:eeb41e25a490 68 //現在値と目標値の値を0~100%の範囲に置き換える
porizou 0:eeb41e25a490 69 float scaledPV = scaledParcent((processVariable_ - inMin_) / inSpan_);
porizou 0:eeb41e25a490 70
porizou 0:eeb41e25a490 71 float scaledSP = scaledParcent((setPoint_ - inMin_) / inSpan_);
porizou 0:eeb41e25a490 72
porizou 0:eeb41e25a490 73 //偏差の計算
porizou 0:eeb41e25a490 74 Error_= scaledSP - scaledPV;
porizou 0:eeb41e25a490 75
porizou 0:eeb41e25a490 76 //アンチワインドアップ
porizou 0:eeb41e25a490 77 if (!(prevControllerOutput_ >= 1 && Error_ > 0) && !(prevControllerOutput_ <= 0 && Error_ < 0)) {
porizou 0:eeb41e25a490 78 accError_ += (Error_ + prevError_) / 2.0 * tSample_; //偏差の積分値の計算
porizou 0:eeb41e25a490 79 }
porizou 0:eeb41e25a490 80
porizou 0:eeb41e25a490 81 //偏差の微分値の計算(不完全微分が有効な場合,偏差の不完全微分値を計算)
porizou 0:eeb41e25a490 82 float diffError = usingIncompleteDifferential ? calcIncompleteDifferential() : (Error_ - prevError_) / tSample_;
porizou 0:eeb41e25a490 83
porizou 0:eeb41e25a490 84 //フィードフォワード制御が有効な場合,バイアス値の計算
porizou 0:eeb41e25a490 85 float scaledBias = usingFeedForward ? (Bias_ - outMin_) / outSpan_ : 0.0;
porizou 0:eeb41e25a490 86
porizou 0:eeb41e25a490 87 //PIDの計算
porizou 0:eeb41e25a490 88 controllerOutput_ = scaledParcent(scaledBias + Kp_ * Error_ + Ki_ * accError_ + Kd_ * diffError);
porizou 0:eeb41e25a490 89
porizou 0:eeb41e25a490 90 //出力の値,偏差の値を更新
porizou 0:eeb41e25a490 91 prevControllerOutput_ = controllerOutput_;
porizou 0:eeb41e25a490 92 prevError_ = Error_;
porizou 0:eeb41e25a490 93 //微分項の値を更新
porizou 0:eeb41e25a490 94 prevDiffOut = Kd_ * diffError;
porizou 0:eeb41e25a490 95 //PIDの出力を実際の値に変換して返す
porizou 0:eeb41e25a490 96 return ((controllerOutput_ * outSpan_) + outMin_);
porizou 0:eeb41e25a490 97
porizou 0:eeb41e25a490 98 }
porizou 0:eeb41e25a490 99
porizou 0:eeb41e25a490 100 float PID::scaledParcent(float value) {
porizou 0:eeb41e25a490 101
porizou 0:eeb41e25a490 102 if(value > 1.0) {
porizou 0:eeb41e25a490 103 return 1.0;
porizou 0:eeb41e25a490 104 } else if(value < 0.0) {
porizou 0:eeb41e25a490 105 return 0.0;
porizou 0:eeb41e25a490 106 }
porizou 0:eeb41e25a490 107 else return value;
porizou 0:eeb41e25a490 108 }
porizou 0:eeb41e25a490 109
porizou 0:eeb41e25a490 110 /**
porizou 0:eeb41e25a490 111 * 不完全微分の式
porizou 0:eeb41e25a490 112 *
porizou 0:eeb41e25a490 113 * 伝達関数
porizou 0:eeb41e25a490 114 * Yn = (Td*s / (1 + k*Td*s)) * E
porizou 0:eeb41e25a490 115 *
porizou 0:eeb41e25a490 116 * 差分の式
porizou 0:eeb41e25a490 117 * yn = (k*Td / (Δt + k*Td)) * yn-1 + (Td / (Δt + k*Td))*(en - en-1)
porizou 0:eeb41e25a490 118 *
porizou 0:eeb41e25a490 119 * Δt 制御周期
porizou 0:eeb41e25a490 120 * k 定数
porizou 0:eeb41e25a490 121 * Td 微分ゲイン
porizou 0:eeb41e25a490 122 * yn 現在の出力
porizou 0:eeb41e25a490 123 * yn-1 前回の出力
porizou 0:eeb41e25a490 124 * en 現在の偏差
porizou 0:eeb41e25a490 125 * en-1 前回の偏差
porizou 0:eeb41e25a490 126 *
porizou 0:eeb41e25a490 127 * 参考文献 http://www.nikko-pb.co.jp/products/k_data/P12_13.pdf
porizou 0:eeb41e25a490 128 */
porizou 0:eeb41e25a490 129
porizou 0:eeb41e25a490 130 float PID::calcIncompleteDifferential(void) {
porizou 0:eeb41e25a490 131
porizou 0:eeb41e25a490 132 float k = 1 / (tSample_ + u_ * Kd_);
porizou 0:eeb41e25a490 133
porizou 0:eeb41e25a490 134 return (k * u_ * prevDiffOut) + (k * (Error_ - prevError_));
porizou 0:eeb41e25a490 135 }