style and corrections, just cuz

Marlin/src/libs/pid.h

@@ -3,100 +3,91 @@
 #include <type_traits>
 #include <cmath>
 
-
 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 <class Config>
+template <class PIDConfig>
 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
 };