Diff: main_processing/setup_command_active/setup.cpp

Revision:

10:6df631c39f9b

Parent:

9:c966191926c5

Child:

16:4fadb7a87497

--- a/main_processing/setup_command_active/setup.cpp	Tue Mar 08 09:52:22 2016 +0000
+++ b/main_processing/setup_command_active/setup.cpp	Thu Mar 10 03:03:50 2016 +0000
@@ -21,12 +21,13 @@
     sys.jump_flag=JUMP_TAG_MAX;//0<=x<=JUMP_TAG_NUM
     sys.stopflag=0;
     sys.KickOffFlag=0;
+    sys.TurnStartFlag=0;
     sys.DribbleFlag=0;
     sys.KickFlag=0;
     sys.MotorFlag=0;
     sys.IrFlag=0;
     sys.UswFlag=0;
-    sys.UswFlag_2=0;
+    sys.UswFlag2=0;
     sys.PidFlag=0;
     //motor 
     sys.pow_num = 1;
@@ -36,7 +37,7 @@
     /*sys.s_pow = 30;
     sys.m_pow = 30;
     sys.l_pow = 30;*/
-    sys.dri_pow = 30;
+    sys.dri_pow = 50;
     sys.ir_pow_table = 0;
     
     //ir
@@ -51,7 +52,7 @@
     motor.baud(115200);                             //ボーレート設定
     motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     //compass
-    hmc_reset=0;
+    hmc_reset=HMC_RUN;
     for(int i=0; i<5; i++){
         ReadCmps();
         cmps_set.CmpsInitialValue = cmps_set.cmps;
@@ -72,11 +73,17 @@
      __enable_irq(); // 許可
     sys.KickOffFlag=1;
     
+    SetKick();
+    SetPidAndMotor();
+    SetIr();
+    SetPing();
+    SetLine();
+    /*
     //solenoid
     sys.KickFlag=0;
     kicker=0;
     //compass
-    hmc_reset=0;
+    hmc_reset=HMC_RUN;
     for(int i=0; i<5; i++){
         ReadCmps();
         cmps_set.CmpsInitialValue = cmps_set.cmps;
@@ -95,8 +102,18 @@
     //Hmc_ticker.attach(&HmcReset, 1.0);
     Motor_ticker.attach(&ValidTx_motor, 0.020);
     pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
+    */
 }
 void StopProcess(void){//コマンドに戻るときの処理
+
+    sys.KickOffFlag=0;
+    
+    SetKick();
+    SetPidAndMotor();
+    SetIr();
+    SetPing();
+    SetLine();
+    /*
     pidupdate.detach();
     Motor_ticker.detach();
     ////Line_ticker.detach();
@@ -112,6 +129,7 @@
     kicker=1;
     wait(0.25);
     kicker=0;
+    */
     
     __disable_irq(); // 禁止
     __enable_irq(); // 許可
@@ -136,14 +154,21 @@
 void SetPidAndMotor(void){
     if(sys.KickOffFlag==1){
         //compass
-        hmc_reset=0;
+        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;
-        cmps_set.FrontDeg=0;
+        if(sys.TurnStartFlag==1){
+            //cmps_set.FrontDeg=-180;
+            cmps_set.FrontDeg=179;
+        }
+        else{
+            cmps_set.FrontDeg=0;
+        }
+        FrontHere();
+        Motor_ticker.attach(&ValidTx_motor, 0.020);
         pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
     }
     else{
@@ -167,9 +192,11 @@
 void SetPing(void){
     if(sys.KickOffFlag==1){
         Ping_ticker.attach(&ValidPing, 0.050);
+        Ping_ticker2.attach(&ValidPing2, 0.050);
     }
     else{
         Ping_ticker.detach();
+        Ping_ticker2.detach();
     }
 }
 void SetLine(void){
@@ -186,4 +213,4 @@
     else{
         ////Line_ticker.detach();
     }
-}
\ No newline at end of file
+}