Diff: manualMode.h

Revision:

36:b8954b13a6d5

Parent:

34:aa2a5c888a27

Child:

37:75fcd28f48c7

--- a/manualMode.h	Sat Nov 14 04:05:13 2015 +0000
+++ b/manualMode.h	Sun Nov 15 00:30:44 2015 +0000
@@ -3,22 +3,6 @@
 
 void manualIM920()
 {
-    /*if(b==7){  //mode change
-        if(edge7){
-            edge7=0;
-            autoflag=1;
-            Indicator4=1;
-            IndicatorAuto=0;
-            resetState(1);
-            flaga=0;
-            step=114;
-            CStep=114;
-            mstep=114;
-            mCStep=114;
-            spcount=speed;
-            dpcount=0.0;
-        }
-    }*/
     if(modeflag){  //mode change
         autoflag=1;
         Indicator4=1;
@@ -31,6 +15,7 @@
         mCStep=114;
         spcount=speed;
         dpcount=0.0;
+        swingmoved=0;
     }
     if(b==9){
         if(edge9){
@@ -39,7 +24,7 @@
             if(toggle9){
 //              swingspeed=15.0;
 //                swingspeed=20.0;
-                swingspeed=25.0;
+//                swingspeed=5.0;
                 resetSwingSpeed = 1;
                 interruptSwingSpeed.attach(&countSwingSpeed, 0.05);
                 enableSwing=1;
@@ -66,7 +51,7 @@
             else sendData(1,12);
         }
     }
-    else if((b==5)&&(!flaga)){ //Swing Start
+    else if((b==5)&&(!flaga)){ //Swing Start(middle)
         if(edge5){
             edge5=0;
             resetState(0);
@@ -84,38 +69,47 @@
             mstep   = 0;
             mCStep  = 114;
             flaga  = 1;
+            swingspeed = 18.0;
         }
     }
-    else if((b==8)&&(!flaga)){
+    else if((b==8)&&(!flaga)){ //Swing Start(over)
         if(edge8){
             edge8=0;
-            resetState(1);
-            flagf=1;
+            resetState(0);
+            flagf=0;
             spcount=0.0;
             dpcount=0.0;
             direction_controller.setBias(0.0);
-#ifdef BLUE
             sendData(5,60); //right
             wait(0.05);
             sendData(4,60); //left
             wait(0.05);
-            sendData(6,10); //middle
-            stateR = 69;
-            stateL = 69;
-#else       
-            sendData(5,60); //right
-            wait(0.05);
-            sendData(4,60); //left
-            wait(0.05);
-            sendData(6,10); //middle
-            stateR = 69;
-            stateL = 69;
-#endif
+            sendData(6,30); //middle
             stateR = 90;
             stateL = 90;
-            mstep  = 10;
-            mCStep = 0;
+            mstep   = 10;
+            mCStep  = 114;
             flaga  = 1;
+            swingspeed = 19.5;  //入った
+//            swingspeed = 19.3;
+        }
+    }
+    
+    else if((b==7)&&(swingmoved!=0)){
+        if(edge7){
+            edge7=0;
+            if(swingmoved==1){
+                mstep=2;
+                swingmoved=0;
+            }
+            else if(swingmoved==2){
+                mstep=12;
+                swingmoved=0;
+            }
+            else if(swingmoved==3){
+                mstep=22;
+                swingmoved=0;
+            }
         }
     }
     //Move