Diff: main.cpp

Revision:

4:055913d9c737

Parent:

3:8a8dc30fb08e

Child:

5:93ccec91a4ae

--- a/main.cpp	Wed Oct 29 15:46:21 2014 +0000
+++ b/main.cpp	Wed Oct 29 16:45:46 2014 +0000
@@ -2,7 +2,7 @@
 #include "encoder.h"
 
 #define K_P (0.1)
-#define K_I (0.1)
+#define K_I (0.00001)
 #define K_D (0.0005 /TSAMP)
 #define TSAMP 0.001
 #define I_LIMIT 1.
@@ -40,7 +40,7 @@
     clamp(&out_i,-I_LIMIT,I_LIMIT);
     prev_error = error;
     out  = out_i;
-   
+
     return out;
 }
 
@@ -54,9 +54,9 @@
         enca2 = enca1;
         enca1 = enc;
         n++;
-       
+
     }
-   
+
     counts = (enca1 - enca2)/delta_t;
     v = (counts)*((2*3.14159265359)/1550);
     return v;
@@ -65,43 +65,47 @@
 
 
 int main()
-{       
+{
+    while(1) {
         float velocity =1;
-        while(velocity !=0){
-        
-        dir = 0;
-        pwm.write(0.1);
-        velocity =getv(0.2);
+        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);
+        cout<<"enc in: "<<pos<<endl;
+
         enc = encoderA.getPosition();
-        out = pid(pos, enc);
-        cout<<"out "<<out<<endl;
-        cout<<"enc: "<<enc<<endl;
-        pwm = out;
-        
-        
+
+        while((pos - enc)>0) {
+           wait(0.1);
+            enc = encoderA.getPosition();
+            out = pid(pos, enc);
+            pwm = out;
+            cout<<"pwm: "<<pwm<<endl;
+
+
         }
-       
+
         pwm =0;
-       cout<<"final error: "<<error<<endl;
-       while(1);
-       
+        enc = encoderA.getPosition();
+        cout<<"final enc: "<<enc<<endl;
+        cout<<"final error: "<<error<<endl;
+        wait(5);
+    }
 }
 
 
 
+
+