From 443c4412796979ceaaaaaecfec2bb15788a49b6e Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Sun, 2 Sep 2018 19:04:44 +0200
Subject: [PATCH] Some controllers implemented PD and PI

---
 Makefile            |   1 +
 src/control/PID.cpp | 103 ++++++++++++++++++++++++++++++++++++++++++++
 src/control/PID.h   |  63 +++++++++++++++++++++++++++
 3 files changed, 167 insertions(+)
 create mode 100644 src/control/PID.cpp
 create mode 100644 src/control/PID.h

diff --git a/Makefile b/Makefile
index 1ce3ac6..f78c47d 100644
--- a/Makefile
+++ b/Makefile
@@ -645,6 +645,7 @@ OBJECTS += ./src/drivers/MPU6000.o
 OBJECTS += ./src/drivers/MS5611.o
 OBJECTS += ./src/drivers/stepper.o
 OBJECTS += ./src/drivers/servo.o
+OBJECTS += ./src/control/PID.o
 OBJECTS += ./src/math/Utilities.o
 
 
diff --git a/src/control/PID.cpp b/src/control/PID.cpp
new file mode 100644
index 0000000..78d1fc6
--- /dev/null
+++ b/src/control/PID.cpp
@@ -0,0 +1,103 @@
+#include "src/control/PID.h"
+#include "src/math/Utilities.h"
+
+
+using namespace math;
+
+namespace control {
+
+/*----------------------------------- P Controller -----------------------------------*/
+controllerP::controllerP(float kP)
+: m_kP(kP)
+{
+}
+
+float controllerP::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    // output
+    return m_kP*error;
+}
+
+/*----------------------------------- PD Controller ----------------------------------*/
+controllerPD::controllerPD(float kP, float kD, float saturation)
+: m_kP(kP)
+, m_kD(kD)
+, m_saturation(saturation)
+, m_lastError(0)
+, m_pt1FilterApply4(10.0f)
+{
+}
+
+float controllerPD::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kP*error);
+
+    float dTerm(m_kD*((error-m_lastError)/dT));
+
+    // Store error for next iteration
+    m_lastError = error;
+
+    dTerm = m_pt1FilterApply4.filter(dTerm, dT);
+
+    float output(constrain(pTerm + dTerm, m_saturation));
+
+    return output;
+}
+
+/*----------------------------------- Alternative PD Controller ----------------------------------*/
+controllerPD2::controllerPD2(float kP, float kD, float saturation)
+: m_kP(kP)
+, m_kD(kD)
+, m_saturation(saturation)
+, m_inputTMinus1(0.0f)
+, m_inputTMinus2(0.0f)
+, m_setPointTMinus1(0.0f)
+{
+}
+
+float controllerPD2::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kP*error);
+
+    float dTerm = + (m_kD*(setPoint - m_setPointTMinus1) - m_kD*(input - m_inputTMinus2))/dT;
+    
+    m_inputTMinus2 = m_inputTMinus1;
+    m_inputTMinus1 = input;
+    m_setPointTMinus1 = setPoint;
+    return(pTerm + dTerm);
+}
+
+/*----------------------------------- PI Controller ----------------------------------*/
+controllerPI::controllerPI(float kP, float kI, float saturation, float iTermSaturation)
+: m_kP(kP)
+, m_kI(kI)
+, m_saturation(saturation)
+, m_iTermSaturation(iTermSaturation)
+, m_integrator(0)
+{
+}
+
+float controllerPI::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kP*error);
+
+    // Accumulate to the integrator
+    float iTerm(constrain(m_integrator + m_kI*error*dT, m_iTermSaturation));
+
+    // Store
+    m_integrator = iTerm;
+
+    float output(constrain(pTerm + iTerm, m_saturation));
+
+    return output;
+}
+
+}
\ No newline at end of file
diff --git a/src/control/PID.h b/src/control/PID.h
new file mode 100644
index 0000000..2473cba
--- /dev/null
+++ b/src/control/PID.h
@@ -0,0 +1,63 @@
+#include "src/control/lpf.h"
+
+namespace control {
+
+class controllerP
+{
+public:
+    controllerP(float kP);
+
+    float run(float dT, float input, float setPoint);
+
+private:
+    float m_kP;
+
+};
+
+class controllerPD
+{
+public:
+    controllerPD(float kP, float kD, float saturation);
+
+    float run(float dT, float input, float setPoint);
+
+private:
+    float m_kP;
+    float m_kD;
+    float m_saturation;
+    float m_lastError;
+    pt1FilterApply4 m_pt1FilterApply4;
+};
+
+class controllerPD2
+{
+public:
+    controllerPD2(float kP, float kD, float saturation);
+
+    float run(float dT, float input, float setPoint);
+
+private:
+    float m_kP;
+    float m_kD;
+    float m_saturation;
+    float m_inputTMinus1;
+    float m_inputTMinus2;
+    float m_setPointTMinus1;
+};
+
+class controllerPI
+{
+public:
+    controllerPI(float kP, float kI, float saturation, float iTermSaturation);
+
+    float run(float dT, float input, float setPoint);
+
+private:
+    float m_kP;
+    float m_kI;
+    float m_saturation;
+    float m_iTermSaturation;
+    float m_integrator;
+};
+
+}
\ No newline at end of file