Willem Melching / Mbed 2 deprecated bootje_v4

Dependencies:   mbed

Diff: verwerking/pid.cpp

Revision:
0:bec310bde899
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/verwerking/pid.cpp	Tue Sep 27 19:46:30 2011 +0000
@@ -0,0 +1,40 @@
+#include "pid.h"
+
+Pid::Pid(Goto * p, Kompas * k, Roer * r){
+    sp = p;
+    pv = k;
+    output = r;
+    dt = 0.1;
+    
+    kp = 1.2;
+    ki = 1;
+    kd = 0.1;
+    
+    prev_error = 0;
+}
+
+void Pid::update(void){
+    double error = verschil(pv->get(),sp->get());
+    
+    p = error * kp;
+    i = i + error * dt;
+    if (i > 25)
+        i = 25;
+    if (i < -25)
+        i = -25;
+    i *= ki;
+    
+    d = (error - prev_error)/dt * kd;
+    o = p+i+d + 50;
+    
+    if (o > 100)
+        o = 100;
+    if (0 < 0)
+        o = 0;
+    
+    
+    output->set(o);
+    prev_error = error;
+ 
+    
+}