Diff: main_processing/setup_command_active/setup.cpp

Revision:

24:9fb74ea3c25d

Parent:

23:d95d8d3e89f3

Child:

25:f98a7c2a5970

--- a/main_processing/setup_command_active/setup.cpp	Sat Jan 30 10:08:47 2016 +0000
+++ b/main_processing/setup_command_active/setup.cpp	Sat Jan 30 16:05:58 2016 +0000
@@ -10,7 +10,7 @@
     Sw[1].mode(PullUp);
     Sw[2].mode(PullUp);
     Sw[3].mode(PullUp);
-    Sw_ticker.attach(Sw_sample, 0.1);
+    Sw_ticker.attach(Sw_sample, 0.050);
     
     Lcd.cls();
     Lcd.print(lcdstr[0][0]);
@@ -20,7 +20,8 @@
     
     //value
     data.strategy = 0;
-    
+    data.motorlog[X_AXIS] = 0;
+    data.motorlog[Y_AXIS] = 0;
     
     //spi
     spi.format(16, 3);
@@ -40,16 +41,40 @@
     data.CmpsDiff = REFERENCE - data.cmps;
     data.FrontDeg=0;
     
-    //pid.setInputLimits(MINIMUM,MAXIMUM);            //pid sed def
-    //pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);     //pid sed def
-    //pid.setBias(PID_BIAS);                          //pid sed def
-    //pid.setMode(AUTO_MODE);                         //pid sed def
-    //pid.setSetPoint(REFERENCE);                     //pid sed def
+    pid.setInputLimits(MINIMUM,MAXIMUM);            //pid sed def
+    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);     //pid sed def
+    pid.setBias(PID_BIAS);                          //pid sed def
+    pid.setMode(AUTO_MODE);                         //pid sed def
+    pid.setSetPoint(REFERENCE);                     //pid sed def
     //pidupdate.attach(&PidUpdate, PID_CYCLE);
 }
 void SetUp2(void){//スタートした時の処理
     data.strategy=0;
+    
+    data.motorlog[X_AXIS] = 0;
+    data.motorlog[Y_AXIS] = 0;
+    for(int i=0; i<5; i++){
+        ReadCmps();
+        data.CmpsInitialValue = data.cmps;
+        wait_ms(10);
+    }
+    data.CmpsDiff = REFERENCE - data.cmps;
+    data.FrontDeg=0;
+    
+    Line_ticker.attach(&ReadLine, 0.020);
+    Solenoid_ticker.attach(&AvailableSolenoid, 6.0);
+    Ir_ticker.attach(&ValidIr, 0.050);
+    //motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
+    Motor_ticker.attach(&tx_motor, 0.020);
+    pidupdate.attach(&PidUpdate, PID_CYCLE);
 }
 void StopProcess(void){//コマンドに戻るときの処理
-    
+    pidupdate.detach();
+    Motor_ticker.detach();
+    Line_ticker.detach();
+    Solenoid_ticker.detach();
+    Ir_ticker.detach();
+    DriveSolenoid();
+    motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
+    move(0,0,0);
 }