Diff: main_processing/setup_command_active/setup.cpp

Revision:

32:367b16d69a32

Parent:

30:5998ba42237e

Child:

34:1c86c1299ea4

--- a/main_processing/setup_command_active/setup.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/setup_command_active/setup.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -45,6 +45,10 @@
     
     sys.HomeBlind=1;
     sys.DriBlind=1;
+    sys.TurnAtkBlind=0;
+    sys.TurnDriBlind=0;
+    sys.TurnHoldBlind=0;
+    sys.KickBlind=0;
     //defence
     sys.DefenceFlag=0;
     ////////important
@@ -62,12 +66,20 @@
     motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     //compass
     hmc_reset=HMC_RUN;
-    for(int i=0; i<5; i++){
+    /*for(int i=0; i<5; i++){
         ReadCmps();
         cmps_set.CmpsInitialValue = cmps_set.cmps;
         wait_ms(10);
     }
-    cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;
+    cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;*/
+    for(int i=0; i<5; i++){
+        ReadCmps();
+        cmps_set.CmpsInitialValue = cmps_set.cmps;
+        cmps_set.CmpsInitialValue0 = cmps_set.cmps;
+        wait_ms(10);
+    }
+    cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
+    
     cmps_set.FrontDeg=0;
     cmps_set.AtkDeg=0;
     
@@ -104,6 +116,7 @@
     if(sys.KickOffFlag==1){
         kicker=0;
         ValidSolenoid();
+        BallChecker.fall(&DriveSolenoidJudge);
     }
     else{
         kicker=1;
@@ -116,21 +129,32 @@
         //compass
         ValidTurn();
         hmc_reset=HMC_RUN;
-        for(int i=0; i<5; i++){
-            ReadCmps();
-            cmps_set.CmpsInitialValue = cmps_set.cmps;
-            wait_ms(10);
-        }
-        cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
         if(sys.TurnStartFlag==1){
             //正．．．右回転
             //負．．．左回転
             //cmps_set.FrontDeg=-180;
             //cmps_set.FrontDeg=179;
             //cmps_set.FrontDeg=180;
-            cmps_set.FrontDeg=-180;
+            //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;
+            
+            /*for(int i=0; i<5; i++){
+                ReadCmps();
+                cmps_set.CmpsInitialValue2 = cmps_set.cmps;
+                wait_ms(10);
+            }*/
+            //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue2;
+            
+            cmps_set.FrontDeg=-180;//-(cmps_set.CmpsInitialValue2-cmps_set.CmpsInitialValue0);
         }
         else{
+            for(int i=0; i<5; i++){
+                ReadCmps();
+                cmps_set.CmpsInitialValue = cmps_set.cmps;
+                wait_ms(10);
+            }
+            cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
+            
+            
             cmps_set.FrontDeg=0;
         }
         Motor_ticker.attach(&ValidTx_motor, 0.020);