Improve the PID controller

libs/indibase/pid/pid.cpp

@@ -1,4 +1,6 @@
 /**
+ * Copyright 2021 Jasem Mutlaq <mutlaqja@ikarustech.com>
+ * Copyright (c) 2020 Philip Salmony
  * Copyright 2019 Bradley J. Snyder <snyder.bradleyj@gmail.com>
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -20,9 +22,6 @@
  * THE SOFTWARE.
  */
 
-#ifndef _PID_SOURCE_
-#define _PID_SOURCE_
-
 #include <iostream>
 #include <cmath>
 #include "pid.h"
@@ -34,28 +33,88 @@ class PIDImpl
     public:
         PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki );
         ~PIDImpl();
-        double calculate( double setpoint, double pv );
+        void setIntegratorLimits(double min, double max)
+        {
+            m_IntegratorMin = min;
+            m_IntegratorMax = max;
+        }
+        void setTau(double value)
+        {
+            m_Tau = value;
+        }
+        double calculate( double setpoint, double measurement );
+        double propotionalTerm() const
+        {
+            return m_PropotionalTerm;
+        }
+        double integralTerm() const
+        {
+            return m_IntegralTerm;
+        }
+        double derivativeTerm() const
+        {
+            return m_DerivativeTerm;
+        }
 
     private:
-        double _dt;
-        double _max;
-        double _min;
-        double _Kp;
-        double _Kd;
-        double _Ki;
-        double _pre_error;
-        double _integral;
+        // Sample Time
+        double m_T {1};
+        // Derivative Low-Pass filter time constant */
+        double m_Tau {2};
+
+        // Output limits
+        double m_Max {0};
+        double m_Min {0};
+
+        // Integrator Limits
+        double m_IntegratorMin {0};
+        double m_IntegratorMax {0};
+
+        // Gains
+        double m_Kp {0};
+        double m_Kd {0};
+        double m_Ki {0};
+
+        // Controller volatile data
+        double m_PreviousError {0};
+        double m_PreviousMeasurement {0};
+
+        // Terms
+        double m_PropotionalTerm {0};
+        double m_IntegralTerm {0};
+        double m_DerivativeTerm {0};
+
 };
 
 
 PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki )
 {
     pimpl = new PIDImpl(dt, max, min, Kp, Kd, Ki);
 }
+void PID::setIntegratorLimits(double min, double max)
+{
+    pimpl->setIntegratorLimits(min, max);
+}
+void PID::setTau(double value)
+{
+    pimpl->setTau(value);
+}
 double PID::calculate( double setpoint, double pv )
 {
     return pimpl->calculate(setpoint, pv);
 }
+double PID::propotionalTerm() const
+{
+    return pimpl->propotionalTerm();
+}
+double PID::integralTerm() const
+{
+    return pimpl->integralTerm();
+}
+double PID::derivativeTerm() const
+{
+    return pimpl->derivativeTerm();
+}
 PID::~PID()
 {
     delete pimpl;
@@ -66,45 +125,43 @@ PID::~PID()
  * Implementation
  */
 PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) :
-    _dt(dt),
-    _max(max),
-    _min(min),
-    _Kp(Kp),
-    _Kd(Kd),
-    _Ki(Ki),
-    _pre_error(0),
-    _integral(0)
+    m_T(dt),
+    m_Max(max),
+    m_Min(min),
+    m_Kp(Kp),
+    m_Kd(Kd),
+    m_Ki(Ki)
 {
 }
 
-double PIDImpl::calculate( double setpoint, double pv )
+double PIDImpl::calculate(double setpoint, double measurement )
 {
-
     // Calculate error
-    double error = setpoint - pv;
+    double error = setpoint - measurement;
 
     // Proportional term
-    double Pout = _Kp * error;
+    m_PropotionalTerm = m_Kp * error;
 
     // Integral term
-    _integral += error * _dt;
-    double Iout = _Ki * _integral;
+    m_IntegralTerm = m_IntegralTerm + 0.5 * m_Ki * m_T * (error + m_PreviousError);
+
+    // Clamp Integral
+    if (m_IntegratorMin || m_IntegratorMax)
+        m_IntegralTerm = std::min(m_IntegratorMax, std::max(m_IntegratorMin, m_IntegralTerm));
 
-    // Derivative term
-    double derivative = (error - _pre_error) / _dt;
-    double Dout = _Kd * derivative;
+    // Derivative term (N.B. on measurement NOT error)
+    m_DerivativeTerm = -(2.0f * m_Kd * (measurement - m_PreviousMeasurement) + (2.0f * m_Tau - m_T) * m_DerivativeTerm)
+                       / (2.0f * m_Tau + m_T);
 
     // Calculate total output
-    double output = Pout + Iout + Dout;
+    double output = m_PropotionalTerm + m_IntegralTerm + m_DerivativeTerm;
 
-    // Restrict to max/min
-    if( output > _max )
-        output = _max;
-    else if( output < _min )
-        output = _min;
+    // Clamp Output
+    output = std::min(m_Max, std::max(m_Min, output));
 
     // Save error to previous error
-    _pre_error = error;
+    m_PreviousError = error;
+    m_PreviousMeasurement = measurement;
 
     return output;
 }
@@ -113,4 +170,3 @@ PIDImpl::~PIDImpl()
 {
 }
 
-#endif

libs/indibase/pid/pid.h

@@ -1,4 +1,6 @@
 /**
+ * Copyright 2021 Jasem Mutlaq <mutlaqja@ikarustech.com>
+ * Copyright (c) 2020 Philip Salmony
  * Copyright 2019 Bradley J. Snyder <snyder.bradleyj@gmail.com>
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -20,8 +22,7 @@
  * THE SOFTWARE.
  */
 
-#ifndef _PID_H_
-#define _PID_H_
+#pragma once
 
 class PIDImpl;
 class PID
@@ -35,12 +36,16 @@ class PID
         // min - minimum value of manipulated variable
         PID( double dt, double max, double min, double Kp, double Kd, double Ki );
 
+        void setIntegratorLimits(double min, double max);
+        void setTau(double value);
+
         // Returns the manipulated variable given a setpoint and current process value
         double calculate( double setpoint, double pv );
+        double propotionalTerm() const;
+        double integralTerm() const;
+        double derivativeTerm() const;
         ~PID();
 
     private:
         PIDImpl *pimpl;
 };
-
-#endif