12 dt_ = calculate_relative_time_();
15 error_ = setpoint - process_value;
17 calculate_proportional_term_();
18 calculate_integral_term_();
19 calculate_derivative_term_(setpoint);
22 float output = proportional_term_ + integral_term_ + derivative_term_;
25 int samples =
in_deadband() ? deadband_output_samples_ : output_samples_;
26 return weighted_average_(output_list_, output, samples);
32 return (threshold_low_ < err && err < threshold_high_);
35 void PIDController::calculate_proportional_term_() {
37 proportional_term_ = kp_ * error_;
42 proportional_term_ *= kp_multiplier_;
46 float threshold = (error_ < 0) ? threshold_high_ : threshold_low_;
47 float pdm_offset = (threshold - (kp_multiplier_ * threshold)) * kp_;
48 proportional_term_ += pdm_offset;
52 void PIDController::calculate_integral_term_() {
54 float new_integral = error_ * dt_ * ki_;
58 accumulated_integral_ += new_integral * ki_multiplier_;
60 accumulated_integral_ += new_integral;
64 if (!std::isnan(min_integral_) && accumulated_integral_ < min_integral_)
65 accumulated_integral_ = min_integral_;
66 if (!std::isnan(max_integral_) && accumulated_integral_ > max_integral_)
67 accumulated_integral_ = max_integral_;
69 integral_term_ = accumulated_integral_;
72 void PIDController::calculate_derivative_term_(
float setpoint) {
75 float derivative = 0.0f;
78 if (!std::isnan(previous_setpoint_) && previous_setpoint_ != setpoint)
79 previous_error_ -= previous_setpoint_ - setpoint;
80 derivative = (error_ - previous_error_) / dt_;
82 previous_error_ = error_;
83 previous_setpoint_ = setpoint;
86 derivative = weighted_average_(derivative_list_, derivative, derivative_samples_);
88 derivative_term_ = kd_ * derivative;
92 derivative_term_ *= kd_multiplier_;
96 float PIDController::weighted_average_(std::deque<float> &list,
float new_value,
int samples) {
104 list.push_front(new_value);
107 while (list.size() > samples)
112 for (
auto &elem : list)
114 return sum / list.size();
117 float PIDController::calculate_relative_time_() {
119 uint32_t dt = now - this->last_time_;
120 if (last_time_ == 0) {
uint32_t IRAM_ATTR HOT millis()
float update(float setpoint, float process_value)
Implementation of SPI Controller mode.