Diff: autoMode.h

Revision:

30:cd344beb415d

Parent:

29:460e49e37048

Child:

32:b8c8ad2eeca7

--- a/autoMode.h	Tue Nov 10 07:46:00 2015 +0000
+++ b/autoMode.h	Tue Nov 10 09:42:33 2015 +0000
@@ -6,35 +6,29 @@
 PID direction_controller(36.0,3.5,0.0,RATE);
 //PID direction_controller(36.0,3.0,0.0,RATE);
 
-Timeout OpStart;
-
-void OpponentsStart(){
-    spcount = 0.0;
-    step    = 15;
-    CStep   = 15;
-    flaga   = 1;
-    flagf   = 1;
-}
-
 /***IM920 correspondence***/
 void autoIM920() {
-    if(b==7){  /*mode change*/
+    if(b==7){  //mode change
         if(edge7) {
             edge7=0;
             autoflag=0;
             Indicator4=0;
             IndicatorAuto=1;
-            direction_controller.setTunings(12.0,9.0,0.0);
             targ_velocity=0.0;
+            resetState(0);
             flaga=0;
+            step=114;
+            CStep=114;
+            mstep=114;
+            mCStep=114;
             spcount=speed;
             dpcount=0.0;
         }
     }
-    else if((b==6)&&(!flaga)){ /*start*/
+    else if((b==6)&&(!flaga)){ //Own & Middle Start
         if(edge6){
             edge6=0;
-            resetState();
+            resetState(1);
             flagf=1;
             spcount=0.0;
             dpcount=0.0;
@@ -64,7 +58,44 @@
             stateL = 69;
 #endif
             step   = 0;
-            CStep  = 1;
+            CStep  = 0;
+            flaga  = 1;
+        }
+    }
+    else if((b==9)&&(!flaga)){ //Own & Opponent Start
+        if(edge9){
+            edge9=0;
+            resetState(1);
+            flagf=1;
+            spcount=0.0;
+            dpcount=0.0;
+            direction_controller.setBias(0.0);
+#ifdef BLUE
+            sendData(5,67); //right
+            wait(0.05);
+            sendData(4,67); //left
+            wait(0.05);
+            sendData(6,60); //middle
+            stateR = 69;
+            stateL = 69;
+            /*sendData(5,65); //right
+            wait(0.05);
+            sendData(4,65); //left
+            wait(0.05);
+            sendData(6,10); //middle
+            stateR = 69;
+            stateL = 69;*/
+#else       
+            sendData(5,69); //right
+            wait(0.05);
+            sendData(4,69); //left
+            wait(0.05);
+            sendData(6,69); //middle
+            stateR = 69;
+            stateL = 69;
+#endif
+            step   = 10;
+            CStep  = 10;
             flaga  = 1;
         }
     }
@@ -84,7 +115,9 @@
     if(b!=7) edge7=1;
     if(b!=8) edge8=1;
     if(b!=9) edge9=1;
+    if(b!=10) edge10=1;
+    if(b!=11) edge11=1;
 }
 
 
-#endif /*autoMode.h*/
+#endif /*autoMode.h*/
\ No newline at end of file