Diff: main_processing/setup_command_active/setup.cpp

Revision:

9:c966191926c5

Parent:

7:efe90188a49d

Child:

10:6df631c39f9b

--- a/main_processing/setup_command_active/setup.cpp	Sun Mar 06 15:43:26 2016 +0000
+++ b/main_processing/setup_command_active/setup.cpp	Tue Mar 08 09:52:22 2016 +0000
@@ -70,10 +70,10 @@
 void SetUp2(void){//スタートした時の処理
     
      __enable_irq(); // 許可
+    sys.KickOffFlag=1;
     
-    sys.KickOffFlag=1;
+    //solenoid
     sys.KickFlag=0;
-    //solenoid
     kicker=0;
     //compass
     hmc_reset=0;
@@ -100,7 +100,6 @@
     pidupdate.detach();
     Motor_ticker.detach();
     ////Line_ticker.detach();
-    Solenoid_ticker.detach();
     Ir_ticker.detach();
     Ping_ticker.detach();
     ////Hmc_ticker.detach();
@@ -109,6 +108,7 @@
     cmps_set.OutputPID=0;
     cmps_set.InputPID=REFERENCE;
     //solenoid
+    Solenoid_ticker.detach();
     kicker=1;
     wait(0.25);
     kicker=0;
@@ -118,3 +118,72 @@
     Sw_ticker.attach(Sw_sample, 0.050);
     
 }
+void SetKick(void){
+    if(sys.KickOffFlag==1){
+        //solenoid
+        sys.KickFlag=0;
+        kicker=0;
+        Solenoid_ticker.attach(&AvailableSolenoid, 6.0);
+    }
+    else{
+        //solenoid
+        Solenoid_ticker.detach();
+        kicker=1;
+        wait(0.25);
+        kicker=0;
+    }
+}
+void SetPidAndMotor(void){
+    if(sys.KickOffFlag==1){
+        //compass
+        hmc_reset=0;
+        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;
+        pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
+    }
+    else{
+        pidupdate.detach();
+        Motor_ticker.detach();
+        ////Hmc_ticker.detach();
+        motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
+        move(0,0,0);
+        cmps_set.OutputPID=0;
+        cmps_set.InputPID=REFERENCE;
+    }
+}
+void SetIr(void){
+    if(sys.KickOffFlag==1){
+        Ir_ticker.attach(&ValidIr, 0.050);
+    }
+    else{
+        Ir_ticker.detach();
+    }
+}
+void SetPing(void){
+    if(sys.KickOffFlag==1){
+        Ping_ticker.attach(&ValidPing, 0.050);
+    }
+    else{
+        Ping_ticker.detach();
+    }
+}
+void SetLine(void){
+    if(sys.KickOffFlag==1){
+        //Line_ticker.attach(&ReadLine, 0.005);
+        Line[A_SPOT].rise(&LineCall_A);
+        Line[B_SPOT].rise(&LineCall_B);
+        Line[C_SPOT].rise(&LineCall_C);
+        
+        //LineHolding[A_SPOT].rise(&LineCall_ABC);
+        //LineHolding[B_SPOT].rise(&LineCall_ABC);
+        //LineHolding[C_SPOT].rise(&LineCall_ABC);
+    }
+    else{
+        ////Line_ticker.detach();
+    }
+}
\ No newline at end of file