Diff: autoMode.h

Revision:

84:919a335ac81e

Parent:

83:e1638c58e1f1

Child:

85:dd18a2d79956

--- a/autoMode.h	Mon Oct 12 06:15:45 2015 +0000
+++ b/autoMode.h	Mon Oct 12 07:42:49 2015 +0000
@@ -7,8 +7,6 @@
 PID velocity_controller(9.0,5274.0,0.0,RATE);
 PID direction_controller(36.0,3.0,0.0,RATE);
 
-#define MAX_VALUE 93
-
 #ifdef IM920
 /***IM920 correspondence***/
 void autoIM920() {
@@ -59,6 +57,7 @@
             stateR = 17;
             stateL = 17;
             step   = 15;
+            CStep  = 15;
         }
     }
     else if(b==9){  /*stop*/
@@ -107,14 +106,14 @@
     else if(!a2){
         skip = 0;
     }
-    if(b!=7) edge7=1;
     if(b!=1) edge1=1;
     if(b!=2) edge2=1;
     if(b!=3) edge3=1;
     if(b!=4) edge4=1;
+    if(b!=5) edge5=1;
     if(b!=6) edge6=1;
+    if(b!=7) edge7=1;
     if(b!=8) edge8=1;
-    if(b!=5) edge5=1;
 }
 #else
 void autoPS3(){
@@ -127,35 +126,46 @@
         }
     }
     else if(up){  /*start*/
-        resetState();
-        flagf=1;
-        targ_velocity=speed;
-        sendData(7,defoR);
-        stateR = defoR;
-        stateL = defoL;
-        step   = 0;
-        CStep  = 1;
+        if(edge_up){
+            edge_up=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(right){  /*middle start*/
-        resetState();
-        flagf = 1;
-        targ_velocity=speed;
-        sendData(7,60);
-        stateR = 60;
-        stateL = 60;
-        step   = 5;
-        CStep  = 7;
+        if(edge_right){
+            edge_right=0;
+            resetState();
+            flagf = 1;
+            targ_velocity=speed;
+            sendData(7,61);
+            stateR = 61;
+            stateL = 61;
+            step   = 5;
+            CStep  = 6;
+        }
     }
     else if(down){   /*opponents start*/
-        resetState();
-        flagf = 1;
-        sita=3.0*(PI/4.0);
-        targ_velocity = speed;
-        targ_sita     = 3.0*(PI/4.0);
-        sendData(7,65);
-        stateR = 65;
-        stateL = 65;
-        step   = 8;
+        if(edge_down){
+            edge_down=0;
+            resetState();
+            flagf = 1;
+            targ_velocity = speed;
+            sendData(7,60);
+            stateR = 60;
+            stateL = 60;
+            step   = 15;
+            CStep  = 15;
+        }
     }
     else if(square){ /*reset*/
         sendData(7,255);