setup.cpp

00001 #include "mbed.h"
00002 #include "extern.h"
00003 
00004 
00005 void SetUp(void){//電源を入れた時の処理
00006     //int i=1;
00007     //solenoid
00008     kicker=0;
00009     //sw&lcd
00010     Sw[0].mode(PullUp);
00011     Sw[1].mode(PullUp);
00012     Sw[2].mode(PullUp);
00013     Sw[3].mode(PullUp);
00014     Sw_ticker.attach(Sw_sample, 0.050);
00015     
00016     Lcd.cls();
00017     Lcd.print(lcdstr[0][0]);
00018     
00019     //ball
00020     BallChecker.mode(PullUp);
00021     
00022     //value
00023     data.strategy = 0;
00024     data.motorlog[X_AXIS] = 0;
00025     data.motorlog[Y_AXIS] = 0;
00026     data.s_pow=30;
00027     data.l_pow=30;
00028     
00029     //spi
00030     spi.format(16, 3);
00031     spi.frequency(1000000);
00032     spi_ss[0]=spi_ss[1]=spi_ss[2]=spi_ss[3]=1;
00033     
00034     //motor
00035     motor.baud(115200);                             //ボーレート設定
00036     motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
00037     //motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
00038     //compass
00039     RN42_Reset=0;
00040     for(int i=0; i<5; i++){
00041         ReadCmps();
00042         data.CmpsInitialValue = data.cmps;
00043         wait_ms(10);
00044     }
00045     data.CmpsDiff = REFERENCE - data.cmps;
00046     data.FrontDeg=0;
00047     
00048     pid.setInputLimits(MINIMUM,MAXIMUM);            //pid sed def
00049     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);     //pid sed def
00050     pid.setBias(PID_BIAS);                          //pid sed def
00051     pid.setMode(AUTO_MODE);                         //pid sed def
00052     pid.setSetPoint(REFERENCE);                     //pid sed def
00053     //pidupdate.attach(&PidUpdate, PID_CYCLE);
00054 }
00055 void SetUp2(void){//スタートした時の処理
00056     
00057      __enable_irq(); // 許可
00058     
00059     data.KickOffFlag=1;
00060     data.KickFlag=0;
00061     //solenoid
00062     kicker=0;
00063     //motorsum
00064     data.motorlog[X_AXIS] = 0;
00065     data.motorlog[Y_AXIS] = 0;
00066     //compass
00067     RN42_Reset=0;
00068     for(int i=0; i<5; i++){
00069         ReadCmps();
00070         data.CmpsInitialValue = data.cmps;
00071         wait_ms(10);
00072     }
00073     data.CmpsDiff = REFERENCE - data.cmps;
00074     data.FrontDeg=0;
00075     
00076     //Line_ticker.attach(&ReadLine, 0.005);
00077     Line[A_SPOT].rise(&LineCall_A);
00078     Line[B_SPOT].rise(&LineCall_B);
00079     Line[C_SPOT].rise(&LineCall_C);
00080     Solenoid_ticker.attach(&AvailableSolenoid, 6.0);
00081     Ir_ticker.attach(&ValidIr, 0.050);
00082     Ping_ticker.attach(&ValidPing, 0.050);
00083     //Hmc_ticker.attach(&HmcReset, 1.0);
00084     //motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
00085     Motor_ticker.attach(&ValidTx_motor, 0.020);
00086     pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
00087 }
00088 void StopProcess(void){//コマンドに戻るときの処理
00089     pidupdate.detach();
00090     Motor_ticker.detach();
00091     ////Line_ticker.detach();
00092     Solenoid_ticker.detach();
00093     Ir_ticker.detach();
00094     Ping_ticker.detach();
00095     ////Hmc_ticker.detach();
00096     motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
00097     move(0,0,0);
00098     data.OutputPID=0;
00099     data.InputPID=REFERENCE;
00100     //solenoid
00101     kicker=1;
00102     wait(0.25);
00103     kicker=0;
00104     
00105     __disable_irq(); // 禁止
00106     __enable_irq(); // 許可
00107     Sw_ticker.attach(Sw_sample, 0.050);
00108     
00109 }