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

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

@ -3,7 +3,6 @@
#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;
@ -13,51 +12,47 @@ struct PIDConfig {
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 Config::pid_t get_pid(typename Config::pid_t error, uint32_t tnow, typename Config::pid_t scaler = 1.0) { 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; uint32_t dt = tnow - _last_t;
typename Config::pid_t output = 0; typename PIDConfig::pid_t output = 0;
typename Config::pid_t delta_time; typename PIDConfig::pid_t delta_time;
if (_last_t == 0 || dt > 1000) { if (_last_t == 0 || dt > 1000) {
dt = 0; dt = 0;
reset_I(); reset_I();
} }
_last_t = tnow; _last_t = tnow;
delta_time = (typename Config::pid_t)dt / 1000.0f; delta_time = (typename PIDConfig::pid_t)dt / 1000.0f;
// Compute proportional component // Compute proportional component
output += error * Config::_kp; output += error * PIDConfig::_kp;
// Compute derivative component if time has elapsed // Compute derivative component if time has elapsed
if (dt > 0) { if (dt > 0) {
typename Config::pid_t derivative; typename PIDConfig::pid_t derivative;
if (isnan(_last_derivative)) { if (isnan(_last_derivative))
derivative = 0; derivative = _last_derivative = 0;
_last_derivative = 0; else
}
else {
derivative = (error - _last_error) / delta_time; derivative = (error - _last_error) / delta_time;
}
// discrete low pass filter, cuts out the // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // high frequency noise that can drive the controller crazy
constexpr typename Config::pid_t RC = 1/(2*M_PI*Config::_fCut); constexpr typename PIDConfig::pid_t RC = 1/(2*M_PI*PIDConfig::_fCut);
derivative derivative = _last_derivative
= _last_derivative
+ ((delta_time / (RC + delta_time)) + ((delta_time / (RC + delta_time))
* (derivative - _last_derivative)); * (derivative - _last_derivative));
@ -66,7 +61,7 @@ public:
_last_derivative = derivative; _last_derivative = derivative;
// add in derivative component // add in derivative component
output += Config::pid_t * derivative; output += PIDConfig::pid_t * derivative;
} }
// scale the P and D components // scale the P and D components
@ -74,13 +69,9 @@ public:
// Compute integral component if time has elapsed // Compute integral component if time has elapsed
if (dt > 0) { if (dt > 0) {
_integrator += (error * Config::_ki) * scaler * delta_time; _integrator += (error * PIDConfig::_ki) * scaler * delta_time;
if (_integrator < -Config::_imax) { if (_integrator < -PIDConfig::_imax) _integrator = -PIDConfig::_imax;
_integrator = -Config::_imax; if (_integrator > +PIDConfig::_imax) _integrator = +PIDConfig::_imax;
}
if (_integrator > Config::_imax) {
_integrator = Config::_imax;
}
output += _integrator; output += _integrator;
} }
@ -90,13 +81,13 @@ public:
void reset(); void reset();
void reset_I(); void reset_I();
typename Config::pid_t get_integrator() const { typename PIDConfig::pid_t get_integrator() const {
return _integrator; 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
}; };