From cd0d81c72ab491e92070a70fc42dec5bffa105f3 Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Thu, 20 Sep 2018 21:23:15 +0200
Subject: [PATCH] PID controller added with gain scaling

---
 src/control/PID.cpp | 55 ++++++++++++++++++++++++++++++++++++++++++++-
 src/control/PID.h   | 21 +++++++++++++++++
 2 files changed, 75 insertions(+), 1 deletion(-)

diff --git a/src/control/PID.cpp b/src/control/PID.cpp
index 7f23708..b2843aa 100644
--- a/src/control/PID.cpp
+++ b/src/control/PID.cpp
@@ -26,7 +26,7 @@ controllerPD::controllerPD(float kP, float kD, float saturation)
 , m_kD(kD)
 , m_saturation(saturation)
 , m_lastError(0)
-, m_pt1FilterApply4(100.0f)
+, m_pt1FilterApply4(50.0f)
 {
 }
 
@@ -100,4 +100,57 @@ float controllerPI::run(float dT, float input, float setPoint)
     return output;
 }
 
+/*----------------------------------- PID Controller ---------------------------------*/
+
+controllerPID::controllerPID(float kP, float kI, float kD, float saturation, float iTermSaturation)
+: m_kP(kP)
+, m_kI(kI)
+, m_kD(kD)
+, m_kPScale(0.0f)
+, m_kIScale(0.0f)
+, m_kDScale(0.0f)
+, m_saturation(saturation)
+, m_lastError(0.0f)
+, m_iTermSaturation(iTermSaturation)
+, m_integrator(0.0f)
+, m_pt1FilterApply4(100.0f)
+{
+}
+
+float controllerPID::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kPScale * m_kP * error);
+
+    // Accumulate to the integrator
+    float iTerm(
+        constrain(
+            m_integrator + (m_kIScale * m_kI * error * dT), 
+            m_iTermSaturation));
+
+    // Store
+    m_integrator = iTerm;
+
+    float dTerm((dT != 0.0f) ? m_kDScale * m_kD * ((error-m_lastError)/dT) : 0.0f);
+
+    // Store error for next iteration
+    m_lastError = error;
+
+    dTerm = m_pt1FilterApply4.filter(dTerm, dT);
+
+    float output(constrain(pTerm + iTerm + dTerm, m_saturation));
+
+    return output;
+}
+
+void controllerPID::setGainScaling(float kPScale, float kIScale, float kDScale)
+{
+    static float scaleFactor(1.0f / 1000.0f);
+    
+    m_kPScale = (1000.0f + kPScale) * scaleFactor;
+    m_kIScale = (1000.0f + kIScale) * scaleFactor;
+    m_kDScale = (1000.0f + kDScale) * scaleFactor;
+}
+
 }
\ No newline at end of file
diff --git a/src/control/PID.h b/src/control/PID.h
index 2473cba..80cefbe 100644
--- a/src/control/PID.h
+++ b/src/control/PID.h
@@ -60,4 +60,25 @@ private:
     float m_integrator;
 };
 
+class controllerPID
+{
+public:
+    controllerPID(float kP, float kI, float kD, float saturation, float iTermSaturation);
+
+    float run(float dT, float input, float setPoint);
+
+    //! Input values in range [-1000, 1000] will result in scales 
+    //! from [0-2].
+    void setGainScaling(float kPScale, float kIScale, float kDScale);
+
+private:
+    float m_kP, m_kI, m_kD;
+    float m_kPScale, m_kIScale, m_kDScale;
+    float m_saturation;
+    float m_lastError;
+    float m_iTermSaturation;
+    float m_integrator;
+    pt1FilterApply4 m_pt1FilterApply4;
+};
+
 }
\ No newline at end of file