From 366932039891fc415902e37786036753960fb773 Mon Sep 17 00:00:00 2001 From: Patrick Collins Date: Wed, 30 Nov 2022 08:58:43 +1100 Subject: [PATCH] PID Climate - deadband and output sampling (#3254) --- esphome/components/pid/climate.py | 46 ++++++++ esphome/components/pid/pid_climate.cpp | 18 +++- esphome/components/pid/pid_climate.h | 50 ++++++--- esphome/components/pid/pid_controller.cpp | 124 ++++++++++++++++++++++ esphome/components/pid/pid_controller.h | 99 ++++++++--------- tests/test3.yaml | 12 +++ 6 files changed, 278 insertions(+), 71 deletions(-) create mode 100644 esphome/components/pid/pid_controller.cpp diff --git a/esphome/components/pid/climate.py b/esphome/components/pid/climate.py index ffc6392ec2..7cd414f912 100644 --- a/esphome/components/pid/climate.py +++ b/esphome/components/pid/climate.py @@ -18,6 +18,7 @@ CONF_DEFAULT_TARGET_TEMPERATURE = "default_target_temperature" CONF_KP = "kp" CONF_KI = "ki" +CONF_STARTING_INTEGRAL_TERM = "starting_integral_term" CONF_KD = "kd" CONF_CONTROL_PARAMETERS = "control_parameters" CONF_COOL_OUTPUT = "cool_output" @@ -27,6 +28,17 @@ CONF_POSITIVE_OUTPUT = "positive_output" CONF_NEGATIVE_OUTPUT = "negative_output" CONF_MIN_INTEGRAL = "min_integral" CONF_MAX_INTEGRAL = "max_integral" +CONF_OUTPUT_AVERAGING_SAMPLES = "output_averaging_samples" +CONF_DERIVATIVE_AVERAGING_SAMPLES = "derivative_averaging_samples" + +# Deadband parameters +CONF_DEADBAND_PARAMETERS = "deadband_parameters" +CONF_THRESHOLD_HIGH = "threshold_high" +CONF_THRESHOLD_LOW = "threshold_low" +CONF_DEADBAND_OUTPUT_AVERAGING_SAMPLES = "deadband_output_averaging_samples" +CONF_KP_MULTIPLIER = "kp_multiplier" +CONF_KI_MULTIPLIER = "ki_multiplier" +CONF_KD_MULTIPLIER = "kd_multiplier" CONFIG_SCHEMA = cv.All( climate.CLIMATE_SCHEMA.extend( @@ -36,13 +48,28 @@ CONFIG_SCHEMA = cv.All( cv.Required(CONF_DEFAULT_TARGET_TEMPERATURE): cv.temperature, cv.Optional(CONF_COOL_OUTPUT): cv.use_id(output.FloatOutput), cv.Optional(CONF_HEAT_OUTPUT): cv.use_id(output.FloatOutput), + cv.Optional(CONF_DEADBAND_PARAMETERS): cv.Schema( + { + cv.Required(CONF_THRESHOLD_HIGH): cv.temperature, + cv.Required(CONF_THRESHOLD_LOW): cv.temperature, + cv.Optional(CONF_KP_MULTIPLIER, default=0.1): cv.float_, + cv.Optional(CONF_KI_MULTIPLIER, default=0.0): cv.float_, + cv.Optional(CONF_KD_MULTIPLIER, default=0.0): cv.float_, + cv.Optional( + CONF_DEADBAND_OUTPUT_AVERAGING_SAMPLES, default=1 + ): cv.int_, + } + ), cv.Required(CONF_CONTROL_PARAMETERS): cv.Schema( { cv.Required(CONF_KP): cv.float_, cv.Optional(CONF_KI, default=0.0): cv.float_, cv.Optional(CONF_KD, default=0.0): cv.float_, + cv.Optional(CONF_STARTING_INTEGRAL_TERM, default=0.0): cv.float_, cv.Optional(CONF_MIN_INTEGRAL, default=-1): cv.float_, cv.Optional(CONF_MAX_INTEGRAL, default=1): cv.float_, + cv.Optional(CONF_DERIVATIVE_AVERAGING_SAMPLES, default=1): cv.int_, + cv.Optional(CONF_OUTPUT_AVERAGING_SAMPLES, default=1): cv.int_, } ), } @@ -69,11 +96,29 @@ async def to_code(config): cg.add(var.set_kp(params[CONF_KP])) cg.add(var.set_ki(params[CONF_KI])) cg.add(var.set_kd(params[CONF_KD])) + cg.add(var.set_starting_integral_term(params[CONF_STARTING_INTEGRAL_TERM])) + cg.add(var.set_derivative_samples(params[CONF_DERIVATIVE_AVERAGING_SAMPLES])) + + cg.add(var.set_output_samples(params[CONF_OUTPUT_AVERAGING_SAMPLES])) + if CONF_MIN_INTEGRAL in params: cg.add(var.set_min_integral(params[CONF_MIN_INTEGRAL])) if CONF_MAX_INTEGRAL in params: cg.add(var.set_max_integral(params[CONF_MAX_INTEGRAL])) + if CONF_DEADBAND_PARAMETERS in config: + params = config[CONF_DEADBAND_PARAMETERS] + cg.add(var.set_threshold_low(params[CONF_THRESHOLD_LOW])) + cg.add(var.set_threshold_high(params[CONF_THRESHOLD_HIGH])) + cg.add(var.set_kp_multiplier(params[CONF_KP_MULTIPLIER])) + cg.add(var.set_ki_multiplier(params[CONF_KI_MULTIPLIER])) + cg.add(var.set_kd_multiplier(params[CONF_KD_MULTIPLIER])) + cg.add( + var.set_deadband_output_samples( + params[CONF_DEADBAND_OUTPUT_AVERAGING_SAMPLES] + ) + ) + cg.add(var.set_default_target_temperature(config[CONF_DEFAULT_TARGET_TEMPERATURE])) @@ -140,4 +185,5 @@ async def set_control_parameters(config, action_id, template_arg, args): kd_template_ = await cg.templatable(config[CONF_KD], args, float) cg.add(var.set_kd(kd_template_)) + return var diff --git a/esphome/components/pid/pid_climate.cpp b/esphome/components/pid/pid_climate.cpp index 3f8377c720..81c3e1f12e 100644 --- a/esphome/components/pid/pid_climate.cpp +++ b/esphome/components/pid/pid_climate.cpp @@ -61,7 +61,17 @@ climate::ClimateTraits PIDClimate::traits() { void PIDClimate::dump_config() { LOG_CLIMATE("", "PID Climate", this); ESP_LOGCONFIG(TAG, " Control Parameters:"); - ESP_LOGCONFIG(TAG, " kp: %.5f, ki: %.5f, kd: %.5f", controller_.kp, controller_.ki, controller_.kd); + ESP_LOGCONFIG(TAG, " kp: %.5f, ki: %.5f, kd: %.5f, output samples: %d", controller_.kp_, controller_.ki_, + controller_.kd_, controller_.output_samples_); + + if (controller_.threshold_low_ == 0 && controller_.threshold_high_ == 0) { + ESP_LOGCONFIG(TAG, " Deadband disabled."); + } else { + ESP_LOGCONFIG(TAG, " Deadband Parameters:"); + ESP_LOGCONFIG(TAG, " threshold: %0.5f to %0.5f, multipliers(kp: %.5f, ki: %.5f, kd: %.5f), output samples: %d", + controller_.threshold_low_, controller_.threshold_high_, controller_.kp_multiplier_, + controller_.ki_multiplier_, controller_.kd_multiplier_, controller_.deadband_output_samples_); + } if (this->autotuner_ != nullptr) { this->autotuner_->dump_config(); @@ -114,9 +124,9 @@ void PIDClimate::update_pid_() { if (this->autotuner_ != nullptr && !this->autotuner_->is_finished()) { auto res = this->autotuner_->update(this->target_temperature, this->current_temperature); if (res.result_params.has_value()) { - this->controller_.kp = res.result_params->kp; - this->controller_.ki = res.result_params->ki; - this->controller_.kd = res.result_params->kd; + this->controller_.kp_ = res.result_params->kp; + this->controller_.ki_ = res.result_params->ki; + this->controller_.kd_ = res.result_params->kd; // keep autotuner instance so that subsequent dump_configs will print the long result message. } else { value = res.output; diff --git a/esphome/components/pid/pid_climate.h b/esphome/components/pid/pid_climate.h index 095c00eb49..da57209a7e 100644 --- a/esphome/components/pid/pid_climate.h +++ b/esphome/components/pid/pid_climate.h @@ -21,20 +21,46 @@ class PIDClimate : public climate::Climate, public Component { void set_sensor(sensor::Sensor *sensor) { sensor_ = sensor; } void set_cool_output(output::FloatOutput *cool_output) { cool_output_ = cool_output; } void set_heat_output(output::FloatOutput *heat_output) { heat_output_ = heat_output; } - void set_kp(float kp) { controller_.kp = kp; } - void set_ki(float ki) { controller_.ki = ki; } - void set_kd(float kd) { controller_.kd = kd; } - void set_min_integral(float min_integral) { controller_.min_integral = min_integral; } - void set_max_integral(float max_integral) { controller_.max_integral = max_integral; } + void set_kp(float kp) { controller_.kp_ = kp; } + void set_ki(float ki) { controller_.ki_ = ki; } + void set_kd(float kd) { controller_.kd_ = kd; } + void set_min_integral(float min_integral) { controller_.min_integral_ = min_integral; } + void set_max_integral(float max_integral) { controller_.max_integral_ = max_integral; } + void set_output_samples(int in) { controller_.output_samples_ = in; } + void set_derivative_samples(int in) { controller_.derivative_samples_ = in; } + + void set_threshold_low(float in) { controller_.threshold_low_ = in; } + void set_threshold_high(float in) { controller_.threshold_high_ = in; } + void set_kp_multiplier(float in) { controller_.kp_multiplier_ = in; } + void set_ki_multiplier(float in) { controller_.ki_multiplier_ = in; } + void set_kd_multiplier(float in) { controller_.kd_multiplier_ = in; } + void set_starting_integral_term(float in) { controller_.set_starting_integral_term(in); } + + void set_deadband_output_samples(int in) { controller_.deadband_output_samples_ = in; } float get_output_value() const { return output_value_; } - float get_error_value() const { return controller_.error; } - float get_kp() { return controller_.kp; } - float get_ki() { return controller_.ki; } - float get_kd() { return controller_.kd; } - float get_proportional_term() const { return controller_.proportional_term; } - float get_integral_term() const { return controller_.integral_term; } - float get_derivative_term() const { return controller_.derivative_term; } + float get_error_value() const { return controller_.error_; } + float get_kp() { return controller_.kp_; } + float get_ki() { return controller_.ki_; } + float get_kd() { return controller_.kd_; } + float get_proportional_term() const { return controller_.proportional_term_; } + float get_integral_term() const { return controller_.integral_term_; } + float get_derivative_term() const { return controller_.derivative_term_; } + int get_output_samples() { return controller_.output_samples_; } + int get_derivative_samples() { return controller_.derivative_samples_; } + + float get_threshold_low() { return controller_.threshold_low_; } + float get_threshold_high() { return controller_.threshold_high_; } + float get_kp_multiplier() { return controller_.kp_multiplier_; } + float get_ki_multiplier() { return controller_.ki_multiplier_; } + float get_kd_multiplier() { return controller_.kd_multiplier_; } + int get_deadband_output_samples() { return controller_.deadband_output_samples_; } + bool in_deadband() { return controller_.in_deadband(); } + + // int get_derivative_samples() const { return controller_.derivative_samples; } + // float get_deadband() const { return controller_.deadband; } + // float get_proportional_deadband_multiplier() const { return controller_.proportional_deadband_multiplier; } + void add_on_pid_computed_callback(std::function &&callback) { pid_computed_callback_.add(std::move(callback)); } diff --git a/esphome/components/pid/pid_controller.cpp b/esphome/components/pid/pid_controller.cpp new file mode 100644 index 0000000000..afc2d91000 --- /dev/null +++ b/esphome/components/pid/pid_controller.cpp @@ -0,0 +1,124 @@ +#include "pid_controller.h" + +namespace esphome { +namespace pid { + +float PIDController::update(float setpoint, float process_value) { + // e(t) ... error at timestamp t + // r(t) ... setpoint + // y(t) ... process value (sensor reading) + // u(t) ... output value + + dt_ = calculate_relative_time_(); + + // e(t) := r(t) - y(t) + error_ = setpoint - process_value; + + calculate_proportional_term_(); + calculate_integral_term_(); + calculate_derivative_term_(); + + // u(t) := p(t) + i(t) + d(t) + float output = proportional_term_ + integral_term_ + derivative_term_; + + // smooth/sample the output + int samples = in_deadband() ? deadband_output_samples_ : output_samples_; + return weighted_average_(output_list_, output, samples); +} + +bool PIDController::in_deadband() { + // return (fabs(error) < deadband_threshold); + float err = -error_; + return ((err > 0 && err < threshold_high_) || (err < 0 && err > threshold_low_)); +} + +void PIDController::calculate_proportional_term_() { + // p(t) := K_p * e(t) + proportional_term_ = kp_ * error_; + + // set dead-zone to -X to +X + if (in_deadband()) { + // shallow the proportional_term in the deadband by the pdm + proportional_term_ *= kp_multiplier_; + + } else { + // pdm_offset prevents a jump when leaving the deadband + float threshold = (error_ < 0) ? threshold_high_ : threshold_low_; + float pdm_offset = (threshold - (kp_multiplier_ * threshold)) * kp_; + proportional_term_ += pdm_offset; + } +} + +void PIDController::calculate_integral_term_() { + // i(t) := K_i * \int_{0}^{t} e(t) dt + float new_integral = error_ * dt_ * ki_; + + if (in_deadband()) { + // shallow the integral when in the deadband + accumulated_integral_ += new_integral * ki_multiplier_; + } else { + accumulated_integral_ += new_integral; + } + + // constrain accumulated integral value + if (!std::isnan(min_integral_) && accumulated_integral_ < min_integral_) + accumulated_integral_ = min_integral_; + if (!std::isnan(max_integral_) && accumulated_integral_ > max_integral_) + accumulated_integral_ = max_integral_; + + integral_term_ = accumulated_integral_; +} + +void PIDController::calculate_derivative_term_() { + // derivative_term_ + // d(t) := K_d * de(t)/dt + float derivative = 0.0f; + if (dt_ != 0.0f) + derivative = (error_ - previous_error_) / dt_; + previous_error_ = error_; + + // smooth the derivative samples + derivative = weighted_average_(derivative_list_, derivative, derivative_samples_); + + derivative_term_ = kd_ * derivative; + + if (in_deadband()) { + // shallow the derivative when in the deadband + derivative_term_ *= kd_multiplier_; + } +} + +float PIDController::weighted_average_(std::deque &list, float new_value, int samples) { + // if only 1 sample needed, clear the list and return + if (samples == 1) { + list.clear(); + return new_value; + } + + // add the new item to the list + list.push_front(new_value); + + // keep only 'samples' readings, by popping off the back of the list + while (list.size() > samples) + list.pop_back(); + + // calculate and return the average of all values in the list + float sum = 0; + for (auto &elem : list) + sum += elem; + return sum / list.size(); +} + +float PIDController::calculate_relative_time_() { + uint32_t now = millis(); + uint32_t dt = now - this->last_time_; + if (last_time_ == 0) { + last_time_ = now; + return 0.0f; + } + last_time_ = now; + return dt / 1000.0f; +} + +} // namespace pid +} // namespace esphome diff --git a/esphome/components/pid/pid_controller.h b/esphome/components/pid/pid_controller.h index 35e3eb9fc0..05ce5f9224 100644 --- a/esphome/components/pid/pid_controller.h +++ b/esphome/components/pid/pid_controller.h @@ -1,81 +1,70 @@ #pragma once - #include "esphome/core/hal.h" +#include +#include namespace esphome { namespace pid { struct PIDController { - float update(float setpoint, float process_value) { - // e(t) ... error at timestamp t - // r(t) ... setpoint - // y(t) ... process value (sensor reading) - // u(t) ... output value - - float dt = calculate_relative_time_(); - - // e(t) := r(t) - y(t) - error = setpoint - process_value; - - // p(t) := K_p * e(t) - proportional_term = kp * error; - - // i(t) := K_i * \int_{0}^{t} e(t) dt - accumulated_integral_ += error * dt * ki; - // constrain accumulated integral value - if (!std::isnan(min_integral) && accumulated_integral_ < min_integral) - accumulated_integral_ = min_integral; - if (!std::isnan(max_integral) && accumulated_integral_ > max_integral) - accumulated_integral_ = max_integral; - integral_term = accumulated_integral_; - - // d(t) := K_d * de(t)/dt - float derivative = 0.0f; - if (dt != 0.0f) - derivative = (error - previous_error_) / dt; - previous_error_ = error; - derivative_term = kd * derivative; - - // u(t) := p(t) + i(t) + d(t) - return proportional_term + integral_term + derivative_term; - } + float update(float setpoint, float process_value); void reset_accumulated_integral() { accumulated_integral_ = 0; } + void set_starting_integral_term(float in) { accumulated_integral_ = in; } + bool in_deadband(); + + friend class PIDClimate; + + private: /// Proportional gain K_p. - float kp = 0; + float kp_ = 0; /// Integral gain K_i. - float ki = 0; + float ki_ = 0; /// Differential gain K_d. - float kd = 0; + float kd_ = 0; - float min_integral = NAN; - float max_integral = NAN; + // smooth the derivative value using a weighted average over X samples + int derivative_samples_ = 8; + + /// smooth the output value using a weighted average over X values + int output_samples_ = 1; + + float threshold_low_ = 0.0f; + float threshold_high_ = 0.0f; + float kp_multiplier_ = 0.0f; + float ki_multiplier_ = 0.0f; + float kd_multiplier_ = 0.0f; + int deadband_output_samples_ = 1; + + float min_integral_ = NAN; + float max_integral_ = NAN; // Store computed values in struct so that values can be monitored through sensors - float error; - float proportional_term; - float integral_term; - float derivative_term; + float error_; + float dt_; + float proportional_term_; + float integral_term_; + float derivative_term_; - protected: - float calculate_relative_time_() { - uint32_t now = millis(); - uint32_t dt = now - this->last_time_; - if (last_time_ == 0) { - last_time_ = now; - return 0.0f; - } - last_time_ = now; - return dt / 1000.0f; - } + void calculate_proportional_term_(); + void calculate_integral_term_(); + void calculate_derivative_term_(); + float weighted_average_(std::deque &list, float new_value, int samples); + float calculate_relative_time_(); /// Error from previous update used for derivative term float previous_error_ = 0; /// Accumulated integral value float accumulated_integral_ = 0; uint32_t last_time_ = 0; -}; + // this is a list of derivative values for smoothing. + std::deque derivative_list_; + + // this is a list of output values for smoothing. + std::deque output_list_; + +}; // Struct PID Controller } // namespace pid } // namespace esphome diff --git a/tests/test3.yaml b/tests/test3.yaml index 546df1efb5..9b4e530fbd 100644 --- a/tests/test3.yaml +++ b/tests/test3.yaml @@ -1162,6 +1162,18 @@ climate: kp: 0.0 ki: 0.0 kd: 0.0 + max_integral: 0.0 + output_averaging_samples: 1 + derivative_averaging_samples: 1 + deadband_parameters: + threshold_high: 0.4 + threshold_low: -2.0 + kp_multiplier: 0.0 + ki_multiplier: 0.0 + kd_multiplier: 0.0 + deadband_output_averaging_samples: 1 + + sprinkler: - id: yard_sprinkler_ctrlr