From f8416707278aabcebcccbbe4e1a8a9b5d66e0f82 Mon Sep 17 00:00:00 2001
From: Shlomo Hecht <shlomo@twine-s.com>
Date: Thu, 29 Nov 2018 14:58:05 +0200
Subject: advanced PID in thread and heaters - use only with modified
 congiguration

---
 .../Embedded_SW/Embedded/Modules/Control/PIDAlgo.c | 36 ++++++++++++++++++++++
 .../Embedded_SW/Embedded/Modules/Control/PIDAlgo.h |  4 +++
 2 files changed, 40 insertions(+)

(limited to 'Software/Embedded_SW/Embedded/Modules/Control')

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index f37759943..72774461a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -36,4 +36,40 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P
 	return output;
 }
 
+float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
+{
+    float error;
+    float derivative;
+    float output;
+
+    //Calculate P,I,D
+    error = _setPoint - _mesuredParam;
+
+    //In case of error too small then stop integration
+    if(abs(error) > params->epsilon)
+    {
+        *_integral = *_integral + error*params->dt;
+    }
+
+    derivative = (error - *_pre_error)/params->dt;
+    float IntegralErrorMultiplier;
+    float ProportionalErrorMultiplier;
+
+    output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
+
+    //Saturation Filter
+    if(output > params->MAX)
+    {
+        output = params->MAX;
+    }
+    else if(output < params->MIN)
+    {
+        output = params->MIN;
+    }
+
+    //Update error
+    *_pre_error = error;
+
+    return output;
+}
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
index 03c363131..4792be9ec 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
@@ -12,7 +12,11 @@ typedef struct
     float Kp;
     float Kd;
     float Ki;
+    float IntegralErrorMultiplier;
+    float ProportionalErrorMultiplier;
+
 }PID_Config_Params;
 float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
+float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
 
 #endif /* MODULES_PIDALGO_H_ */
-- 
cgit v1.3.1