Diff: autoMode.h

Revision:

79:7f86e18f40ef

Parent:

78:abb760e0a935

Child:

80:7e24b99b9b61

--- a/autoMode.h	Sat Oct 10 09:53:52 2015 +0000
+++ b/autoMode.h	Sun Oct 11 07:34:51 2015 +0000
@@ -20,7 +20,7 @@
             IndicatorAuto=1;
         }
     }
-    else if(b==6){  /*start*/
+    else if(b==6){ /*start*/
         resetState();
         flagf=1;
         targ_velocity=speed;
@@ -28,26 +28,38 @@
         stateR = defoR;
         stateL = defoL;
         step   = 0;
-        CMode  = 0;
+        CStep  = 1;
     }
     else if(b==8){ /*middle start*/
         resetState();
-        flagf=1;
+        flagf = 1;
         targ_velocity=speed;
-        sendData(7,57);
+        sendData(7,60);
         stateR = 17;
         stateL = 17;
         step   = 4;
-        CMode  = 6;
+        CStep  = 7;
     }   
-    else if(b==5){ /*reset*/
-        sendData(7,255);
+    else if(b==5){  /*opponents start*/
+        resetState();
+        flagf = 0;
+        sita=3.0*(PI/4.0);
+        targ_velocity = speed;
+        targ_sita     = 3.0*(PI/4.0);
+        sendData(7,60);
+        stateR = 17;
+        stateL = 17;
+        step   = 8;
     }
     else if(b==9){  /*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;
+#ifdef BLUE
         targ_sita=PI/4.0;
+#else   
+        targ_sita=-PI/4.0;
+#endif
         velocity_controller.reset();
         direction_controller.reset();
     }
@@ -115,18 +127,19 @@
         resetState();
         flagf = 1;
         targ_velocity=speed;
-        sendData(7,57);
+        sendData(7,60);
         stateR = 17;
         stateL = 17;
         step   = 4;
-        medge  = 1;
+        CStep  = 7;
     }
     else if(down){   /*opponents start*/
         resetState();
-        flagf = 1;
+        flagf = 0;
+        sita=3.0*(PI/4.0);
         targ_velocity = speed;
-        targ_sita     = 0.0;
-        sendData(7,17);
+        targ_sita     = 3.0*(PI/4.0);
+        sendData(7,60);
         stateR = 17;
         stateL = 17;
         step   = 8;
@@ -138,7 +151,11 @@
         Motor_swing=0;
 //        sita=PI/4.0,x=0.0,y=0.0;
         targ_velocity=0.0;
+#ifdef BLUE
         targ_sita=PI/4.0;
+#else   
+        targ_sita=-PI/4.0;
+#endif
         velocity_controller.reset();
         direction_controller.reset();
     }
@@ -186,7 +203,8 @@
     direction_controller.setInputLimits(-10.0, 10.0); //x2
 
     //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
-    velocity_controller.setOutputLimits(-1.0, 1.0);
+//    velocity_controller.setOutputLimits(-1.0, 1.0);
+    velocity_controller.setOutputLimits(0.0, 1.0);
     direction_controller.setOutputLimits(-1.0, 1.0);
 
     //set bias. 初期値
@@ -221,8 +239,8 @@
     sita_following();
     
     if(flagf==0){
-        Vr                  = ( x1 + x2 ) / 2.0;
-        Vl                  = ( x1 - x2 ) / 2.0;
+        Vr                  = ( 2.0*(-x1) + x2 ) / 3.0;
+        Vl                  = ( 2.0*(-x1) - x2 ) / 3.0;
     }
     else if(flagf==1){
         if(x2>0){