3d_print_resources/Marlin
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

style and corrections, just cuz

Browse source
This commit is contained in:
Scott Lahteine 2018-10-05 21:24:23 -05:00 • committed by GitHub
parent ad3edcad03
commit f21a49271c
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 73 additions and 82 deletions
Download patch file Download diff file Expand all files Collapse all files

155
Marlin/src/libs/pid.h
View file

@ -3,100 +3,91 @@
#include <type_traits> #include <type_traits>
#include <cmath> #include <cmath>
struct PIDConfig { struct PIDConfig {
static constexpr float _kp = 0; static constexpr float _kp = 0;
static constexpr float _ki = 0; static constexpr float _ki = 0;
static constexpr float _kd = 0; static constexpr float _kd = 0;
static constexpr float _fCut = 20; static constexpr float _fCut = 20;
static constexpr int16_t _imax = 0; static constexpr int16_t _imax = 0;
typedef float pid_t; typedef float pid_t;
}; };
template <class Config> template <class PIDConfig>
class PID { class PID {
public: public:
PID() { PID() {
static_assert(Config::_kp > 0, "_kp smaller 0"); static_assert(PIDConfig::_kp > 0, "_kp <= 0");
static_assert(Config::_ki > 0, "_ki smaller 0"); static_assert(PIDConfig::_ki > 0, "_ki <= 0");
static_assert(Config::_kd > 0, "_kd smaller 0"); static_assert(PIDConfig::_kd > 0, "_kd <= 0");
static_assert(Config::_imax > 0, "_imax smaller 0"); static_assert(PIDConfig::_imax > 0, "_imax <= 0");
// set _last_derivative as invalid when we startup // set _last_derivative as invalid on startup
_last_derivative = NAN; _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) { // scale the P and D components
uint32_t dt = tnow - _last_t; output *= scaler;
typename Config::pid_t output = 0;
typename Config::pid_t delta_time;
if (_last_t == 0 || dt > 1000) { // Compute integral component if time has elapsed
dt = 0; if (dt > 0) {
reset_I(); _integrator += (error * PIDConfig::_ki) * scaler * delta_time;
} if (_integrator < -PIDConfig::_imax) _integrator = -PIDConfig::_imax;
_last_t = tnow; if (_integrator > +PIDConfig::_imax) _integrator = +PIDConfig::_imax;
delta_time = (typename Config::pid_t)dt / 1000.0f; output += _integrator;
// 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(); return output;
void reset_I(); }
typename Config::pid_t get_integrator() const { void reset();
return _integrator; void reset_I();
}
typename PIDConfig::pid_t get_integrator() const {
return _integrator;
}
private: private:
typename Config::pid_t _integrator;///< integrator value typename PIDConfig::pid_t _integrator; ///< integrator value
typename Config::pid_t _last_error;///< last error for derivative typename PIDConfig::pid_t _last_error; ///< last error for derivative
typename Config::pid_t _last_derivative;///< last derivative for low-pass filter typename PIDConfig::pid_t _last_derivative; ///< last derivative for low-pass filter
uint32_t _last_t;///< last time get_pid() was called in millis uint32_t _last_t; ///< last time get_pid() was called in millis
}; };