Diff: autoMode.h

Revision:

83:e1638c58e1f1

Parent:

80:7e24b99b9b61

Child:

84:919a335ac81e

--- a/autoMode.h	Sun Oct 11 07:42:18 2015 +0000
+++ b/autoMode.h	Mon Oct 12 06:15:45 2015 +0000
@@ -4,7 +4,7 @@
 /***PID Controller***/
 //PID velocity_controller(36.0,5274.0 ,0.0,RATE);
 //PID direction_controller(36.0,3.0,0.0,RATE);
-PID velocity_controller(9.0,5274.0 ,0.0,RATE);
+PID velocity_controller(9.0,5274.0,0.0,RATE);
 PID direction_controller(36.0,3.0,0.0,RATE);
 
 #define MAX_VALUE 93
@@ -21,35 +21,45 @@
         }
     }
     else if(b==6){ /*start*/
-        resetState();
-        flagf=1;
-        targ_velocity=speed;
-        sendData(7,defoR);
-        stateR = defoR;
-        stateL = defoL;
-        step   = 0;
-        CStep  = 1;
+        if(edge6){
+            edge6=0;
+            resetState();
+            flagf=1;
+            targ_velocity=speed;
+//            sendData(7,defoR);
+            sendData(4,defoR);
+            wait(0.1);
+            sendData(5,defoL);
+            stateR = defoR;
+            stateL = defoL;
+            step   = 0;
+            CStep  = 1;
+        }
     }
-    else if(b==8){ /*middle start*/
-        resetState();
-        flagf = 1;
-        targ_velocity=speed;
-        sendData(7,60);
-        stateR = 17;
-        stateL = 17;
-        step   = 4;
-        CStep  = 7;
+    else if(b==5){ /*middle start*/
+        if(edge5){
+            edge5=0;
+            resetState();
+            flagf = 1;
+            targ_velocity=speed;
+            sendData(7,61);
+            stateR = 17;
+            stateL = 17;
+            step   = 5;
+            CStep  = 6;
+        }
     }   
-    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==8){  /*opponents start*/
+        if(edge8){
+            edge8=0;
+            resetState();
+            flagf = 1;
+            targ_velocity = speed;
+            sendData(7,60);
+            stateR = 17;
+            stateL = 17;
+            step   = 15;
+        }
     }
     else if(b==9){  /*stop*/
         Motor_swing=0;
@@ -102,6 +112,9 @@
     if(b!=2) edge2=1;
     if(b!=3) edge3=1;
     if(b!=4) edge4=1;
+    if(b!=6) edge6=1;
+    if(b!=8) edge8=1;
+    if(b!=5) edge5=1;
 }
 #else
 void autoPS3(){
@@ -128,20 +141,20 @@
         flagf = 1;
         targ_velocity=speed;
         sendData(7,60);
-        stateR = 17;
-        stateL = 17;
-        step   = 4;
+        stateR = 60;
+        stateL = 60;
+        step   = 5;
         CStep  = 7;
     }
     else if(down){   /*opponents start*/
         resetState();
-        flagf = 0;
+        flagf = 1;
         sita=3.0*(PI/4.0);
         targ_velocity = speed;
         targ_sita     = 3.0*(PI/4.0);
-        sendData(7,60);
-        stateR = 17;
-        stateL = 17;
+        sendData(7,65);
+        stateR = 65;
+        stateL = 65;
         step   = 8;
     }
     else if(square){ /*reset*/