Calvin Kalintra / pid

PID

Dependents:   balance_all

Diff: PID.cpp

Revision:
0:3d03a93d9671
diff -r 000000000000 -r 3d03a93d9671 PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Wed May 16 10:27:33 2018 +0000
@@ -0,0 +1,18 @@
+#include "PID.h"
+#define Kp 0.1
+#define Ki 0.00001
+
+void PID::control(float x, float *output, float time, float integral)
+{
+    float error = - x;
+    float Pout = Kp * error;//multiply the error with kp to get P output
+
+    integral += error * time;//integral adds the previous error + the error now * time the error presist
+    
+    float Iout = Ki * integral;//multiply by Ki
+    output[0] = Pout + Iout;
+    if( output[0] > 1 )//limit the output
+        {output[0] = 1;}
+    else if( output[0] < -1 )
+        {output[0] = -1;}
+}
\ No newline at end of file