ジャパンオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers setup.cpp Source File

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     //ball
00017     BallChecker.mode(PullUp);
00018     
00019     //system,flag
00020     sys.strategy=0;
00021     sys.jump_flag=JUMP_TAG_MAX;//0<=x<=JUMP_TAG_NUM
00022     sys.stopflag=0;
00023     sys.KickOffFlag=0;
00024     sys.TurnStartFlag=0;
00025     sys.DribbleFlag=0;
00026     sys.MotorFlag=0;
00027     sys.InfoFlag=0;
00028     //sys.PidFlag=0;
00029     //motor 
00030     sys.pow_num = 1;
00031     sys.s_pow = ir_pow_val[sys.pow_num][POW_SHORT];
00032     sys.m_pow = ir_pow_val[sys.pow_num][POW_MIDDLE];
00033     sys.l_pow = ir_pow_val[sys.pow_num][POW_LONG];
00034     /*sys.s_pow = 30;
00035     sys.m_pow = 30;
00036     sys.l_pow = 30;*/
00037     sys.dri_pow = 100;
00038     sys.ir_pow_table = 0;
00039     
00040     ///////important
00041     ///Blindシリーズ.1を代入であらゆるセンサーなどの値を反映させないようにする.
00042     sys.IrBlind=0;
00043     sys.LineBlind=0;
00044     sys.PingBlind=0;
00045     
00046     sys.HomeBlind=1;
00047     sys.DriBlind=1;
00048     sys.DriMotorBlind=0;
00049     sys.TurnAtkBlind=0;
00050     sys.TurnDriBlind=0;
00051     sys.TurnHoldBlind=0;
00052     sys.KickBlind=0;
00053     //defence
00054     sys.DefenceFlag=0;
00055     ////////important
00056     
00057     //ir
00058     data.irNotice=IR_NONE;
00059     
00060     //spi
00061     spi.format(16, 3);
00062     spi.frequency(1000000);
00063     spi_ss[0]=spi_ss[1]=spi_ss[2]=spi_ss[3]=1;
00064     
00065     //motor
00066     motor.baud(115200);                             //ボーレート設定
00067     motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
00068     //compass
00069     hmc_reset=HMC_RUN;
00070     /*for(int i=0; i<5; i++){
00071         ReadCmps();
00072         cmps_set.CmpsInitialValue = cmps_set.cmps;
00073         wait_ms(10);
00074     }
00075     cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;*/
00076     for(int i=0; i<5; i++){
00077         ReadCmps();
00078         cmps_set.CmpsInitialValue = cmps_set.cmps;
00079         cmps_set.CmpsInitialValue0 = cmps_set.cmps;
00080         wait_ms(10);
00081     }
00082     cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
00083     
00084     cmps_set.FrontDeg=0;
00085     cmps_set.AtkDeg=0;
00086     
00087     pid.setInputLimits(MINIMUM,MAXIMUM);            //pid sed def
00088     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);     //pid sed def
00089     pid.setBias(PID_BIAS);                          //pid sed def
00090     pid.setMode(AUTO_MODE);                         //pid sed def
00091     pid.setSetPoint(REFERENCE);                     //pid sed def
00092     //pidupdate.attach(&PidUpdate, PID_CYCLE);
00093 }
00094 void SetUp2(void){//スタートした時の処理
00095      __enable_irq(); // 許可
00096      
00097     sys.KickOffFlag=1;
00098     SetKick();
00099     SetPidAndMotor();
00100     SetInfo();
00101     SetLine();
00102 }
00103 void StopProcess(void){//コマンドに戻るときの処理
00104 
00105     sys.KickOffFlag=0;
00106     SetKick();
00107     SetPidAndMotor();
00108     SetInfo();
00109     SetLine();
00110     
00111     __disable_irq(); // 禁止
00112     __enable_irq(); // 許可
00113     Sw_ticker.attach(Sw_sample, 0.050);
00114 }
00115 
00116 void SetKick(void){
00117     if(sys.KickOffFlag==1){
00118         kicker=0;
00119         ValidSolenoid();
00120         BallChecker.fall(&DriveSolenoidJudge);
00121     }
00122     else{
00123         kicker=1;
00124         wait(0.25);
00125         kicker=0;
00126     }
00127 }
00128 void SetPidAndMotor(void){
00129     if(sys.KickOffFlag==1){
00130         //compass
00131         ValidTurn();
00132         hmc_reset=HMC_RUN;
00133         if(sys.TurnStartFlag==1){
00134             //正...右回転
00135             //負...左回転
00136             //cmps_set.FrontDeg=-180;
00137             //cmps_set.FrontDeg=179;
00138             //cmps_set.FrontDeg=180;
00139             //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;
00140             
00141             /*for(int i=0; i<5; i++){
00142                 ReadCmps();
00143                 cmps_set.CmpsInitialValue2 = cmps_set.cmps;
00144                 wait_ms(10);
00145             }*/
00146             //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue2;
00147             
00148             cmps_set.FrontDeg=-180;//-(cmps_set.CmpsInitialValue2-cmps_set.CmpsInitialValue0);
00149         }
00150         else{
00151             for(int i=0; i<5; i++){
00152                 ReadCmps();
00153                 cmps_set.CmpsInitialValue = cmps_set.cmps;
00154                 wait_ms(10);
00155             }
00156             cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
00157             
00158             
00159             cmps_set.FrontDeg=0;
00160         }
00161         Motor_ticker.attach(&ValidTx_motor, 0.020);
00162         //pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
00163     }
00164     else{
00165         //pidupdate.detach();
00166         Motor_ticker.detach();
00167         ////Hmc_ticker.detach();
00168         motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
00169         move(0,0,0);
00170         cmps_set.OutputPID=0;
00171         cmps_set.InputPID=REFERENCE;
00172         HmcReset();
00173     }
00174 }
00175 void SetInfo(void){
00176     if(sys.KickOffFlag==1){
00177         Info_ticker.attach(&ValidInfo, 0.050);
00178     }
00179     else{
00180         Info_ticker.detach();
00181     }
00182 }
00183 void SetLine(void){
00184     if(sys.KickOffFlag==1){
00185         //Line_ticker.attach(&ReadLine, 0.005);
00186         /*Line[A_SPOT].rise(&LineCall_A);
00187         Line[B_SPOT].rise(&LineCall_B);
00188         Line[C_SPOT].rise(&LineCall_C);*/
00189         /*
00190         LineHolding[A_SPOT].rise(&LineRanking_A);
00191         LineHolding[B_SPOT].rise(&LineRanking_B);
00192         LineHolding[C_SPOT].rise(&LineRanking_C);
00193         */
00194         /*
00195         Line[A_SPOT].rise(&LineRawCall_A);
00196         Line[B_SPOT].rise(&LineRawCall_B);
00197         Line[C_SPOT].rise(&LineRawCall_C);
00198         */
00199         
00200         Line[A_SPOT].rise(&LineRawRanking_A);
00201         Line[B_SPOT].rise(&LineRawRanking_B);
00202         Line[C_SPOT].rise(&LineRawRanking_C);
00203     }
00204     else{
00205         ////Line_ticker.detach();
00206     }
00207 }