From ad3edcad0397d3f6e863024b1eee5d01ba01aaa1 Mon Sep 17 00:00:00 2001 From: dgrat Date: Thu, 4 Oct 2018 15:17:04 +0200 Subject: [PATCH 1/2] Added separate PID class --- Marlin/src/libs/pid.h | 102 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 102 insertions(+) create mode 100644 Marlin/src/libs/pid.h diff --git a/Marlin/src/libs/pid.h b/Marlin/src/libs/pid.h new file mode 100644 index 0000000000..b0f43830ff --- /dev/null +++ b/Marlin/src/libs/pid.h @@ -0,0 +1,102 @@ +#pragma once + +#include +#include + + +struct PIDConfig { + static constexpr float _kp = 0; + static constexpr float _ki = 0; + static constexpr float _kd = 0; + static constexpr float _fCut = 20; + static constexpr int16_t _imax = 0; + typedef float pid_t; +}; + +template +class PID { +public: + PID() { + static_assert(Config::_kp > 0, "_kp smaller 0"); + static_assert(Config::_ki > 0, "_ki smaller 0"); + static_assert(Config::_kd > 0, "_kd smaller 0"); + static_assert(Config::_imax > 0, "_imax smaller 0"); + + // set _last_derivative as invalid when we startup + _last_derivative = NAN; + } + + typename Config::pid_t get_pid(typename Config::pid_t error, uint32_t tnow, typename Config::pid_t scaler = 1.0) { + uint32_t dt = tnow - _last_t; + typename Config::pid_t output = 0; + typename Config::pid_t delta_time; + + if (_last_t == 0 || dt > 1000) { + dt = 0; + reset_I(); + } + _last_t = tnow; + delta_time = (typename Config::pid_t)dt / 1000.0f; + + // Compute proportional component + output += error * Config::_kp; + + // Compute derivative component if time has elapsed + if (dt > 0) { + typename Config::pid_t derivative; + + if (isnan(_last_derivative)) { + derivative = 0; + _last_derivative = 0; + } + else { + derivative = (error - _last_error) / delta_time; + } + + // discrete low pass filter, cuts out the + // high frequency noise that can drive the controller crazy + constexpr typename Config::pid_t RC = 1/(2*M_PI*Config::_fCut); + derivative + = _last_derivative + + ((delta_time / (RC + delta_time)) + * (derivative - _last_derivative)); + + // update state + _last_error = error; + _last_derivative = derivative; + + // add in derivative component + output += Config::pid_t * derivative; + } + + // scale the P and D components + output *= scaler; + + // Compute integral component if time has elapsed + if (dt > 0) { + _integrator += (error * Config::_ki) * scaler * delta_time; + if (_integrator < -Config::_imax) { + _integrator = -Config::_imax; + } + if (_integrator > Config::_imax) { + _integrator = Config::_imax; + } + output += _integrator; + } + + return output; + } + + void reset(); + void reset_I(); + + typename Config::pid_t get_integrator() const { + return _integrator; + } + +private: + typename Config::pid_t _integrator;///< integrator value + typename Config::pid_t _last_error;///< last error for derivative + typename Config::pid_t _last_derivative;///< last derivative for low-pass filter + uint32_t _last_t;///< last time get_pid() was called in millis +}; From f21a49271cb9696f0f1536191d1139ce42b439fb Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 5 Oct 2018 21:24:23 -0500 Subject: [PATCH 2/2] style and corrections, just cuz --- Marlin/src/libs/pid.h | 155 ++++++++++++++++++++---------------------- 1 file changed, 73 insertions(+), 82 deletions(-) diff --git a/Marlin/src/libs/pid.h b/Marlin/src/libs/pid.h index b0f43830ff..155732b288 100644 --- a/Marlin/src/libs/pid.h +++ b/Marlin/src/libs/pid.h @@ -3,100 +3,91 @@ #include #include - struct PIDConfig { - static constexpr float _kp = 0; - static constexpr float _ki = 0; - static constexpr float _kd = 0; - static constexpr float _fCut = 20; - static constexpr int16_t _imax = 0; - typedef float pid_t; + static constexpr float _kp = 0; + static constexpr float _ki = 0; + static constexpr float _kd = 0; + static constexpr float _fCut = 20; + static constexpr int16_t _imax = 0; + typedef float pid_t; }; -template +template class PID { public: - PID() { - static_assert(Config::_kp > 0, "_kp smaller 0"); - static_assert(Config::_ki > 0, "_ki smaller 0"); - static_assert(Config::_kd > 0, "_kd smaller 0"); - static_assert(Config::_imax > 0, "_imax smaller 0"); + PID() { + static_assert(PIDConfig::_kp > 0, "_kp <= 0"); + static_assert(PIDConfig::_ki > 0, "_ki <= 0"); + static_assert(PIDConfig::_kd > 0, "_kd <= 0"); + static_assert(PIDConfig::_imax > 0, "_imax <= 0"); - // set _last_derivative as invalid when we startup - _last_derivative = NAN; + // set _last_derivative as invalid on startup + _last_derivative = NAN; + } + + typename PIDConfig::pid_t get_pid(typename PIDConfig::pid_t error, uint32_t tnow, typename PIDConfig::pid_t scaler=1.0) { + uint32_t dt = tnow - _last_t; + typename PIDConfig::pid_t output = 0; + typename PIDConfig::pid_t delta_time; + + if (_last_t == 0 || dt > 1000) { + dt = 0; + reset_I(); + } + _last_t = tnow; + delta_time = (typename PIDConfig::pid_t)dt / 1000.0f; + + // Compute proportional component + output += error * PIDConfig::_kp; + + // Compute derivative component if time has elapsed + if (dt > 0) { + typename PIDConfig::pid_t derivative; + + if (isnan(_last_derivative)) + derivative = _last_derivative = 0; + else + derivative = (error - _last_error) / delta_time; + + // discrete low pass filter, cuts out the + // high frequency noise that can drive the controller crazy + constexpr typename PIDConfig::pid_t RC = 1/(2*M_PI*PIDConfig::_fCut); + derivative = _last_derivative + + ((delta_time / (RC + delta_time)) + * (derivative - _last_derivative)); + + // update state + _last_error = error; + _last_derivative = derivative; + + // add in derivative component + output += PIDConfig::pid_t * derivative; } - typename Config::pid_t get_pid(typename Config::pid_t error, uint32_t tnow, typename Config::pid_t scaler = 1.0) { - uint32_t dt = tnow - _last_t; - typename Config::pid_t output = 0; - typename Config::pid_t delta_time; + // scale the P and D components + output *= scaler; - if (_last_t == 0 || dt > 1000) { - dt = 0; - reset_I(); - } - _last_t = tnow; - delta_time = (typename Config::pid_t)dt / 1000.0f; - - // Compute proportional component - output += error * Config::_kp; - - // Compute derivative component if time has elapsed - if (dt > 0) { - typename Config::pid_t derivative; - - if (isnan(_last_derivative)) { - derivative = 0; - _last_derivative = 0; - } - else { - derivative = (error - _last_error) / delta_time; - } - - // discrete low pass filter, cuts out the - // high frequency noise that can drive the controller crazy - constexpr typename Config::pid_t RC = 1/(2*M_PI*Config::_fCut); - derivative - = _last_derivative - + ((delta_time / (RC + delta_time)) - * (derivative - _last_derivative)); - - // update state - _last_error = error; - _last_derivative = derivative; - - // add in derivative component - output += Config::pid_t * derivative; - } - - // scale the P and D components - output *= scaler; - - // Compute integral component if time has elapsed - if (dt > 0) { - _integrator += (error * Config::_ki) * scaler * delta_time; - if (_integrator < -Config::_imax) { - _integrator = -Config::_imax; - } - if (_integrator > Config::_imax) { - _integrator = Config::_imax; - } - output += _integrator; - } - - return output; + // Compute integral component if time has elapsed + if (dt > 0) { + _integrator += (error * PIDConfig::_ki) * scaler * delta_time; + if (_integrator < -PIDConfig::_imax) _integrator = -PIDConfig::_imax; + if (_integrator > +PIDConfig::_imax) _integrator = +PIDConfig::_imax; + output += _integrator; } - void reset(); - void reset_I(); + return output; + } - typename Config::pid_t get_integrator() const { - return _integrator; - } + void reset(); + void reset_I(); + + typename PIDConfig::pid_t get_integrator() const { + return _integrator; + } private: - typename Config::pid_t _integrator;///< integrator value - typename Config::pid_t _last_error;///< last error for derivative - typename Config::pid_t _last_derivative;///< last derivative for low-pass filter - uint32_t _last_t;///< last time get_pid() was called in millis + typename PIDConfig::pid_t _integrator; ///< integrator value + typename PIDConfig::pid_t _last_error; ///< last error for derivative + typename PIDConfig::pid_t _last_derivative; ///< last derivative for low-pass filter + uint32_t _last_t; ///< last time get_pid() was called in millis };