87 lines
1.9 KiB
C++
87 lines
1.9 KiB
C++
#include "PidController.h"
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
PidController::PidController(float kp, float ki, float kd)
|
|
: kp_(kp), ki_(ki), kd_(kd), last_error_(0.0f), integral_(0.0f), output_(0.0f),
|
|
output_min_(-100.0f), output_max_(100.0f), first_iteration_(true) {}
|
|
|
|
float PidController::update(float setpoint, float measured_value, float dt) {
|
|
float error = setpoint - measured_value;
|
|
|
|
// 处理积分饱和
|
|
integral_ += error * dt;
|
|
|
|
// 计算微分（使用前向差分）
|
|
float derivative = 0.0f;
|
|
if (!first_iteration_) {
|
|
derivative = (error - last_error_) / dt;
|
|
} else {
|
|
first_iteration_ = false;
|
|
}
|
|
|
|
// PID计算
|
|
float proportional = kp_ * error;
|
|
float integral_contribution = ki_ * integral_;
|
|
float derivative_contribution = kd_ * derivative;
|
|
|
|
output_ = proportional + integral_contribution + derivative_contribution;
|
|
|
|
// 限制输出
|
|
output_ = std::max(output_min_, std::min(output_max_, output_));
|
|
|
|
// 保存当前误差用于下次计算微分
|
|
last_error_ = error;
|
|
|
|
return output_;
|
|
}
|
|
|
|
void PidController::reset() {
|
|
last_error_ = 0.0f;
|
|
integral_ = 0.0f;
|
|
output_ = 0.0f;
|
|
first_iteration_ = true;
|
|
}
|
|
|
|
void PidController::setKp(float kp) {
|
|
kp_ = kp;
|
|
}
|
|
|
|
void PidController::setKi(float ki) {
|
|
ki_ = ki;
|
|
}
|
|
|
|
void PidController::setKd(float kd) {
|
|
kd_ = kd;
|
|
}
|
|
|
|
void PidController::setParameters(float kp, float ki, float kd) {
|
|
kp_ = kp;
|
|
ki_ = ki;
|
|
kd_ = kd;
|
|
}
|
|
|
|
float PidController::getKp() const {
|
|
return kp_;
|
|
}
|
|
|
|
float PidController::getKi() const {
|
|
return ki_;
|
|
}
|
|
|
|
float PidController::getKd() const {
|
|
return kd_;
|
|
}
|
|
|
|
void PidController::setOutputLimits(float min, float max) {
|
|
output_min_ = min;
|
|
output_max_ = max;
|
|
}
|
|
|
|
float PidController::getOutput() const {
|
|
return output_;
|
|
}
|
|
|
|
float PidController::getError() const {
|
|
return last_error_;
|
|
} |