1 files changed, 39 insertions, 0 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
new file mode 100644
index 000000000..fbea57f7d
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -0,0 +1,39 @@
+
+#include "PIDAlgo.h"
+
+float PIDAlgorithmCalculation(uint32_t _setPoint,uint32_t _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;
+
+	output = params->Kp*error + params->Ki**_integral + 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;
+}
+
+