Diff: main.cpp

Revision:

6:b69b9597d4fc

Parent:

5:36df561f3ac1

Child:

7:2e4eb23700b0

--- a/main.cpp	Mon Nov 03 11:49:06 2014 +0000
+++ b/main.cpp	Mon Nov 03 15:01:09 2014 +0000
@@ -400,51 +400,46 @@
         
         check =0;
     }
-    clampint(&stand,0,3);
+    clampint(&stand,1,3);
     return stand;
 }
 
-void armtopos(int pos1)
+float gotopos(int pos)
 {
-    int place=0;
-    switch(pos1) {
-        case 0:
-            place = 0;
-            break;
+    float out1;
+    
+    switch(pos) {
         case 1:
-            place = 100;
+            pos = 100;
             break;
 
         case 2:
-            place = 200;
+            pos = 200;
             break;
 
         case 3:
-            place = 250;
+            pos = 250;
             break;
     }
-
-    while((abs(place-encoder1.getPosition()) >50)|| (v1!=0)) {
+    
+    while((abs(pos-encoder1.getPosition()) >6)|| (v1!= 0)) {
 
         while(!looptimerflag);
         looptimerflag = false;
-        cout<<v1<<endl;
-        cout<<place-encoder1.getPosition()<<endl;
-        pwm1 = pidposition(place,encoder1.getPosition());
+        out1 = pidposition(pos,encoder1.getPosition());
 
-        if(pwm1>0) {
+        if(out1>0) {
             dir1 = 1;
 
-        } else if(pwm1<0) {
+        } else if(out1<0) {
             dir1 = 0;
         }
-        pwm1 = fabs(pwm1);
+        pwm1 = fabs(out1);
         v1 = getv(0.001);
     }
     pwm1 =0;
 
-    cout<<"place done"<<endl;
-    wait(1);
+    return pwm1;
 }
 
 // MAIN SCRIPT
@@ -490,6 +485,7 @@
                 case 1:
                     cout<<"fase2"<<endl;
                     armstand=armposition(y1,y2);
+                    gotopos(armstand);
                     cout<<"armstand "<<armstand<<endl;
                     
                     //armtopos(armstand);