Diff: main.cpp

Revision:

3:8a8dc30fb08e

Parent:

2:ca2573919cb5

Child:

4:055913d9c737

diff -r ca2573919cb5 -r 8a8dc30fb08e main.cpp
--- a/main.cpp	Mon Oct 27 13:38:30 2014 +0000
+++ b/main.cpp	Wed Oct 29 15:46:21 2014 +0000
@@ -1,6 +1,11 @@
 #include "mbed.h"
 #include "encoder.h"
 
+#define K_P (0.1)
+#define K_I (0.1)
+#define K_D (0.0005 /TSAMP)
+#define TSAMP 0.001
+#define I_LIMIT 1.
 #include <iostream>
 
 Encoder encoderA(PTD0,PTD2);
@@ -9,33 +14,94 @@
 DigitalOut dir(PTA4);
 
 
-int32_t enca = 0, encb =0;
-float speed = 0.1;
-int n=0,a =1 ,b =0;
-
-int main()
-{
-
-    while(1) {
-
-        
-
-        enca = encoderA.getPosition();
+int32_t enc = 0,enca2 =0,enca1=0, encp=0, counts =0;
+float speed = 0.1, out =0;
+int pos =0,zero =0, fout;
 
 
-        if(enca < -900) {
-            dir = 1;
-        }
-        if(enca > 0) {
-            dir = 0;
-        }
-        
-        pwm = 1;
-        cout<<"enca"<<enca<<endl;
-
-        wait(0.1);
-    }
+void clamp(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
 }
 
 
 
+float pid(float setpoint, float measurement)
+{
+    float error;
+    static float prev_error = 0;
+    float           out_p = 0;
+    static float    out_i = 0;
+    float           out_d = 0;
+    error  = setpoint-measurement;
+    out_p  = error*K_P;
+    out_i += error*K_I;
+    out_d  = (error-prev_error)*K_D;
+    clamp(&out_i,-I_LIMIT,I_LIMIT);
+    prev_error = error;
+    out  = out_i;
+   
+    return out;
+}
+
+float getv(float delta_t)
+{
+    float v;
+    int n =0 ;
+    while(n<3) {
+        wait(delta_t);
+        enc = encoderA.getPosition();
+        enca2 = enca1;
+        enca1 = enc;
+        n++;
+       
+    }
+   
+    counts = (enca1 - enca2)/delta_t;
+    v = (counts)*((2*3.14159265359)/1550);
+    return v;
+}
+
+
+
+int main()
+{       
+        float velocity =1;
+        while(velocity !=0){
+        
+        dir = 0;
+        pwm.write(0.1);
+        velocity =getv(0.2);
+        }
+        pwm = 0;
+        dir =1;
+        
+        encoderA.setPosition(0);
+        zero = encoderA.getPosition();
+        cout<<"zero encoder: "<<zero<<endl;
+        
+        cout<<"positie in encoder bits: "<<endl;
+        cin>>pos;
+        cout<<"pos: "<<endl;
+        
+        enc = encoderA.getPosition();
+        
+        while((pos - enc)>10){
+        wait(0.2);
+        enc = encoderA.getPosition();
+        out = pid(pos, enc);
+        cout<<"out "<<out<<endl;
+        cout<<"enc: "<<enc<<endl;
+        pwm = out;
+        
+        
+        }
+       
+        pwm =0;
+       cout<<"final error: "<<error<<endl;
+       while(1);
+       
+}
+
+
+