ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/setup_command_active/setup.cpp
- Revision:
- 0:ea35c18c85fc
- Child:
- 2:635947de1583
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main_processing/setup_command_active/setup.cpp Sat Feb 27 09:14:37 2016 +0000 @@ -0,0 +1,104 @@ +#include "mbed.h" +#include "extern.h" + + +void SetUp(void){//電源を入れた時の処理 + //int i=1; + //solenoid + kicker=0; + //sw&lcd + Sw[0].mode(PullUp); + Sw[1].mode(PullUp); + Sw[2].mode(PullUp); + Sw[3].mode(PullUp); + Sw_ticker.attach(Sw_sample, 0.050); + + Lcd.cls(); + Lcd.print(lcdstr[0][0]); + + //ball + BallChecker.mode(PullUp); + + //value + sys.strategy = 0; + sys.s_pow=30; + sys.l_pow=30; + + //spi + spi.format(16, 3); + spi.frequency(1000000); + spi_ss[0]=spi_ss[1]=spi_ss[2]=spi_ss[3]=1; + + //motor + motor.baud(115200); //ボーレート設定 + motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 + //motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み(モータ用) + //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; + + 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){//スタートした時の処理 + + __enable_irq(); // 許可 + + sys.KickOffFlag=1; + sys.KickFlag=0; + //solenoid + kicker=0; + //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; + + //Line_ticker.attach(&ReadLine, 0.005); + Line[A_SPOT].rise(&LineCall_A); + Line[B_SPOT].rise(&LineCall_B); + Line[C_SPOT].rise(&LineCall_C); + Solenoid_ticker.attach(&AvailableSolenoid, 6.0); + Ir_ticker.attach(&ValidIr, 0.050); + Ping_ticker.attach(&ValidPing, 0.050); + //Hmc_ticker.attach(&HmcReset, 1.0); + //motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み(モータ用) + Motor_ticker.attach(&ValidTx_motor, 0.020); + pidupdate.attach(&ValidPidUpdate, PID_CYCLE); +} +void StopProcess(void){//コマンドに戻るときの処理 + pidupdate.detach(); + Motor_ticker.detach(); + ////Line_ticker.detach(); + Solenoid_ticker.detach(); + Ir_ticker.detach(); + Ping_ticker.detach(); + ////Hmc_ticker.detach(); + motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 + move(0,0,0); + cmps_set.OutputPID=0; + cmps_set.InputPID=REFERENCE; + //solenoid + kicker=1; + wait(0.25); + kicker=0; + + __disable_irq(); // 禁止 + __enable_irq(); // 許可 + Sw_ticker.attach(Sw_sample, 0.050); + +}