Diff: Pid_control.cpp

Revision:

0:7ab090cd6520

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pid_control.cpp	Thu Jul 13 13:42:50 2017 +0000
@@ -0,0 +1,61 @@
+#include "Pid_control.h"
+
+Pid_control::Pid_control()
+{
+  
+    derivate =0;
+    sum=0.0;
+    delta_t= 0.1;
+    actual_value_old=0;
+}
+
+Pid_control::~Pid_control()
+{
+
+  
+}
+
+
+float Pid_control::control( int actual_value, int target_value, float p, float i,float d,  float min_control_out,  float max_control_out  )
+{
+ 
+
+
+    error = target_value - actual_value;
+
+    // I Anteil
+
+    sum = ((float) error) + sum;
+
+    // D Anteil
+
+    derivate = ((float)(actual_value - actual_value_old))/(delta_t);
+
+    // PID Control
+
+    control_out= p * ((float) error) +  i*sum - d*derivate;
+
+
+
+    if(control_out > max_control_out )
+    {
+        if(i > 0)
+        {
+        sum = sum - (control_out - max_control_out)/i;
+        }
+        control_out =  max_control_out;
+
+    }
+    else if (control_out < min_control_out )
+    {
+        control_out = min_control_out;
+
+    }
+
+
+
+    actual_value_old = actual_value;
+
+    return control_out;
+
+}