mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
PIDController.cpp
[詳解]
1
5
6#include "PIDController.hpp"
7
8namespace {
9// 出力制限。範囲外は端に丸める
10double clampOutput(double value, double min_value, double max_value) {
11 if (value > max_value) {
12 return max_value;
13 }
14 if (value < min_value) {
15 return min_value;
16 }
17 return value;
18}
19} // namespace
20
21PIDController::PIDController(double kp, double ki, double kd, double nominal_dt)
22 : kp_(kp)
23 , ki_(ki)
24 , kd_(kd)
25 , nominal_dt_(nominal_dt)
26 , err_sum_(0.0)
27 , prev_err_(0.0)
28 , output_min_(-255.0)
29 , output_max_(255.0)
30{
31}
32
33void PIDController::setGains(double kp, double ki, double kd) {
34 kp_ = kp;
35 ki_ = ki;
36 kd_ = kd;
37}
38
39void PIDController::setNominalDt(double nominal_dt) {
40 nominal_dt_ = nominal_dt;
41}
42
43void PIDController::setOutputLimits(double min, double max) {
44 output_min_ = min;
45 output_max_ = max;
46}
47
48double PIDController::compute(double setpoint, double measured, double dt_sec) {
49 // 誤差計算
50 const double error = setpoint - measured;
51
52 // dt異常時 (<=0) はP+保持Iのみ出力し、積分・微分状態を凍結する
53 if (dt_sec <= 0.0) {
54 return clampOutput(kp_ * error + ki_ * err_sum_, output_min_, output_max_);
55 }
56
57 // スコープ内ローカルでdtを公称周期の1/3〜3倍にクランプする。
58 // これによりゲインの効きが更新周期に依存しなくなる
59 double dt = dt_sec;
60 if (nominal_dt_ > 0.0) {
61 const double dt_min = nominal_dt_ / 3.0;
62 const double dt_max = nominal_dt_ * 3.0;
63 if (dt < dt_min) {
64 dt = dt_min;
65 } else if (dt > dt_max) {
66 dt = dt_max;
67 }
68 }
69 const double p = kp_ * error;
70 // I項の候補 (条件付き積分でアンチワインドアップ。dt正規化済み)
71 const double trial_sum = err_sum_ + error * dt;
72 const double i = ki_ * trial_sum;
73 const double d = kd_ * (error - prev_err_) / dt;
74 const double trial_out = p + i + d;
75
76 // 出力制限
77 const double output = clampOutput(trial_out, output_min_, output_max_);
78
79 // 飽和していない、または飽和を緩和する方向なら積分を採用。
80 // 飽和を悪化させる方向(errorと出力が同符号)のみ積分を凍結する
81 if (output == trial_out || (error * trial_out) < 0.0) {
82 err_sum_ = trial_sum;
83 }
84
85 // 前回誤差を保存
86 prev_err_ = error;
87
88 return output;
89}
90
92 err_sum_ = 0.0;
93 prev_err_ = 0.0;
94}
目標値と計測値の差から、上限を考慮した制御出力を計算する。時間間隔は秒で受け取る。
void setGains(double kp, double ki, double kd)
PIDゲインを設定
void setOutputLimits(double min, double max)
出力リミットを設定
PIDController(double kp, double ki, double kd, double nominal_dt=0.015)
コンストラクタ
void reset()
PID制御状態をリセット
double compute(double setpoint, double measured, double dt_sec)
PID制御計算を実行
void setNominalDt(double nominal_dt)
公称制御周期を設定