ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
main_processing/setup_command_active/setup.cpp
- Committer:
- lilac0112_1
- Date:
- 2016-03-27
- Revision:
- 38:67bc78f3c0ab
- Parent:
- 34:1c86c1299ea4
File content as of revision 38:67bc78f3c0ab:
#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); //ball BallChecker.mode(PullUp); //system,flag sys.strategy=0; sys.jump_flag=JUMP_TAG_MAX;//0<=x<=JUMP_TAG_NUM sys.stopflag=0; sys.KickOffFlag=0; sys.TurnStartFlag=0; sys.DribbleFlag=0; sys.MotorFlag=0; sys.InfoFlag=0; //sys.PidFlag=0; //motor sys.pow_num = 1; sys.s_pow = ir_pow_val[sys.pow_num][POW_SHORT]; sys.m_pow = ir_pow_val[sys.pow_num][POW_MIDDLE]; sys.l_pow = ir_pow_val[sys.pow_num][POW_LONG]; /*sys.s_pow = 30; sys.m_pow = 30; sys.l_pow = 30;*/ sys.dri_pow = 100; sys.ir_pow_table = 0; ///////important ///Blindシリーズ.1を代入であらゆるセンサーなどの値を反映させないようにする. sys.IrBlind=0; sys.LineBlind=0; sys.PingBlind=0; sys.HomeBlind=1; sys.DriBlind=1; sys.DriMotorBlind=0; sys.TurnAtkBlind=0; sys.TurnDriBlind=0; sys.TurnHoldBlind=0; sys.KickBlind=0; //defence sys.DefenceFlag=0; ////////important //ir data.irNotice=IR_NONE; //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"); //モータ停止 //compass 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.CmpsInitialValue;*/ for(int i=0; i<5; i++){ ReadCmps(); cmps_set.CmpsInitialValue = cmps_set.cmps; cmps_set.CmpsInitialValue0 = cmps_set.cmps; wait_ms(10); } cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps; cmps_set.FrontDeg=0; cmps_set.AtkDeg=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; SetKick(); SetPidAndMotor(); SetInfo(); SetLine(); } void StopProcess(void){//コマンドに戻るときの処理 sys.KickOffFlag=0; SetKick(); SetPidAndMotor(); SetInfo(); SetLine(); __disable_irq(); // 禁止 __enable_irq(); // 許可 Sw_ticker.attach(Sw_sample, 0.050); } void SetKick(void){ if(sys.KickOffFlag==1){ kicker=0; ValidSolenoid(); BallChecker.fall(&DriveSolenoidJudge); } else{ kicker=1; wait(0.25); kicker=0; } } void SetPidAndMotor(void){ if(sys.KickOffFlag==1){ //compass ValidTurn(); hmc_reset=HMC_RUN; if(sys.TurnStartFlag==1){ //正...右回転 //負...左回転 //cmps_set.FrontDeg=-180; //cmps_set.FrontDeg=179; //cmps_set.FrontDeg=180; //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue; /*for(int i=0; i<5; i++){ ReadCmps(); cmps_set.CmpsInitialValue2 = cmps_set.cmps; wait_ms(10); }*/ //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue2; cmps_set.FrontDeg=-180;//-(cmps_set.CmpsInitialValue2-cmps_set.CmpsInitialValue0); } else{ 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; } Motor_ticker.attach(&ValidTx_motor, 0.020); //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; HmcReset(); } } void SetInfo(void){ if(sys.KickOffFlag==1){ Info_ticker.attach(&ValidInfo, 0.050); } else{ Info_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(&LineRanking_A); LineHolding[B_SPOT].rise(&LineRanking_B); LineHolding[C_SPOT].rise(&LineRanking_C); */ /* Line[A_SPOT].rise(&LineRawCall_A); Line[B_SPOT].rise(&LineRawCall_B); Line[C_SPOT].rise(&LineRawCall_C); */ Line[A_SPOT].rise(&LineRawRanking_A); Line[B_SPOT].rise(&LineRawRanking_B); Line[C_SPOT].rise(&LineRawRanking_C); } else{ ////Line_ticker.detach(); } }