ESPHome  2024.11.3
pid_controller.cpp
Go to the documentation of this file.
1 #include "pid_controller.h"
2 
3 namespace esphome {
4 namespace pid {
5 
6 float PIDController::update(float setpoint, float process_value) {
7  // e(t) ... error at timestamp t
8  // r(t) ... setpoint
9  // y(t) ... process value (sensor reading)
10  // u(t) ... output value
11 
12  dt_ = calculate_relative_time_();
13 
14  // e(t) := r(t) - y(t)
15  error_ = setpoint - process_value;
16 
17  calculate_proportional_term_();
18  calculate_integral_term_();
19  calculate_derivative_term_(setpoint);
20 
21  // u(t) := p(t) + i(t) + d(t)
22  float output = proportional_term_ + integral_term_ + derivative_term_;
23 
24  // smooth/sample the output
25  int samples = in_deadband() ? deadband_output_samples_ : output_samples_;
26  return weighted_average_(output_list_, output, samples);
27 }
28 
30  // return (fabs(error) < deadband_threshold);
31  float err = -error_;
32  return (threshold_low_ < err && err < threshold_high_);
33 }
34 
35 void PIDController::calculate_proportional_term_() {
36  // p(t) := K_p * e(t)
37  proportional_term_ = kp_ * error_;
38 
39  // set dead-zone to -X to +X
40  if (in_deadband()) {
41  // shallow the proportional_term in the deadband by the pdm
42  proportional_term_ *= kp_multiplier_;
43 
44  } else {
45  // pdm_offset prevents a jump when leaving the deadband
46  float threshold = (error_ < 0) ? threshold_high_ : threshold_low_;
47  float pdm_offset = (threshold - (kp_multiplier_ * threshold)) * kp_;
48  proportional_term_ += pdm_offset;
49  }
50 }
51 
52 void PIDController::calculate_integral_term_() {
53  // i(t) := K_i * \int_{0}^{t} e(t) dt
54  float new_integral = error_ * dt_ * ki_;
55 
56  if (in_deadband()) {
57  // shallow the integral when in the deadband
58  accumulated_integral_ += new_integral * ki_multiplier_;
59  } else {
60  accumulated_integral_ += new_integral;
61  }
62 
63  // constrain accumulated integral value
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_;
68 
69  integral_term_ = accumulated_integral_;
70 }
71 
72 void PIDController::calculate_derivative_term_(float setpoint) {
73  // derivative_term_
74  // d(t) := K_d * de(t)/dt
75  float derivative = 0.0f;
76  if (dt_ != 0.0f) {
77  // remove changes to setpoint from error
78  if (!std::isnan(previous_setpoint_) && previous_setpoint_ != setpoint)
79  previous_error_ -= previous_setpoint_ - setpoint;
80  derivative = (error_ - previous_error_) / dt_;
81  }
82  previous_error_ = error_;
83  previous_setpoint_ = setpoint;
84 
85  // smooth the derivative samples
86  derivative = weighted_average_(derivative_list_, derivative, derivative_samples_);
87 
88  derivative_term_ = kd_ * derivative;
89 
90  if (in_deadband()) {
91  // shallow the derivative when in the deadband
92  derivative_term_ *= kd_multiplier_;
93  }
94 }
95 
96 float PIDController::weighted_average_(std::deque<float> &list, float new_value, int samples) {
97  // if only 1 sample needed, clear the list and return
98  if (samples == 1) {
99  list.clear();
100  return new_value;
101  }
102 
103  // add the new item to the list
104  list.push_front(new_value);
105 
106  // keep only 'samples' readings, by popping off the back of the list
107  while (list.size() > samples)
108  list.pop_back();
109 
110  // calculate and return the average of all values in the list
111  float sum = 0;
112  for (auto &elem : list)
113  sum += elem;
114  return sum / list.size();
115 }
116 
117 float PIDController::calculate_relative_time_() {
118  uint32_t now = millis();
119  uint32_t dt = now - this->last_time_;
120  if (last_time_ == 0) {
121  last_time_ = now;
122  return 0.0f;
123  }
124  last_time_ = now;
125  return dt / 1000.0f;
126 }
127 
128 } // namespace pid
129 } // namespace esphome
uint32_t IRAM_ATTR HOT millis()
Definition: core.cpp:25
float update(float setpoint, float process_value)
Implementation of SPI Controller mode.
Definition: a01nyub.cpp:7