chose PID library implement PID (weight is NOT determined)
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: PIDcontroller.cpp
- Revision:
- 2:a05ba9d3be55
- Parent:
- 0:eeb41e25a490
- Child:
- 3:c99dfa48b606
diff -r 5f8577f03edb -r a05ba9d3be55 PIDcontroller.cpp --- a/PIDcontroller.cpp Tue Feb 09 03:31:04 2021 +0000 +++ b/PIDcontroller.cpp Thu Aug 26 04:26:47 2021 +0000 @@ -12,6 +12,7 @@ prevControllerOutput_ = 0.0; accError_ = 0.0; + accErrorMax_ = 1.0; Error_ = 0.0; prevError_= 0.0; Bias_ = 0.0; @@ -63,6 +64,10 @@ Kp_ = Kp; Ki_ = Ki; Kd_ = Kd; } +void PID::setAccErrorMax(float maxVal) { + accErrorMax_ = maxVal; +} + float PID::compute(){ //現在値と目標値の値を0~100%の範囲に置き換える @@ -77,13 +82,16 @@ 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);