Diff: main.cpp

Revision:

0:9a990e099b26

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 30 15:01:15 2013 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+//const float dt = 0.02;
+
+const float dt = 0.02;
+float Kp = 0.125;
+float Ki = 0.844;
+float Kd = 0.00679;
+float error_t0 = 0;
+float error_t_1;
+float P_regelaar;
+float I_regelaar;
+float D_regelaar;
+float integral_i;
+float integral_0 = 0;
+
+// setpoint is gewilde waarde
+// motorA.getPosition() is gemeten waarde
+
+do{
+    wait(dt)
+    error_ti = setpointA - motorA.getPosition();
+    P_regelaar = Kp * error_ti
+    D_regelaar = Kd * ((error_ti - error_t0) / dt);
+    integral_i = integral_0 + (error_ti * dt);
+    I_regelaar = Ki * integral_i;
+    integral_0 = integral_i;
+    error_t0 = error_ti;
+}
+
+output_regelaar = P_regelaar + I_regelaar + D_regelaar;
+
+
+
+