CatPot 2015-2016
/
CatPot_Main_T_2v00
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed AQM1602 HMC6352 PID
Embed:
(wiki syntax)
Show/hide line numbers
strategy.cpp
00001 #include "mbed.h" 00002 #include "extern.h" 00003 00004 //Atk 00005 void modeAttack0(void){ 00006 double ir_x, ir_y; 00007 int vx,vy,vs; 00008 uint8_t LineDir[4]; 00009 uint8_t LineStop[2]; 00010 //uint8_t IrRange[4]; 00011 //uint8_t LineBind[4]; 00012 if(data.IrFlag==1){ 00013 ReadIr(); 00014 data.IrFlag=0; 00015 } 00016 if(data.PidFlag==1){ 00017 Line_ticker.detach(); 00018 PidUpdate(); 00019 Line_ticker.attach(&ReadLine, 0.005); 00020 data.PidFlag=0; 00021 } 00022 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X]; 00023 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y]; 00024 if(data.irPosition<8){ 00025 ir_x *= (double)(data.l_pow); 00026 ir_y *= (double)(data.l_pow); 00027 } 00028 else{ 00029 ir_x *= (double)(data.s_pow); 00030 ir_y *= (double)(data.s_pow); 00031 } 00032 //Lineを考慮していないIrのみの値 00033 vx = ir_x; 00034 vy = ir_y; 00035 //Line検出方向を調べる 00036 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0)))); 00037 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1)))); 00038 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))); 00039 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))); 00040 00041 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT]; 00042 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT]; 00043 00044 //Ir 00045 /* 00046 //ForLineAll 00047 data.motorlog[X_AXIS] *= LineStop[X_AXIS]; 00048 data.motorlog[Y_AXIS] *= LineStop[Y_AXIS]; 00049 data.motorlog[X_AXIS] += vx; 00050 data.motorlog[Y_AXIS] += vy; 00051 */ 00052 move( 00053 vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)), 00054 vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)), 00055 vs 00056 ); 00057 if(data.MotorFlag==1){ 00058 tx_motor(); 00059 data.MotorFlag=0; 00060 } 00061 return; 00062 } 00063 uint8_t HmcResetFlag; 00064 void HmcReset(void){ 00065 RN42_Reset=1; 00066 wait_us(100); 00067 RN42_Reset=0; 00068 } 00069 uint8_t PingFlag; 00070 void ValidPing(void){ 00071 if(PingFlag==0){ 00072 PingFlag=1; 00073 } 00074 } 00075 uint8_t LineReverseFlag; 00076 void LineReverse(void){ 00077 LineReverseFlag=0; 00078 } 00079 uint8_t LineSign[3]; 00080 uint8_t LineFirst[2]; 00081 uint8_t LinePriority[2];//0を後に，1を優先 00082 void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;} 00083 void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;} 00084 void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;} 00085 void LineCall_A(void){ 00086 // 00087 LineSign[A_SPOT] = 1; 00088 if(LineSign[B_SPOT]==0){ 00089 LineFirst[X_AXIS] = A_SPOT; 00090 } 00091 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){ 00092 if(LineSign[C_SPOT]==0){ 00093 LineFirst[Y_AXIS] = AB_SPOT; 00094 } 00095 } 00096 // 00097 if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1; 00098 // 00099 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY); 00100 } 00101 void LineCall_B(void){ 00102 // 00103 LineSign[B_SPOT] = 1; 00104 if(LineSign[A_SPOT]==0){ 00105 LineFirst[X_AXIS] = B_SPOT; 00106 } 00107 if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){ 00108 if(LineSign[C_SPOT]==0){ 00109 LineFirst[Y_AXIS] = AB_SPOT; 00110 } 00111 } 00112 // 00113 if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1; 00114 // 00115 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY); 00116 } 00117 void LineCall_C(void){ 00118 // 00119 LineSign[C_SPOT] = 1; 00120 if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){ 00121 LineFirst[Y_AXIS] = C_SPOT; 00122 } 00123 // 00124 if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1; 00125 // 00126 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY); 00127 } 00128 void modeAttack1(void){ 00129 double ir_x, ir_y; 00130 int vx,vy,vs; 00131 uint8_t LineDir[4]; 00132 uint8_t LineStop[2]; 00133 uint8_t IrRange[4]; 00134 uint8_t LinePulse[4]; 00135 uint8_t static LineBind[4]; 00136 if(data.KickOffFlag==1){ 00137 LineBind[0]=0; 00138 LineBind[1]=0; 00139 LineBind[2]=0; 00140 LineBind[3]=0; 00141 LineReverseFlag=0; 00142 00143 LineSign[A_SPOT]=0; 00144 LineSign[B_SPOT]=0; 00145 LineSign[C_SPOT]=0; 00146 00147 data.lnFlag[A_SPOT]=0; 00148 data.lnFlag[B_SPOT]=0; 00149 data.lnFlag[C_SPOT]=0; 00150 00151 data.KickOffFlag=0; 00152 } 00153 if(data.IrFlag==1){ 00154 ReadIr(); 00155 data.IrFlag=0; 00156 } 00157 if(data.PidFlag==1){ 00158 //Line_ticker.detach(); 00159 PidUpdate(); 00160 //Line_ticker.attach(&ReadLine, 0.005); 00161 data.PidFlag=0; 00162 } 00163 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X]; 00164 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y]; 00165 if(data.irPosition<8){ 00166 ir_x *= data.l_pow; 00167 ir_y *= data.l_pow; 00168 } 00169 else{ 00170 ir_x *= data.s_pow; 00171 ir_y *= data.s_pow; 00172 } 00173 00174 //Lineを考慮していないIrのみの値 00175 vx = ir_x; 00176 vy = ir_y; 00177 //Line検出方向を調べる 00178 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))&&(1))); 00179 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1))); 00180 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(1))); 00181 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1))); 00182 00183 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT]; 00184 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT]; 00185 00186 //Ir 00187 //strict 00188 /* 00189 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10)) 00190 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1)); 00191 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5)); 00192 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7)); 00193 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3)); 00194 */ 00195 //sweet 00196 IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9)) 00197 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1)); 00198 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5)); 00199 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7)); 00200 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3)); 00201 00202 00203 LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0)); 00204 LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0)); 00205 LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0)); 00206 LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0)); 00207 00208 LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0); 00209 LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0); 00210 LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0); 00211 LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0); 00212 /* 00213 LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1))); 00214 LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1))); 00215 LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1))); 00216 LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1))); 00217 */ 00218 vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)); 00219 vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)); 00220 vs = data.OutputPID; 00221 if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){ 00222 if(RawLineSignal>0){ 00223 vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]); 00224 vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]); 00225 /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT)); 00226 vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT)); 00227 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY); 00228 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY); 00229 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);*/ 00230 } 00231 else{ 00232 vx=0; 00233 vy=0; 00234 } 00235 } 00236 if(RawLineSignal>0){ 00237 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY); 00238 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY); 00239 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY); 00240 } 00241 move( 00242 vx, 00243 vy, 00244 vs 00245 ); 00246 if(data.MotorFlag==1){ 00247 tx_motor(); 00248 data.MotorFlag=0; 00249 } 00250 return; 00251 } 00252 void modeAttack2(void){ 00253 double ir_x, ir_y; 00254 int vx,vy,vs, LineForce[2]; 00255 uint8_t LineDir[4]; 00256 uint8_t LineOn[4]; 00257 uint8_t LineReturn[4]; 00258 uint8_t LineStop[2]; 00259 uint8_t IrRange[4]; 00260 uint8_t static LineBind[4]; 00261 //buint8_t static spi_count; 00262 if(data.KickOffFlag==1){ 00263 LineBind[0]=0; 00264 LineBind[1]=0; 00265 LineBind[2]=0; 00266 LineBind[3]=0; 00267 LineReverseFlag=0; 00268 00269 LineSign[A_SPOT]=0; 00270 LineSign[B_SPOT]=0; 00271 LineSign[C_SPOT]=0; 00272 00273 data.lnFlag[A_SPOT]=0; 00274 data.lnFlag[B_SPOT]=0; 00275 data.lnFlag[C_SPOT]=0; 00276 00277 HmcResetFlag = 0; 00278 PingFlag = 0; 00279 //spi_count=0; 00280 00281 RN42_Reset=0; 00282 data.KickFlag = 0; 00283 00284 data.KickOffFlag=0; 00285 //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート 00286 } 00287 if(data.IrFlag==1){ 00288 /*spi_count++; 00289 if(spi_count%10 == 0){ 00290 ReadPing(); 00291 } 00292 else{ 00293 ReadIr(); 00294 } 00295 if(spi_count==20) spi_count=0; 00296 */ 00297 ReadIr(); 00298 data.IrFlag=0; 00299 } 00300 if(data.PidFlag==1){ 00301 PidUpdate(); 00302 data.PidFlag=0; 00303 } 00304 if(PingFlag==1){ 00305 ReadPing(); 00306 PingFlag=0; 00307 } 00308 /* 00309 if(HmcResetFlag==1){ 00310 HmcReset(); 00311 HmcResetFlag=0; 00312 } 00313 */ 00314 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X]; 00315 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y]; 00316 if(data.irPosition<8){ 00317 ir_x *= data.l_pow; 00318 ir_y *= data.l_pow; 00319 } 00320 else{ 00321 ir_x *= data.s_pow; 00322 ir_y *= data.s_pow; 00323 } 00324 00325 //Lineを考慮していないIrのみの値 00326 vx = ir_x; 00327 vy = ir_y; 00328 00329 if((data.irPosition==10)&&(vy>0)){ 00330 vy += 0;//前進加速 00331 } 00332 if((data.irPosition==11)&&(vy>0)){ 00333 vy += 0;//前進加速 00334 if(data.KickFlag==1){ 00335 DriveSolenoid(); 00336 } 00337 } 00338 if((data.irPosition==12)&&(vy>0)){ 00339 vy += 0;//前進加速 00340 } 00341 if((data.irPosition==1)&&(vy>0)){ 00342 vy += 0;//前進加速 00343 } 00344 if((data.irPosition==2)&&(vy>0)){ 00345 vy += 0;//前進加速 00346 } 00347 /*if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){ 00348 vx *= -1.0;//背後回り込みの左右判断 00349 }*/ 00350 00351 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){ 00352 vx *= -1.0;//背後回り込みの左右判断 00353 } 00354 /* 00355 if((data.InputPID<(REFERENCE-30))||(data.InputPID>(REFERENCE+30))){ 00356 vx = vx*(0.75); 00357 vy = vy*(0.75); 00358 } 00359 */ 00360 //Lineを踏み始めた方向を調べる 00361 /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT); 00362 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT); 00363 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT); 00364 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/ 00365 00366 LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT); 00367 LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT); 00368 LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT); 00369 LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT); 00370 00371 //Irボールの方向 00372 //strict 00373 00374 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10)) 00375 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1)); 00376 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5)); 00377 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7)); 00378 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3)); 00379 00380 //sweet 00381 /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9)) 00382 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1)); 00383 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5)); 00384 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7)); 00385 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/ 00386 //none 00387 if(data.irNotice==IR_NONE){ 00388 IrRange[A_SPOT] = 0; 00389 IrRange[B_SPOT] = 0; 00390 IrRange[C_SPOT] = 0; 00391 IrRange[AB_SPOT] = 0; 00392 } 00393 //白線を踏み始めた方向とボールの方向が一致．(SelfHold) 00394 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1)); 00395 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1)); 00396 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1)); 00397 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1)); 00398 00399 LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0); 00400 LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0); 00401 00402 //白線踏んでる 00403 if(RawLineSignal>0){ 00404 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT); 00405 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT); 00406 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT); 00407 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT); 00408 //外側に向かう力を消す． 00409 //x 00410 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){ 00411 vx=0; 00412 //yの力を加える． 00413 if(vy>0){vy += 10;} 00414 if(vy<0){vy -= 10;} 00415 } 00416 //y 00417 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){ 00418 vy=0; 00419 } 00420 //内側に向かう力を加える． 00421 LineReturn[A_SPOT] = (LineOn[A_SPOT]==1); 00422 if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){ 00423 if(LineOn[C_SPOT]==0){ 00424 LineReturn[A_SPOT]=0; 00425 } 00426 else{ 00427 LineReturn[A_SPOT]=1; 00428 } 00429 } 00430 00431 LineReturn[B_SPOT] = (LineOn[B_SPOT]==1); 00432 if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){ 00433 if(LineOn[C_SPOT]==0){ 00434 LineReturn[B_SPOT]=0; 00435 } 00436 else{ 00437 LineReturn[B_SPOT]=1; 00438 } 00439 } 00440 LineReturn[C_SPOT] = (LineOn[C_SPOT]==1); 00441 if(LineReturn[C_SPOT]==1){ 00442 LineReturn[A_SPOT]=0; 00443 LineReturn[B_SPOT]=0; 00444 } 00445 LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1); 00446 00447 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + 00448 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0) 00449 *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) + 00450 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0) 00451 *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)); 00452 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1)); 00453 00454 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY); 00455 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY); 00456 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY); 00457 } 00458 else{ 00459 LineForce[X_AXIS] = 0; 00460 LineForce[Y_AXIS] = 0; 00461 } 00462 00463 vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS]; 00464 vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS]; 00465 vs = data.OutputPID; 00466 move( 00467 vx, 00468 vy, 00469 vs 00470 ); 00471 /*move( 00472 0, 00473 0, 00474 10 00475 );*/ 00476 data.motorlog[X_AXIS] += vx; 00477 data.motorlog[Y_AXIS] += vy; 00478 if(data.MotorFlag==1){ 00479 tx_motor(); 00480 data.MotorFlag=0; 00481 } 00482 return; 00483 } 00484 void modeAttack3(void){ 00485 double ir_x, ir_y; 00486 int vx,vy,vs, LineForce[2]; 00487 uint8_t LineDir[4]; 00488 uint8_t LineOn[4]; 00489 uint8_t LineReturn[4]; 00490 uint8_t LineStop[2]; 00491 uint8_t IrRange[4]; 00492 uint8_t static LineBind[4]; 00493 //buint8_t static spi_count; 00494 if(data.KickOffFlag==1){ 00495 LineBind[0]=0; 00496 LineBind[1]=0; 00497 LineBind[2]=0; 00498 LineBind[3]=0; 00499 LineReverseFlag=0; 00500 00501 LineSign[A_SPOT]=0; 00502 LineSign[B_SPOT]=0; 00503 LineSign[C_SPOT]=0; 00504 00505 data.lnFlag[A_SPOT]=0; 00506 data.lnFlag[B_SPOT]=0; 00507 data.lnFlag[C_SPOT]=0; 00508 00509 LinePriority[X_AXIS]=0; 00510 LinePriority[Y_AXIS]=0; 00511 00512 HmcResetFlag = 0; 00513 PingFlag = 0; 00514 //spi_count=0; 00515 00516 RN42_Reset=0; 00517 data.KickFlag = 0; 00518 00519 data.KickOffFlag=0; 00520 //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート 00521 } 00522 if(data.IrFlag==1){ 00523 /*spi_count++; 00524 if(spi_count%10 == 0){ 00525 ReadPing(); 00526 } 00527 else{ 00528 ReadIr(); 00529 } 00530 if(spi_count==20) spi_count=0; 00531 */ 00532 ReadIr(); 00533 data.IrFlag=0; 00534 } 00535 if(data.PidFlag==1){ 00536 PidUpdate(); 00537 data.PidFlag=0; 00538 } 00539 if(PingFlag==1){ 00540 ReadPing(); 00541 PingFlag=0; 00542 } 00543 /* 00544 if(HmcResetFlag==1){ 00545 HmcReset(); 00546 HmcResetFlag=0; 00547 } 00548 */ 00549 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X]; 00550 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y]; 00551 if(data.irPosition<8){ 00552 ir_x *= data.l_pow; 00553 ir_y *= data.l_pow; 00554 } 00555 else{ 00556 ir_x *= data.s_pow; 00557 ir_y *= data.s_pow; 00558 } 00559 00560 //Lineを考慮していないIrのみの値 00561 vx = ir_x; 00562 vy = ir_y; 00563 00564 if((data.irPosition==10)&&(vy>0)){ 00565 vy += 15;//前進加速 00566 } 00567 if((data.irPosition==11)&&(vy>0)){ 00568 vy += 15;//前進加速 00569 if(data.KickFlag==1){ 00570 DriveSolenoid(); 00571 } 00572 } 00573 if((data.irPosition==12)&&(vy>0)){ 00574 vy += 15;//前進加速 00575 } 00576 if((data.irPosition==1)&&(vy>0)){ 00577 vy += 25;//前進加速 00578 } 00579 if((data.irPosition==2)&&(vy>0)){ 00580 vy += 25;//前進加速 00581 } 00582 /*if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){ 00583 vx *= -1.0;//背後回り込みの左右判断 00584 }*/ 00585 00586 if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){ 00587 vx *= -1.0;//背後回り込みの左右判断 00588 } 00589 /* 00590 if((data.InputPID<(REFERENCE-30))||(data.InputPID>(REFERENCE+30))){ 00591 vx = vx*(0.75); 00592 vy = vy*(0.75); 00593 } 00594 */ 00595 //Lineを踏み始めた方向を調べる 00596 /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT); 00597 LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT); 00598 LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT); 00599 LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/ 00600 00601 LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT); 00602 LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT); 00603 LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT); 00604 LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT); 00605 00606 //Irボールの方向 00607 //strict 00608 00609 IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10)) 00610 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1)); 00611 IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5)); 00612 IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7)); 00613 IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||(( 1)&&(data.irPosition<=3)); 00614 00615 //sweet 00616 /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9)) 00617 ||((6<=data.irPosition)&&(data.irPosition<=7))||(( 1)&&(data.irPosition<=1)); 00618 IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5)); 00619 IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7)); 00620 IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||(( 1)&&(data.irPosition<=3));*/ 00621 //none 00622 if(data.irNotice==IR_NONE){ 00623 IrRange[A_SPOT] = 0; 00624 IrRange[B_SPOT] = 0; 00625 IrRange[C_SPOT] = 0; 00626 IrRange[AB_SPOT] = 0; 00627 } 00628 //白線を踏み始めた方向とボールの方向が一致．(SelfHold) 00629 LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1)); 00630 LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1)); 00631 LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1)); 00632 LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1)); 00633 00634 LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0); 00635 LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0); 00636 00637 //白線踏んでる 00638 if(RawLineSignal>0){ 00639 LineOn[A_SPOT] = (LineSign[A_SPOT]==1) &&(LineFirst[X_AXIS]==A_SPOT); 00640 LineOn[B_SPOT] = (LineSign[B_SPOT]==1) &&(LineFirst[X_AXIS]==B_SPOT); 00641 LineOn[C_SPOT] = (LineSign[C_SPOT]==1) &&(LineFirst[Y_AXIS]==C_SPOT); 00642 LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT); 00643 //外側に向かう力を消す． 00644 //x 00645 if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){ 00646 00647 if(LinePriority[Y_AXIS]==0){ 00648 LinePriority[X_AXIS]=1; 00649 LinePriority[Y_AXIS]=0; 00650 } 00651 vx=0; 00652 //yの力を加える． 00653 if(vy>0){vy += 10;} 00654 if(vy<0){vy -= 10;} 00655 } 00656 //y 00657 if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){ 00658 if(LinePriority[X_AXIS]==0){ 00659 LinePriority[X_AXIS]=0; 00660 LinePriority[Y_AXIS]=1; 00661 } 00662 if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){ 00663 LinePriority[X_AXIS]=0; 00664 LinePriority[Y_AXIS]=1; 00665 } 00666 vy=0; 00667 } 00668 //内側に向かう力を加える． 00669 LineReturn[A_SPOT] = (LineOn[A_SPOT]==1); 00670 /*if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){ 00671 if(LineOn[C_SPOT]==0){ 00672 LineReturn[A_SPOT]=0; 00673 } 00674 else{ 00675 LineReturn[A_SPOT]=1; 00676 } 00677 }*/ 00678 00679 LineReturn[B_SPOT] = (LineOn[B_SPOT]==1); 00680 /*if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){ 00681 if(LineOn[C_SPOT]==0){ 00682 LineReturn[B_SPOT]=0; 00683 } 00684 else{ 00685 LineReturn[B_SPOT]=1; 00686 } 00687 }*/ 00688 LineReturn[C_SPOT] = (LineOn[C_SPOT]==1); 00689 /*if(LineReturn[C_SPOT]==1){ 00690 LineReturn[A_SPOT]=0; 00691 LineReturn[B_SPOT]=0; 00692 }*/ 00693 LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1); 00694 00695 LineForce[X_AXIS] = (LINE_RF*2)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + 00696 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) + 00697 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)); 00698 LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1)); 00699 /* 00700 LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + 00701 (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0) 00702 *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) + 00703 (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0) 00704 *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)); 00705 LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1)); 00706 */ 00707 Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY); 00708 Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY); 00709 Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY); 00710 } 00711 else{ 00712 LineForce[X_AXIS] = 0; 00713 LineForce[Y_AXIS] = 0; 00714 00715 LinePriority[X_AXIS]=0; 00716 LinePriority[Y_AXIS]=0; 00717 } 00718 00719 vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS]; 00720 vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS]; 00721 vs = data.OutputPID; 00722 move( 00723 vx, 00724 vy, 00725 vs 00726 ); 00727 /*move( 00728 10, 00729 0, 00730 0 00731 );*/ 00732 data.motorlog[X_AXIS] += vx; 00733 data.motorlog[Y_AXIS] += vy; 00734 if(data.MotorFlag==1){ 00735 tx_motor(); 00736 data.MotorFlag=0; 00737 } 00738 return; 00739 } 00740 void modeAttack4(void){ 00741 if(data.KickFlag==1){ 00742 DriveSolenoid(); 00743 } 00744 return; 00745 } 00746 void modeAttack5(void){ 00747 if(data.IrFlag==1){ 00748 ReadIr(); 00749 data.IrFlag=0; 00750 } 00751 if(data.PidFlag==1){ 00752 PidUpdate(); 00753 data.PidFlag=0; 00754 } 00755 move(0,0,10); 00756 if(data.MotorFlag==1){ 00757 tx_motor(); 00758 data.MotorFlag=0; 00759 } 00760 00761 return; 00762 }
Generated on Tue Jul 12 2022 23:12:38 by
1.7.2