Revision 5:daf561abb67d, committed 2014-10-29

Comitter:

BasvanBuuren

Date:

Wed Oct 29 10:05:43 2014 +0000

Parent:

4:ce3d15797939

Child:

6:3c4a2afb11e5

Commit message:

Alvorens de PID regelaar wordt toegepast

Changed in this revision

--- a/main.cpp	Tue Oct 28 14:04:21 2014 +0000
+++ b/main.cpp	Wed Oct 29 10:05:43 2014 +0000
@@ -4,17 +4,39 @@
 #include "encoder.h"
 #include <iostream>
 
+#define K_P 0
+#define K_I 0
+#define K_D 0
+#define I_LIMIT 1.
+
 #define MAXENCO 300
 #define MINENCO 0
 
+
+//define in and output
 Encoder encoderA(PTD0,PTD2);
 PwmOut m1_speedout(PTA5);
 DigitalOut m1_dir(PTA4);
 
+//define functions
 void slam();
+void clamp(float * in, float min, float max);
+float pid(float rev_value, float mea_value);
+
+//define global variables
 int y1;
 float m1_speed;
 
+int main()
+{
+    while(1)
+    {
+        cin >> y1;
+        cout << y1 << endl;
+        slam();
+    }
+}
+
 void slam()
 {
     float enca;
@@ -64,12 +86,23 @@
     }
 }
 
-int main()
+void clamp(float * in, float min, float max)
+{
+    *in > min ? * in < max? :  *in = max : *in = min;
+}
+
+float pid(float rev_value, float mea_value)
 {
-    while(1)
-    {
-        cin >> y1;
-        cout << y1 << endl;
-        slam();
-    }
+    float error;
+    static float prev_error = 0;
+    float p_out = 0;
+    static float i_out = 0;
+    float d_out = 0;
+    error = rev_value - mea_value;
+    p_out = error * K_P;
+    i_out += error * K_I;
+    d_out = (error - prev_error) * K_D;
+    clamp(&i_out,-I_LIMIT,I_LIMIT);
+    prev_error=error;
+    return p_out + i_out + d_out;
 }
\ No newline at end of file