ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
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 }
Generated on Wed Jul 13 2022 02:59:03 by 1.7.2