Diff: autoMode.h

Revision:

64:a98fe602c26d

Parent:

63:dad3f4e5e99c

Child:

65:cebdfc0184d7

--- a/autoMode.h	Sun Oct 04 07:25:49 2015 +0000
+++ b/autoMode.h	Sun Oct 04 10:17:46 2015 +0000
@@ -17,6 +17,7 @@
         }
     }
     else if(b==6){  /*start*/
+        flagf=1;
         targ_velocity=speed;
         sendData(4,defoR);
         wait(0.1);
@@ -28,13 +29,14 @@
         CMode  = 0;
     }
     else if(b==8){ /*middle start*/
+        flagf=1;
         targ_velocity=speed;
-        sendData(5,13);
+        sendData(5,16);
         wait(0.1);
-        sendData(4,13);
+        sendData(4,16);
         wait(0.1);
-        stateR = 13;
-        stateL = 13;
+        stateR = 16;
+        stateL = 16;
         step   = 4;
         CMode  = 6;
     }   
@@ -97,6 +99,7 @@
         }
     }
     else if(up){  /*start*/
+        flagf=1;
         targ_velocity=speed;
         sendData(5,defoR);
         wait(0.1);
@@ -108,13 +111,14 @@
         CStep  = 1;
     }
     else if(right){  /*middle start*/
+        flagf = 1;
         targ_velocity=speed;
-        sendData(5,13);
+        sendData(5,16);
         wait(0.1);
-        sendData(4,13);
+        sendData(4,16);
         wait(0.1);
-        stateR = 13;
-        stateL = 13;
+        stateR = 16;
+        stateL = 16;
         step   = 4;
         CStep  = 6;
     }
@@ -132,7 +136,7 @@
     }
     else if(cross){  /*stop*/
         Motor_swing=0;
-        sita=PI/4.0,x=0.0,y=0.0;
+//        sita=PI/4.0,x=0.0,y=0.0;
         targ_velocity=0.0;
         targ_sita=PI/4.0;
         velocity_controller.reset();
@@ -208,19 +212,24 @@
 {
     velocity_following();
     sita_following();
-//    Vr                  = ( x1 + x2 ) / 2.0;
-//    Vl                  = ( x1 - x2 ) / 2.0;
-    if(x2>0){
-        Vr                  = x1;
-        Vl                  = x1 - x2;
+    
+    if(flagf==0){
+        Vr                  = ( x1 + x2 ) / 2.0;
+        Vl                  = ( x1 - x2 ) / 2.0;
     }
-    else{
-        Vr                  = x1 + x2;
-        Vl                  = x1;
+    else if(flagf==1){
+        if(x2>0){
+            Vr                  = x1;
+            Vl                  = x1 - x2;
+        }
+        else{
+            Vr                  = x1 + x2;
+            Vl                  = x1;
+        }
     }
     
-    if( abs(Vr) < 0.05 ) Vr = 0.0;
-    if( abs(Vl) < 0.05 ) Vl = 0.0;
+    if( abs(Vr) < 0.2 ) Vr = 0.0;
+    if( abs(Vl) < 0.2 ) Vl = 0.0;
     Move_r( ( float ) Vr );
     Move_l( ( float ) Vl );
 }