mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
PIDController.cpp
[詳解]
1
5
6
#include "
PIDController.hpp
"
7
8
namespace
{
9
// 出力制限。範囲外は端に丸める
10
double
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
21
PIDController::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
33
void
PIDController::setGains
(
double
kp,
double
ki,
double
kd) {
34
kp_ = kp;
35
ki_ = ki;
36
kd_ = kd;
37
}
38
39
void
PIDController::setNominalDt
(
double
nominal_dt) {
40
nominal_dt_ = nominal_dt;
41
}
42
43
void
PIDController::setOutputLimits
(
double
min,
double
max) {
44
output_min_ = min;
45
output_max_ = max;
46
}
47
48
double
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
91
void
PIDController::reset
() {
92
err_sum_ = 0.0;
93
prev_err_ = 0.0;
94
}
PIDController.hpp
目標値と計測値の差から、上限を考慮した制御出力を計算する。時間間隔は秒で受け取る。
PIDController::setGains
void setGains(double kp, double ki, double kd)
PIDゲインを設定
定義
PIDController.cpp:33
PIDController::setOutputLimits
void setOutputLimits(double min, double max)
出力リミットを設定
定義
PIDController.cpp:43
PIDController::PIDController
PIDController(double kp, double ki, double kd, double nominal_dt=0.015)
コンストラクタ
定義
PIDController.cpp:21
PIDController::reset
void reset()
PID制御状態をリセット
定義
PIDController.cpp:91
PIDController::compute
double compute(double setpoint, double measured, double dt_sec)
PID制御計算を実行
定義
PIDController.cpp:48
PIDController::setNominalDt
void setNominalDt(double nominal_dt)
公称制御周期を設定
定義
PIDController.cpp:39
lib
PIDCont
PIDController.cpp
構築:
1.18.0