chose PID library implement PID (weight is NOT determined)
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
PIDcontroller.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-08-26
- Revision:
- 2:a05ba9d3be55
- Parent:
- 0:eeb41e25a490
- Child:
- 3:c99dfa48b606
File content as of revision 2:a05ba9d3be55:
#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; accErrorMax_ = 1.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; } void PID::setAccErrorMax(float maxVal) { accErrorMax_ = maxVal; } 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_; //偏差の積分値の計算 } if(accError_> accErrorMax_){accError_= accErrorMax_} if(accError_< -accErrorMax_){accError_= -accErrorMax_} //偏差の微分値の計算(不完全微分が有効な場合,偏差の不完全微分値を計算) 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_)); }