chose PID library implement PID (weight is NOT determined)
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
PIDcontroller.cpp
00001 #include "PIDcontroller.h" 00002 00003 PID::PID(float Kp, float Ki, float Kd, float tSample){ 00004 00005 tSample_ = tSample; 00006 00007 Kp_ = Kp; Ki_ = Ki; Kd_ = Kd; 00008 00009 setPoint_ = 0.0; 00010 processVariable_ = 0.0; 00011 controllerOutput_ = 0.0; 00012 prevControllerOutput_ = 0.0; 00013 00014 accError_ = 0.0; 00015 Error_ = 0.0; 00016 prevError_= 0.0; 00017 Bias_ = 0.0; 00018 00019 usingFeedForward = 0; 00020 usingIncompleteDifferential = 0; 00021 00022 prevDiffOut = 0.0; 00023 u_ = 0.0; 00024 } 00025 00026 void PID::setInputLimits(float inMin, float inMax){ 00027 00028 if(inMin >= inMax) return; 00029 00030 inMin_ = inMin; 00031 inMax_ = inMax; 00032 inSpan_ = inMax - inMin; 00033 } 00034 00035 void PID::setOutputLimits(float outMin, float outMax){ 00036 00037 if(outMin >= outMax) return; 00038 00039 outMin_ = outMin; 00040 outMax_ = outMax; 00041 outSpan_ = outMax - outMin; 00042 } 00043 00044 void PID::setBias(float Bias) { 00045 Bias_ = Bias; 00046 usingFeedForward = 1; 00047 } 00048 00049 void PID::setIncompleteDifferential(float u) { 00050 u_ = u; 00051 usingIncompleteDifferential = 1; 00052 } 00053 00054 void PID::setSetPoint(float sp){ 00055 setPoint_ = sp; 00056 } 00057 00058 void PID::setProcessValue(float pv){ 00059 processVariable_ = pv; 00060 } 00061 00062 void PID::setGain(float Kp, float Ki, float Kd) { 00063 Kp_ = Kp; Ki_ = Ki; Kd_ = Kd; 00064 } 00065 00066 float PID::compute(){ 00067 00068 //現在値と目標値の値を0~100%の範囲に置き換える 00069 float scaledPV = scaledParcent((processVariable_ - inMin_) / inSpan_); 00070 00071 float scaledSP = scaledParcent((setPoint_ - inMin_) / inSpan_); 00072 00073 //偏差の計算 00074 Error_= scaledSP - scaledPV; 00075 00076 //アンチワインドアップ 00077 if (!(prevControllerOutput_ >= 1 && Error_ > 0) && !(prevControllerOutput_ <= 0 && Error_ < 0)) { 00078 accError_ += (Error_ + prevError_) / 2.0 * tSample_; //偏差の積分値の計算 00079 } 00080 00081 //偏差の微分値の計算(不完全微分が有効な場合,偏差の不完全微分値を計算) 00082 float diffError = usingIncompleteDifferential ? calcIncompleteDifferential() : (Error_ - prevError_) / tSample_; 00083 00084 //フィードフォワード制御が有効な場合,バイアス値の計算 00085 float scaledBias = usingFeedForward ? (Bias_ - outMin_) / outSpan_ : 0.0; 00086 00087 //PIDの計算 00088 controllerOutput_ = scaledParcent(scaledBias + Kp_ * Error_ + Ki_ * accError_ + Kd_ * diffError); 00089 00090 //出力の値,偏差の値を更新 00091 prevControllerOutput_ = controllerOutput_; 00092 prevError_ = Error_; 00093 //微分項の値を更新 00094 prevDiffOut = Kd_ * diffError; 00095 //PIDの出力を実際の値に変換して返す 00096 return ((controllerOutput_ * outSpan_) + outMin_); 00097 00098 } 00099 00100 float PID::scaledParcent(float value) { 00101 00102 if(value > 1.0) { 00103 return 1.0; 00104 } else if(value < 0.0) { 00105 return 0.0; 00106 } 00107 else return value; 00108 } 00109 00110 void PID::resetIntError(){ 00111 accError_ = 0.0; 00112 } 00113 00114 /** 00115 * 不完全微分の式 00116 * 00117 * 伝達関数 00118 * Yn = (Td*s / (1 + k*Td*s)) * E 00119 * 00120 * 差分の式 00121 * yn = (k*Td / (Δt + k*Td)) * yn-1 + (Td / (Δt + k*Td))*(en - en-1) 00122 * 00123 * Δt 制御周期 00124 * k 定数 00125 * Td 微分ゲイン 00126 * yn 現在の出力 00127 * yn-1 前回の出力 00128 * en 現在の偏差 00129 * en-1 前回の偏差 00130 * 00131 * 参考文献 http://www.nikko-pb.co.jp/products/k_data/P12_13.pdf 00132 */ 00133 00134 float PID::calcIncompleteDifferential(void) { 00135 00136 float k = 1 / (tSample_ + u_ * Kd_); 00137 00138 return (k * u_ * prevDiffOut) + (k * (Error_ - prevError_)); 00139 }
Generated on Sun Jul 24 2022 01:28:27 by 1.7.2