Diff: main.cpp

Revision:

1:5ac17a29fa08

Parent:

0:e89ba43e3f05

Child:

2:44cc689e14d6

--- a/main.cpp	Wed Oct 29 11:33:37 2014 +0000
+++ b/main.cpp	Wed Oct 29 13:25:18 2014 +0000
@@ -1,6 +1,10 @@
 #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,9 +13,22 @@
 DigitalOut dir(PTA4);
 
 
+
+
 int32_t enca = 0,enca1 =0,enca2 =0,counts=0, encb;
-float speed = 0,v=0;
+float speed = 0,v=0,v_ref=0;
 int n=0,a =1 ,b =0;
+float out;
+
+//clamps value 'in' to min or max when exceeding those values
+//if you'd like to understand the statement below take a google for
+//'ternary operators'.
+void clamp(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+
+
 
 float pid(float setpoint, float measurement)
 {
@@ -26,11 +43,45 @@
     out_d  = (error-prev_error)*K_D;
     clamp(&out_i,-I_LIMIT,I_LIMIT);
     prev_error = error;
-    scope.set(1,out_p);
-    scope.set(2,out_i);
-    scope.set(3,out_d);
-    return out_p + out_i + out_d;
+    out  = out_i;
+    clamp(&out, -1,1);
+    out = fabs(out);
+    return out;
 }
+
+float getv()
+{
+    while(n<3) {
+        wait(0.2);
+        enca = encoderA.getPosition();
+        enca2 = enca1;
+        enca1 = enca;
+        n++;
+        cout<<n<<endl;
+    }
+
+    n =0 ;
+    counts = (enca1 - enca2)/0.4;
+    cout<<"counts: "<<counts<<endl;
+    v = (counts)*((2*3.14159265359)/1550);
+    return v;
+}
+int main()
+{
+    dir = 1;
+    cout<<"typ v_ref: "<<endl;
+    cin>>v_ref;
+    cout<<"v_ref: "<<v_ref<<endl;
+    while(1) {
+        v = getv();
+        cout<<"v: "<<v<<endl;
+        out = pid(v_ref,v);
+        cout<<"out: "<<out<<endl;
+        pwm = out;
+    }
+}
+
+
 /*
 int main()
 {
@@ -44,40 +95,4 @@
 }
 */
 
-int main()
-{
-    speed = 0.3;
-    pwm = speed;
-   
-    while(1){
-    wait(0.5);
-    enca = encoderA.getPosition();
-   
-    enca2 = enca1;
-    enca1 = enca;
-    
-    counts = (enca1 - enca2)/0.5;
-    v = (counts)*((2*3.14159265359)/1550);
-    
-    cout<<"v: "<<v<<endl;
-    
 
-    
-    }
-   
-    
-    
-    
-
-}
-//clamps value 'in' to min or max when exceeding those values
-//if you'd like to understand the statement below take a google for
-//'ternary operators'.
-void clamp(float * in, float min, float max)
-{
-    *in > min ? *in < max? : *in = max: *in = min;
-}
-
-
-
-