Version 1.3.6.2 load dancer parameters, othe rsw parameters

1 files changed, 48 insertions, 0 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index f9aee7929..a443b4785 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -71,3 +71,51 @@ float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_
     return output;
 }
 
+float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
+{
+    float error;
+    float derivative;
+    float output;
+
+    //double error = *mySetpoint - input;
+    error = _setPoint - _mesuredParam;
+    //ITerm+= (ki * error);
+    *_integral = *_integral + (error*params->Ki);
+    /*  if(ITerm > outMax)
+      {
+          ITerm= outMax;
+      }
+      else if(ITerm < outMin)
+      {
+          ITerm= outMin;
+      } */
+    if(*_integral > params->MAX)
+    {
+        *_integral = params->MAX;
+    }
+    else if(*_integral < params->MIN)
+    {
+        *_integral = params->MIN;
+    }
+    //  double dInput = (input - lastInput);
+    derivative = error - *_pre_error;
+
+      /*Compute PID Output*/
+    //  double output = kp * error + ITerm- kd * dInput;
+    output = params->Kp*error/params->ProportionalErrorMultiplier + *_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;
+}