Diff: main.cpp

Revision:

1:79c4c5746a33

Parent:

0:210b81f6c5b6

--- a/main.cpp	Tue Jun 17 08:24:18 2014 +0000
+++ b/main.cpp	Wed Jun 25 12:25:22 2014 +0000
@@ -8,7 +8,7 @@
 PwmOut motormin(PTC3);
 
 double deliver, ts, pos, pos1, move;
-volatile bool flag, loop;
+volatile bool flag, statloop,viscloop;
 double Move[25];
 int n;
 
@@ -29,10 +29,12 @@
     motorplus.period(1.0/60000.0);
     motormin.period(1.0/60000.0);
     flag=true;
-    loop=true;
+    statloop=true;
+    viscloop=true;
     wait(3);
+    
     Loop.attach(setflag,ts);
-    while(loop==true) {
+    while(statloop==true) {
         while(flag !=true) {
         }
         flag=false;
@@ -46,18 +48,45 @@
         }
         move=Move[24]-Move[0];
         if (abs(move)>0.002) {
-            loop=false;
+            statloop=false;
             motorplus.write(0.0);
             motormin.write(0.0);
-            pc.printf("Fr %f\n\r",deliver);
+            pc.printf("Fs %f\n\r",deliver);
+            wait(1);
+        }
+        
+        deliver = deliver+ts;
+
+    }
+
+    deliver=0.4;
+    motorplus.write(-deliver);
+    motormin.write(deliver);
+    wait(1.0);
+    while(viscloop==true) {
+        while(flag !=true) {
+        }
+        flag=false;
+        motorplus.write(-deliver);
+        motormin.write(deliver);
+        pos=Enc.getPosition()/1024.0;
+        Move[n]=pos-pos1;
+        n += 1;
+        if (n >24) {
+            n=0;
+        }
+        move=Move[24]-Move[0];
+
+        if (abs(move)==0.0) {
+            viscloop=false;
+            motorplus.write(0.0);
+            motormin.write(0.0);
+            pc.printf("Fv %f\n\r",deliver);
             return 0;
         }
-        pc.printf("del %f mo %f\n\r",deliver, move);
-        
-        deliver=deliver+ts/10;
+        deliver = deliver-ts;
 
     }
 
 
-
 }
\ No newline at end of file