HAPSRG / PIDcontroller

chose PID library implement PID (weight is NOT determined)

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

PIDcontroller.cpp@2:a05ba9d3be55, 2021-08-26 (annotated)

Committer:
NaotoMorita
Date:
Thu Aug 26 04:26:47 2021 +0000
Revision:
2:a05ba9d3be55
Parent:
0:eeb41e25a490
Child:
3:c99dfa48b606
restrect I gain

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