前言
大家好，今天我們介紹一下經(jīng)典控制理論中的PID控制算法，并著重講解該算法的編碼實現(xiàn)，為實現(xiàn)后續(xù)的倒立擺樣例內(nèi)容做準備。 眾所周知，掌握了 PID ，就相當(dāng)于進入了控制工程的大門，也能為更高階的控制理論學(xué)習(xí)打下基礎(chǔ)。 在很多的自動化控制領(lǐng)域。都會遇到PID控制算法，這種算法具有很好的控制模式，可以讓系統(tǒng)具有很好的魯棒性。
基本介紹
PID 深入理解
（1）閉環(huán)控制系統(tǒng)：講解 PID 之前，我們先解釋什么是閉環(huán)控制系統(tǒng)。簡單說就是一個有輸入有輸出的系統(tǒng)，輸入能影響輸出。一般情況下，人們也稱輸出為反饋，因此也叫閉環(huán)反饋控制系統(tǒng)。比如恒溫水池，輸入就是加熱功率，輸出就是水溫度；比如冷庫，輸入是空調(diào)功率，輸出是內(nèi)部溫度。
（2）什么是PID：英文分解開就是：比例（proportional）、積分（integral）、微分（derivative），其根據(jù)系統(tǒng)反饋，通過比例，積分和微分三個部分的計算，動態(tài)調(diào)整系統(tǒng)輸入，確保被控量穩(wěn)定在人們設(shè)定的目標值附近。PID 是目前最常見的應(yīng)用于閉環(huán)反饋控制系統(tǒng)的算法，三個部分可以只用一個（P，I，D），也可以只用兩個（PI，PD），也可以三個一起用（PID），非常靈活。
（3）PID控制原理圖與表達式：
上面的控制原理圖與下面的數(shù)學(xué)表達式是相互對應(yīng)的。
setpoint 為設(shè)定值，也叫目標值；output(t) 是系統(tǒng)反饋值，隨時間變化；e(t) 是設(shè)定值與反饋值的差值，由于反饋總是作為被減數(shù)，因此也稱為負反饋控制算法；Kp 是比例系數(shù)，Kp * e(t) 就是 PID 的比例部分；Ki 是積分系數(shù)，Ki 乘以 e(t) 對時間的積分，就是 PID 的積分部分；Kd 是微分系數(shù)，Kd 乘以 e(t) 對時間的微分，就是 PID 的微分部分。通常情況下，三個系數(shù)都是正數(shù)，但三個部分正負號并不一定相同，相互之間有抵消和補償。三個部分之和，就是系統(tǒng)輸入值 input(t)。整個控制系統(tǒng)的目標就是讓差值 e(t) 穩(wěn)定到 0。
（4）我們以恒溫水池為例，講解 PID 的三個部分：其中 input(t) 為加熱功率，output(t) 為水池溫度，setpoint 假設(shè)為 36 度， e(t) 為 setpoint 與當(dāng)前溫度的差值 。
比例部分：比例部分最直觀，也比較容易理解，舉例而言：假設(shè)當(dāng)前水溫為 20 度，差值 e 為 36 - 20 = 16 度，乘上比例系數(shù) Kp ，得到加熱功率，于是溫度就會慢慢上漲；如果水溫超過了設(shè)定溫度，比如 40 度，差值 e 為 36 - 40 = -4 度，則停止加熱，讓熱量耗散，溫度就會慢慢下降。
微分部分：只有比例部分，我們可以想象出水池溫度的變化通常會比較大，而且很難恒定，這樣的水池不能算是恒溫水池。解決辦法是引入差值 e(t) 的微分，也就是 e(t) 對時間的導(dǎo)數(shù)。通過數(shù)學(xué)計算，可得導(dǎo)數(shù)為水池溫度的斜率負數(shù)：
根據(jù)求導(dǎo)結(jié)果，我們分兩種情況討論微分部分對比例部分的作用：當(dāng)差值 e(t) 擴大時：微分部分將與比例部分同正負號，對比例部分進行補償，更好的抑制差值擴大；當(dāng)差值 e(t) 縮小時：微分部分將與比例部分異號，對比例部分進行抵消，防止系統(tǒng)輸出過沖。綜合兩種情況，可以認為微分部分提供了一種預(yù)測性的調(diào)控作用，通過考慮差值 e(t) 的未來走勢，更精細地調(diào)整系統(tǒng)輸入，從而讓系統(tǒng)輸出逐漸收斂到目標值。
積分部分：只有比例和微分部分，在某些場景下會失靈。舉例而言，假如我們只使用 PD 算法。此時水池的室外溫度非常低，熱量散失非?？臁．?dāng)加熱到某個溫度的時候（比如 30 度），溫度可能再也無法上漲。這種情況，稱之為系統(tǒng)的穩(wěn)態(tài)誤差。我們分兩部分解釋原因：比例部分：由于差值 e(t) 不那么大了，比例部分會比較小，每次增加的熱量正好被耗散掉，因此溫度不會繼續(xù)上升；微分部分：由于溫度基本恒定，微分部分將約為零，也無法對比例部分進行補償。解決辦法是引入差值 e(t) 的積分，也就是 e(t) 乘以單位時間并不斷累加，數(shù)學(xué)表達式如下：
假設(shè)溫度停在了 30 度，不再上升，此時，積分部分會隨著時間的推移而不斷增加，相當(dāng)于對比例部分進行補償，從而增加加熱功率，最終溫度將繼續(xù)上升。下面的動圖比較形象地展示了三個參數(shù)對系統(tǒng)輸出的影響：
（5）PID 為什么被稱為啟發(fā)式控制算法：
第一，PID 的三個參數(shù)并非基于嚴格的數(shù)學(xué)計算得到，而是靠工程師的直覺和經(jīng)驗。第二，PID 算法調(diào)參的目標是可用，只要實際效果不錯就行，并不追求最優(yōu)解。第三，PID 不依賴精確的數(shù)學(xué)模型，就能進行有效的控制。因此看起來更像是一種基于實踐和實際效果的啟發(fā)式方法，而不是一個理論上推導(dǎo)出來的控制策略。（6）介紹一種 PID 調(diào)參方法：Ziegler-Nichols（齊格勒-尼科爾斯）最終值振蕩法第一，將微分系數(shù) Kd 和積分系數(shù) Kp 都設(shè)置為 0，只保留比例系數(shù)。第二，不斷增加比例系數(shù)，直到達到無衰減的持續(xù)振蕩，此時的比例系數(shù)稱為 Ku ，此時的振蕩周期為 Tu。第三，使用臨界系數(shù)和振蕩周期設(shè)置 PID 參數(shù)：
比例系數(shù)：Kp = 0.60 * Ku積分系數(shù)：Ki = 2 * Kp / Tu微分系數(shù)：Kd = Kp * Tu / 8
PID 編碼實現(xiàn)
這部分我們主要參考 Arduino 的 PID 庫 Arduino-PID-Library，分八步實現(xiàn)一個實際可用的 PID 算法庫。接下來的每一步都需要大家認真的閱讀，因為涉及到很多的細節(jié)。
特別提示：由于本節(jié)講解 PID 的實現(xiàn)，我們將以 PID 作為第一視角，如果提到 input ，指的是 PID 算法輸入，相當(dāng)于上節(jié)中的系統(tǒng)輸出 output(t)，即恒溫水池的溫度；如果提到 ouput，指的是 PID 算法輸出，相當(dāng)于上節(jié)中的系統(tǒng)輸入 input(t)，即加熱功率。
初始版本
代碼實現(xiàn) PID 算法，面臨最大的困惑是如何實現(xiàn)積分和微分。正如上一節(jié)所說，積分可轉(zhuǎn)化為差值 e(t) 乘以采樣間隔并不斷累加；微分可轉(zhuǎn)換為求兩次采樣的差值 e(t) 的斜率。于是有了如下代碼，請讀者關(guān)注代碼注釋（可以直接拿去跑）。
#include
#include
#include
class PIDController {
public:
explicit PIDController() {
InitTime();
}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {
InitTime();
}
void InitTime() {
last_time_ = GetMillis();
}
double Compute(double setpoint, double input) {
uint64_t now = GetMillis();
double time_change = static_cast(now - last_time_);
double error = setpoint - input;
printf("error: %f\n", error);
err_sum_ += error * time_change;
double derivative = (error - last_error_) / time_change;
double output = kp_ * error + ki_ * err_sum_ + kd_ * derivative;
last_error_ = error;
last_time_ = now;
return output;
}
void set_tunings(double kp_para, double ki_para, double kd_para) {
kp_ = kp_para;
ki_ = ki_para;
kd_ = kd_para;
}
private:
double kp_;
double ki_;
double kd_;
double last_error_ = 0;
double err_sum_ = 0;
uint64_t last_time_ = 0;
uint64_t GetMillis() {
return std::chrono::duration_cast(
std::chrono::steady_clock::now().time_since_epoch())
.count();
}
};
int main() {
PIDController pid;
pid.set_tunings(10, 0.01, 0.01);
double setpoint = 36;
double temperature = 20;
std::this_thread::sleep_for(std::chrono::seconds(1));
for (int i = 0; i < 100; ++i) {
double control_signal = pid.Compute(setpoint, temperature);
temperature += control_signal * 0.1;
temperature *= 0.99;
std::cout << "Temperature: " << temperature << std::endl;
std::this_thread::sleep_for(std::chrono::seconds(1));
}
return 0;
}
固定采樣間隔
初始版本的 PID 的采樣間隔是由外部循環(huán)控制的，會導(dǎo)致兩個問題：第一，無法獲取一致的 PID 行為，因為外部有可能調(diào)用，也有可能不調(diào)用；第二，每次都要根據(jù)采樣間隔計算微分和積分部分，這涉及到浮點運算。效率比較低。好的辦法是固定采用間隔，兩個問題都能解決，看下面的代碼以及注釋（可以直接拿去跑）。
#include
#include
#include
class PIDController {
public:
explicit PIDController() {
InitTime();
}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {
InitTime();
}
void InitTime() {
last_time_ = GetMillis();
}
void set_tunings(double kp_para, double ki_para, double kd_para) {
double sample_time_in_sec = static_cast(sample_time_) / 1000.0;
kp_ = kp_para;
// sum = ki * (error(0) * dt + error(1) * dt + ... + error(n) * dt) = (ki * dt) * (error(0) + error(1) + ... + error(n))
ki_ = ki_para * sample_time_in_sec;
// derivative = kd * (error(n) - error(n-1)) / dt = (kd / dt) * (error(n) - error(n-1))
kd_ = kd_para / sample_time_in_sec;
}
void set_sample_time(uint64_t new_sample_time) {
if (new_sample_time > 0) {
double ratio = static_cast(new_sample_time) / static_cast(sample_time_);
ki_ = ki_ * ratio;
kd_ = kd_ / ratio;
sample_time_ = new_sample_time;
}
}
double Compute(double setpoint, double input) {
uint64_t now = GetMillis();
uint64_t time_change = now - last_time_;
if (time_change < sample_time_) {
return last_output_;
}
double error = setpoint - input;
printf("error: %f\n", error);
err_sum_ += error;
double derivative = error - last_error_;
double output = kp_ * error + ki_ * err_sum_ + kd_ * derivative;
last_error_ = error;
last_time_ = now;
last_output_ = output;
return output;
}
private:
double kp_;
double ki_;
double kd_;
double last_error_ = 0.0;
double err_sum_ = 0.0;
uint64_t last_time_ = 0UL;
double last_output_ = 0.0;
uint64_t sample_time_ = 1000UL; // 1 second
uint64_t GetMillis() {
return std::chrono::duration_cast(
std::chrono::steady_clock::now().time_since_epoch())
.count();
}
};
int main() {
PIDController pid;
pid.set_tunings(1, 0.2, 0.02);
pid.set_sample_time(1000); // Set sample time to 1 second
double setpoint = 36;
double temperature = 20;
std::this_thread::sleep_for(std::chrono::seconds(1));
for (int i = 0; i < 1000; ++i) {
double control_signal = pid.Compute(setpoint, temperature);
temperature += control_signal * 0.1;
temperature *= 0.99;
std::cout << "Temperature: " << temperature << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
return 0;
}
消除 spike
spike 的英文含義是尖刺，這里指的是當(dāng)系統(tǒng)運行過程中，突然改變 setpoint 時， PID 的微分部分會因 setpoint 的突然切換而生成一個極大的導(dǎo)數(shù)，導(dǎo)致算法輸出值 output 將產(chǎn)生一次急劇變化，這就是 spike。比如恒溫水池的初始 setpoint 是 36 度，運行過程中，突然改為 50 度。相當(dāng)于在一個采樣周期內(nèi)，差值 error 突然增加了 14 ，再除以采樣周期，數(shù)值將會非常大，如下圖所示。
解決辦法是將 setpoint 從 PID 的微分部分請出去，理論依據(jù)是：差值 error 的導(dǎo)數(shù)也是算法輸入（恒溫水池的溫度）的斜率負數(shù)：
代碼實現(xiàn)如下：
#include
#include
#include
class PIDController {
public:
explicit PIDController() {
InitTime();
}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {
InitTime();
}
void InitTime() {
last_time_ = GetMillis();
}
void set_tunings(double kp_para, double ki_para, double kd_para) {
double sample_time_in_sec = static_cast(sample_time_) / 1000.0;
kp_ = kp_para;
ki_ = ki_para * sample_time_in_sec;
kd_ = kd_para / sample_time_in_sec;
}
void set_sample_time(uint64_t new_sample_time) {
if (new_sample_time > 0) {
double ratio = static_cast(new_sample_time) / static_cast(sample_time_);
ki_ = ki_ * ratio;
kd_ = kd_ / ratio;
sample_time_ = new_sample_time;
}
}
double Compute(double setpoint, double input) {
uint64_t now = GetMillis();
uint64_t time_change = now - last_time_;
if (time_change < sample_time_) {
return last_output_;
}
double error = setpoint - input;
printf("error: %f\n", error);
err_sum_ += error;
double derivative = input - last_input_;
double output = kp_ * error + ki_ * err_sum_ - kd_ * derivative;
last_input_ = input;
last_time_ = now;
last_output_ = output;
return output;
}
private:
double kp_;
double ki_;
double kd_;
double last_input_ = 0.0;
double err_sum_ = 0.0;
uint64_t last_time_ = 0UL;
double last_output_ = 0.0;
uint64_t sample_time_ = 1000UL; // 1 second
uint64_t GetMillis() {
return std::chrono::duration_cast(
std::chrono::steady_clock::now().time_since_epoch())
.count();
}
};
int main() {
PIDController pid;
pid.set_tunings(1, 0.2, 0.02);
pid.set_sample_time(1000);
double setpoint = 36;
double temperature = 20;
std::this_thread::sleep_for(std::chrono::seconds(1));
for (int i = 0; i < 1000; ++i) {
double control_signal = pid.Compute(setpoint, temperature);
temperature += control_signal * 0.1;
temperature *= 0.99;
std::cout << "Temperature: " << temperature << std::endl;
if (i == 200) {
setpoint = 50;
std::cout << "Setpoint changed to 50" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
return 0;
}
動態(tài)改參
好的 PID 算法，允許在系統(tǒng)運行過程中，調(diào)整 PID 參數(shù)。問題的關(guān)鍵是，運行中途修改 PID 參數(shù)，如何保持算法輸出仍然平穩(wěn)，對系統(tǒng)狀態(tài)不產(chǎn)生額外沖擊。仔細分析 PID 的三個部分，當(dāng)對應(yīng)的參數(shù)改變時，影響最大的是積分部分，比例和微分兩部分都只影響當(dāng)前值，而積分部分將會更改歷史值。
解決辦法是放棄先計算積分和，最后乘以積分系數(shù)的做法，而是讓積分系數(shù)參與每一次積分運算并累加起來：
如此一來，即使更新了積分參數(shù)，也只影響當(dāng)前值，歷史值由于被存儲起來，因此不會改變，代碼實現(xiàn)如下 。
#include
#include
#include
class PIDController {
public:
explicit PIDController() {
InitTime();
}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {
InitTime();
}
void InitTime() {
last_time_ = GetMillis();
}
void set_tunings(double kp_para, double ki_para, double kd_para) {
double sample_time_in_sec = static_cast(sample_time_) / 1000.0;
kp_ = kp_para;
ki_ = ki_para * sample_time_in_sec;
kd_ = kd_para / sample_time_in_sec;
}
void set_sample_time(uint64_t new_sample_time) {
if (new_sample_time > 0) {
double ratio = static_cast(new_sample_time) / static_cast(sample_time_);
ki_ = ki_ * ratio;
kd_ = kd_ / ratio;
sample_time_ = new_sample_time;
}
}
double Compute(double setpoint, double input) {
uint64_t now = GetMillis();
uint64_t time_change = now - last_time_;
if (time_change < sample_time_) {
return last_output_;
}
double error = setpoint - input;
printf("error: %f\n", error);
err_item_sum_ += ki_ * error;
double derivative = input - last_input_;
double output = kp_ * error + err_item_sum_ - kd_ * derivative;
last_input_ = input;
last_time_ = now;
last_output_ = output;
return output;
}
private:
double kp_;
double ki_;
double kd_;
double last_input_ = 0.0;
double err_item_sum_ = 0.0;
uint64_t last_time_ = 0UL;
double last_output_ = 0.0;
uint64_t sample_time_ = 1000UL; // 1 second
uint64_t GetMillis() {
return std::chrono::duration_cast(
std::chrono::steady_clock::now().time_since_epoch())
.count();
}
};
int main() {
PIDController pid;
pid.set_tunings(1, 0.2, 0.02);
pid.set_sample_time(1000);
double setpoint = 36;
double temperature = 20;
std::this_thread::sleep_for(std::chrono::seconds(1));
for (int i = 0; i < 1000; ++i) {
double control_signal = pid.Compute(setpoint, temperature);
temperature += control_signal * 0.1;
temperature *= 0.99;
std::cout << "Temperature: " << temperature << std::endl;
if (i == 200) {
pid.set_tunings(1, 0.5, 0.02);
std::cout << "PID coefficients changed, 1, 0.2, 0.02 ->1, 0.5, 0.02" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
return 0;
}
設(shè)置算法輸出限制
通常情況下，PID 算法輸出是有一定限制的，比如恒溫水池的加熱功率不可能無限大，更不可能小于零。當(dāng) PID 的算法輸出為負數(shù)時，實際是停止加熱，也就是功率為零。因此需要給 PID 算法添加限制范圍，代碼實現(xiàn)如下。補充：為了看到輸出限制的作用，這次我們把目標溫度定為 90 度。
#include
#include
#include
class PIDController {
public:
explicit PIDController() {
InitTime();
}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {
InitTime();
}
void InitTime() {
last_time_ = GetMillis();
}
void set_tunings(double kp_para, double ki_para, double kd_para) {
double sample_time_in_sec = static_cast(sample_time_) / 1000.0;
kp_ = kp_para;
ki_ = ki_para * sample_time_in_sec;
kd_ = kd_para / sample_time_in_sec;
}
void set_sample_time(uint64_t new_sample_time) {
if (new_sample_time > 0) {
double ratio = static_cast(new_sample_time) / static_cast(sample_time_);
ki_ = ki_ * ratio;
kd_ = kd_ / ratio;
sample_time_ = new_sample_time;
}
}
void set_output_limits(double min, double max) {
if (min > max) {
return;
}
out_min_ = min;
out_max_ = max;
SetLimits(last_output_);
SetLimits(err_item_sum_);
}
double Compute(double setpoint, double input) {
uint64_t now = GetMillis();
uint64_t time_change = now - last_time_;
if (time_change < sample_time_) {
return last_output_;
}
double error = setpoint - input;
printf("error: %f\n", error);
err_item_sum_ += ki_ * error;
SetLimits(err_item_sum_);
double derivative = input - last_input_;
double output = kp_ * error + err_item_sum_ - kd_ * derivative;
SetLimits(output);
last_input_ = input;
last_time_ = now;
last_output_ = output;
return output;
}
private:
double kp_;
double ki_;
double kd_;
double last_input_ = 0.0;
double last_output_ = 0.0;
double err_item_sum_ = 0.0;
double out_min_ = 0.0;
double out_max_ = 0.0;
uint64_t last_time_ = 0UL;
uint64_t sample_time_ = 1000UL; // 1 second
uint64_t GetMillis() {
return std::chrono::duration_cast(
std::chrono::steady_clock::now().time_since_epoch())
.count();
}
void SetLimits(double& val) {
if (val > out_max_) {
printf("val: %f > out_max_: %f\n", val, out_max_);
val = out_max_;
} else if (val < out_min_) {
printf("val: %f > out_min_: %f\n", val, out_min_);
val = out_min_;
} else {
; // Do nothing
}
}
};
int main() {
PIDController pid;
pid.set_tunings(1, 0.5, 0.05);
pid.set_sample_time(1000);
pid.set_output_limits(0, 100);
double setpoint = 90;
double temperature = 20;
std::this_thread::sleep_for(std::chrono::seconds(1));
for (int i = 0; i < 1000; ++i) {
double control_signal = pid.Compute(setpoint, temperature);
temperature += control_signal * 0.1;
temperature *= 0.99;
std::cout << "Temperature: " << temperature << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
return 0;
}
添加開關(guān)控制
好的 PID 算法應(yīng)允許使用者動態(tài)啟停，比如恒溫水池運行過程中，由于某種原因，管理人員需要停掉自動控制，改為手動控制，操作結(jié)束后，重新啟動自動控制。實現(xiàn)動態(tài)停止并不復(fù)雜，只要 PID 內(nèi)部加一個開關(guān)標識，當(dāng)關(guān)閉時，PID 算法內(nèi)部不執(zhí)行計算，外部直接使用人工操作值替代算法輸出值進行控制。但問題的關(guān)鍵是，當(dāng)從手動模式重新改為自動模式時，需要保證恒溫水池溫度不出現(xiàn)大的抖動，即 PID 算法能接續(xù)人類的控制狀態(tài)，實現(xiàn)平滑過渡。解決辦法是重新初始化：當(dāng)從手動切換到自動時，將水池溫度和人工操作值傳給 PID ，更新 PID 內(nèi)部的歷史輸入值和歷史積分值。如此一來，當(dāng) PID 重新啟動時，就能接續(xù)人類的控制結(jié)果，平滑啟動，如圖所示。
#include
#include
#include
enum PID_MODE: uint8_t {
PID_MODE_MANUAL = 0,
PID_MODE_AUTOMATIC = 1
};
class PIDController {
public:
explicit PIDController() {
InitTime();
}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {
InitTime();
}
void InitTime() {
last_time_ = GetMillis();
}
void set_tunings(double kp_para, double ki_para, double kd_para) {
double sample_time_in_sec = static_cast(sample_time_) / 1000.0;
kp_ = kp_para;
ki_ = ki_para * sample_time_in_sec;
kd_ = kd_para / sample_time_in_sec;
}
void set_sample_time(uint64_t new_sample_time) {
if (new_sample_time > 0) {
double ratio = static_cast(new_sample_time) / static_cast(sample_time_);
ki_ = ki_ * ratio;
kd_ = kd_ / ratio;
sample_time_ = new_sample_time;
}
}
void set_output_limits(double min, double max) {
if (min > max) {
return;
}
out_min_ = min;
out_max_ = max;
SetLimits(last_output_);
SetLimits(err_item_sum_);
}
void InitInnaState(double input, double output) {
last_input_ = input;
err_item_sum_ = output;
SetLimits(err_item_sum_);
}
void set_auto_mode(PID_MODE mode, double input = 0.0, double output = 0.0) {
bool new_auto = (mode == PID_MODE_AUTOMATIC);
if (new_auto == true && in_auto_ == false) {
InitInnaState(input, output);
}
in_auto_ = new_auto;
std::cout << "PID mode: " << (in_auto_ ? "Automatic" : "Manual") << std::endl;
}
double Compute(double setpoint, double input) {
if (in_auto_ == false) {
return last_output_;
}
uint64_t now = GetMillis();
uint64_t time_change = now - last_time_;
if (time_change < sample_time_) {
return last_output_;
}
double error = setpoint - input;
printf("error: %f\n", error);
err_item_sum_ += ki_ * error;
SetLimits(err_item_sum_);
double derivative = input - last_input_;
double output = kp_ * error + err_item_sum_ - kd_ * derivative;
SetLimits(output);
last_input_ = input;
last_time_ = now;
last_output_ = output;
return output;
}
private:
double kp_;
double ki_;
double kd_;
double last_input_ = 0.0;
double last_output_ = 0.0;
double err_item_sum_ = 0.0;
double out_min_ = 0.0;
double out_max_ = 0.0;
uint64_t last_time_ = 0UL;
uint64_t sample_time_ = 1000UL; // 1 second
// PID 內(nèi)部狀態(tài)控制量：false 表示手動模式，true 表示自動模式
bool in_auto_ = false;
uint64_t GetMillis() {
return std::chrono::duration_cast(
std::chrono::steady_clock::now().time_since_epoch())
.count();
}
void SetLimits(double& val) {
if (val > out_max_) {
printf("val: %f > out_max_: %f\n", val, out_max_);
val = out_max_;
} else if (val < out_min_) {
val = out_min_;
} else {
; // Do nothing
}
}
};
int main() {
PIDController pid;
pid.set_tunings(1, 0.2, 0.02);
pid.set_sample_time(1000);
pid.set_output_limits(0, 100);
double setpoint = 36.0;
double temperature = 20.0;
std::this_thread::sleep_for(std::chrono::seconds(1));
pid.set_auto_mode(PID_MODE_AUTOMATIC);
for (int i = 0; i < 1000; ++i) {
if (i == 200) {
pid.set_auto_mode(PID_MODE_MANUAL);
std::cout << "---->>> Switch to manual mode" << std::endl;
}
double control_signal = pid.Compute(setpoint, temperature);
if (i >= 200 && i < 250) {
control_signal = 3;
}
if (i >= 250 && i <= 300) {
control_signal = 4;
}
std::cout << "--> Control signal: " << control_signal << std::endl;
temperature += control_signal * 0.1;
temperature *= 0.99;
std::cout << "<-- Temperature: " << temperature << std::endl;
if (i == 300) {
pid.set_auto_mode(PID_MODE_AUTOMATIC, temperature, control_signal);
std::cout << "---->>> Switch back to automatic mode" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
return 0;
}