Diff: main.cpp

Revision:

6:0403c2e276c2

Parent:

5:93ccec91a4ae

--- a/main.cpp	Thu Oct 30 11:27:04 2014 +0000
+++ b/main.cpp	Thu Oct 30 12:38:09 2014 +0000
@@ -2,7 +2,7 @@
 #include "encoder.h"
 
 #define K_P (0.1)
-#define K_I (0.0001)
+#define K_I (0.00001)
 #define K_D (0.0005 /TSAMP)
 #define TSAMP 0.001
 #define I_LIMIT 1.
@@ -62,54 +62,47 @@
     return v;
 }
 
- float reset(){
-        
-        return pwm;
-        }
+float reset()
+{
+    v = 1;
+    while(v !=0) {
+
+        dir = 0;
+        pwm.write(0.1);
+        v =getv(0.1);
+    }
+    pwm = 0;
+    dir =1;
+    encoderA.setPosition(0);
+    zero = encoderA.getPosition();
+    cout<<"zero encoder: "<<zero<<endl;
+    return pwm;
+}
 
+float gotopos(float pos)
+{
+    enc = encoderA.getPosition();
 
+    while((pos - enc)>0) {
+        wait(0.1);
+        enc = encoderA.getPosition();
+        out = pid(pos, enc);
+        pwm = out;
+    }
+    pwm =0;
+    enc = encoderA.getPosition();
+    cout<<"final enc: "<<enc<<endl;
+    cout<<"final error: "<<error<<endl;
+    return pwm;
+}
 
 int main()
 {
     while(1) {
-       pwm = 1;
-       cout<<"voor de loop"<<endl;
-        
-        while(v !=0) {
-
-            dir = 0;
-            pwm.write(0.1);
-            v =getv(0.2);
-        }
-        pwm = 0;
-        dir =1;
-        encoderA.setPosition(0);
-         zero = encoderA.getPosition();
-        cout<<"zero encoder: "<<zero<<endl;
-
-       
-     
-       
-        cout<<"positie in encoder bits: "<<endl;
+        reset();
+        cout<<"typ pos: "<<endl;
         cin>>pos;
-        cout<<"enc in: "<<pos<<endl;
-
-        enc = encoderA.getPosition();
-
-        while((pos - enc)>0) {
-           wait(0.1);
-            enc = encoderA.getPosition();
-            out = pid(pos, enc);
-            pwm = out;
-            cout<<"pwm: "<<pwm<<endl;
-
-
-        }
-
-        pwm =0;
-        enc = encoderA.getPosition();
-        cout<<"final enc: "<<enc<<endl;
-        cout<<"final error: "<<error<<endl;
+        gotopos(pos);
         wait(5);
     }
 }