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?
User | Revision | Line number | New 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 | } |