CatPot 2015-2016
ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
Embed:
(wiki syntax)
Show/hide line numbers
input.cpp
00001 #include "mbed.h" 00002 #include "extern.h" 00003 00004 //line 00005 void ReadLine(void){ 00006 static uint8_t line_buf[3]={0,0,0}; 00007 wait_us(100); 00008 if(Line[0].read()==1) line_buf[0] = 100; 00009 if(line_buf[0]==0) data.lnFlag[0]=0; 00010 else data.lnFlag[0]=1; 00011 if(line_buf[0]>0) line_buf[0]--; 00012 00013 if(Line[1].read()==1) line_buf[1] = 100; 00014 if(line_buf[1]==0) data.lnFlag[1]=0; 00015 else data.lnFlag[1]=1; 00016 if(line_buf[1]>0) line_buf[1]--; 00017 00018 if(Line[2].read()==1) line_buf[2] = 100; 00019 if(line_buf[2]==0) data.lnFlag[2]=0; 00020 else data.lnFlag[2]=1; 00021 if(line_buf[2]>0) line_buf[2]--; 00022 } 00023 //line_hold 00024 void LineRanking_A(void){ 00025 if(data.lnOrder[0]==LINE_EMPTY){ 00026 data.lnOrder[0]=A_SPOT; 00027 } 00028 else if(data.lnOrder[1]==LINE_EMPTY){ 00029 data.lnOrder[1]=A_SPOT; 00030 } 00031 else if(data.lnOrder[2]==LINE_EMPTY){ 00032 data.lnOrder[2]=A_SPOT; 00033 } 00034 } 00035 void LineRanking_B(void){ 00036 if(data.lnOrder[0]==LINE_EMPTY){ 00037 data.lnOrder[0]=B_SPOT; 00038 } 00039 else if(data.lnOrder[1]==LINE_EMPTY){ 00040 data.lnOrder[1]=B_SPOT; 00041 } 00042 else if(data.lnOrder[2]==LINE_EMPTY){ 00043 data.lnOrder[2]=B_SPOT; 00044 } 00045 } 00046 void LineRanking_C(void){ 00047 if(data.lnOrder[0]==LINE_EMPTY){ 00048 data.lnOrder[0]=C_SPOT; 00049 } 00050 else if(data.lnOrder[1]==LINE_EMPTY){ 00051 data.lnOrder[1]=C_SPOT; 00052 } 00053 else if(data.lnOrder[2]==LINE_EMPTY){ 00054 data.lnOrder[2]=C_SPOT; 00055 } 00056 } 00057 void LineRankClear(void){ 00058 data.lnOrder[0]=LINE_EMPTY; 00059 data.lnOrder[1]=LINE_EMPTY; 00060 data.lnOrder[2]=LINE_EMPTY; 00061 00062 if((0<LineHold)&&(LineHold<7)){ 00063 if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==1)){ 00064 data.lnOrder[0] = C_SPOT; 00065 data.lnOrder[1] = LINE_EMPTY; 00066 } 00067 else if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)){ 00068 data.lnOrder[0] = B_SPOT; 00069 data.lnOrder[1] = LINE_EMPTY; 00070 } 00071 else if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==1)){ 00072 data.lnOrder[0] = C_SPOT; 00073 data.lnOrder[1] = B_SPOT; 00074 } 00075 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==0)){ 00076 data.lnOrder[0] = A_SPOT; 00077 data.lnOrder[1] = LINE_EMPTY; 00078 } 00079 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==1)){ 00080 data.lnOrder[0] = C_SPOT; 00081 data.lnOrder[1] = A_SPOT; 00082 } 00083 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)){ 00084 data.lnOrder[0] = A_SPOT; 00085 data.lnOrder[1] = B_SPOT; 00086 } 00087 } 00088 } 00089 //line raw 00090 /* 00091 void LineRawCall_A(void){data.lnRawMemory[A_SPOT]=1;Line_timeout[A_SPOT].attach(&LineRawClear_A, LINE_DELAY);} 00092 void LineRawCall_B(void){data.lnRawMemory[B_SPOT]=1;Line_timeout[B_SPOT].attach(&LineRawClear_B, LINE_DELAY);} 00093 void LineRawCall_C(void){data.lnRawMemory[C_SPOT]=1;Line_timeout[C_SPOT].attach(&LineRawClear_C, LINE_DELAY);} 00094 void LineRawClear_A(void){if(Line[A_SPOT].read()==1){LineRawCall_A();}else{data.lnRawMemory[A_SPOT]=0;}} 00095 void LineRawClear_B(void){if(Line[B_SPOT].read()==1){LineRawCall_B();}else{data.lnRawMemory[B_SPOT]=0;}} 00096 void LineRawClear_C(void){if(Line[C_SPOT].read()==1){LineRawCall_C();}else{data.lnRawMemory[C_SPOT]=0;}} 00097 */ 00098 void LineRawCall_ALL(void){ 00099 if(data.lnRawReturn==1){ 00100 if( 00101 (data.lnRawOrder[0]==A_SPOT)|| 00102 (data.lnRawOrder[0]==B_SPOT)|| 00103 (data.lnRawOrder[0]==C_SPOT) 00104 ){ 00105 if( 00106 ( 00107 (data.lnRawOrder[0]!=LINE_EMPTY)&& 00108 (data.lnRawOrder[1]!=LINE_EMPTY)&& 00109 ((Line[data.lnRawOrder[0]].read()==1)||(data.lnRawOrder[0]==data.lnRawLastRise)) 00110 )|| 00111 (data.lnRawOrder[0]==C_SPOT) 00112 ){ 00113 Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_3); 00114 } 00115 else{ 00116 Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_2); 00117 } 00118 } 00119 else{ 00120 Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_2); 00121 } 00122 } 00123 else{ 00124 Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_1); 00125 } 00126 } 00127 void LineRawClear_ALL(void){ 00128 if(LineRaw>0){ 00129 LineRawCall_ALL(); 00130 } 00131 else if(LineRaw==0){ 00132 LineRawRankClear(); 00133 } 00134 } 00135 void LineRawRankClear(void){ 00136 00137 //JSO2 00138 LineLiberate(); 00139 00140 data.lnRawOrderLog2[0]=data.lnRawOrderLog1[0]; 00141 data.lnRawOrderLog2[1]=data.lnRawOrderLog1[1]; 00142 data.lnRawOrderLog2[2]=data.lnRawOrderLog1[2]; 00143 00144 data.lnRawOrderLog1[0]=data.lnRawOrder[0]; 00145 data.lnRawOrderLog1[1]=data.lnRawOrder[1]; 00146 data.lnRawOrderLog1[2]=data.lnRawOrder[2]; 00147 00148 data.lnRawOrder[0]=LINE_EMPTY; 00149 data.lnRawOrder[1]=LINE_EMPTY; 00150 data.lnRawOrder[2]=LINE_EMPTY; 00151 00152 if( 00153 (data.lnRawOrderLog2[0]==data.lnRawOrderLog1[0])&& 00154 //(data.lnRawOrderLog2[1]==data.lnRawOrderLog1[1])&& 00155 //(data.lnRawOrderLog2[2]==data.lnRawOrderLog1[2]) 00156 (data.lnRawOrderLog2[0]!=LINE_EMPTY) 00157 //(data.lnRawOrderLog2[1]!=LINE_EMPTY) 00158 ){ 00159 if(data.lnRepeat>100) data.lnRepeat=100; 00160 data.lnRepeat++; 00161 00162 data.irLastNotice = data.irNotice; 00163 data.irLastPosition = data.irPosition; 00164 } 00165 else{ 00166 data.lnRepeat=0; 00167 } 00168 00169 00170 data.lnRawRise[A_SPOT]=0; 00171 data.lnRawRise[B_SPOT]=0; 00172 data.lnRawRise[C_SPOT]=0; 00173 } 00174 void LineRawLogReset(void){ 00175 /* 00176 if( 00177 (!( 00178 (data.irPosition==data.irLastPosition)|| 00179 (data.irPosition==(data.irLastPosition+1))|| 00180 (data.irPosition==(data.irLastPosition-1))|| 00181 (data.irPosition==(data.irLastPosition+11))|| 00182 (data.irPosition==(data.irLastPosition-11)) 00183 ))&& 00184 ( 00185 (data.irNotice==IR_CLOSE)|| 00186 (data.irNotice==IR_CLOSER) 00187 )&& 00188 ( 00189 (data.irLastPosition>7) 00190 ) 00191 ){ 00192 data.lnRawOrderLog2[0]=LINE_EMPTY; 00193 data.lnRawOrderLog2[1]=LINE_EMPTY; 00194 data.lnRawOrderLog2[2]=LINE_EMPTY; 00195 } 00196 else{ 00197 data.lnRawOrderLog2[0]=LINE_EMPTY; 00198 data.lnRawOrderLog2[1]=LINE_EMPTY; 00199 data.lnRawOrderLog2[2]=LINE_EMPTY; 00200 00201 data.lnRawOrderLog1[0]=LINE_EMPTY; 00202 data.lnRawOrderLog1[1]=LINE_EMPTY; 00203 data.lnRawOrderLog1[2]=LINE_EMPTY; 00204 }*/ 00205 if(data.irNotice==IR_NONE){ 00206 data.lnRawOrderLog2[0]=LINE_EMPTY; 00207 data.lnRawOrderLog2[1]=LINE_EMPTY; 00208 data.lnRawOrderLog2[2]=LINE_EMPTY; 00209 00210 data.lnRawOrderLog1[0]=LINE_EMPTY; 00211 data.lnRawOrderLog1[1]=LINE_EMPTY; 00212 data.lnRawOrderLog1[2]=LINE_EMPTY; 00213 } 00214 else{ 00215 data.lnRawOrderLog2[0]=LINE_EMPTY; 00216 data.lnRawOrderLog2[1]=LINE_EMPTY; 00217 data.lnRawOrderLog2[2]=LINE_EMPTY; 00218 } 00219 00220 } 00221 void LineRawRanking_A(void){ 00222 data.lnRawLastRise=A_SPOT; 00223 LineRawCall_ALL(); 00224 if(data.lnRawRise[A_SPOT]==1) return; 00225 00226 if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==A_SPOT)){ 00227 data.lnRawOrder[0]=A_SPOT; 00228 if((sys.DefenceFlag==1)&&(data.lnRawOrder[1]==LINE_EMPTY)){ 00229 data.lnRawOrder[1]=C_SPOT; 00230 } 00231 } 00232 else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==A_SPOT)){ 00233 data.lnRawOrder[1]=A_SPOT; 00234 } 00235 else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==A_SPOT)){ 00236 data.lnRawOrder[2]=A_SPOT; 00237 } 00238 data.lnRawRise[A_SPOT]=1; 00239 } 00240 void LineRawRanking_B(void){ 00241 data.lnRawLastRise=B_SPOT; 00242 LineRawCall_ALL(); 00243 if(data.lnRawRise[B_SPOT]==1) return; 00244 00245 if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==B_SPOT)){ 00246 data.lnRawOrder[0]=B_SPOT; 00247 if((sys.DefenceFlag==1)&&(data.lnRawOrder[1]==LINE_EMPTY)){ 00248 data.lnRawOrder[1]=C_SPOT; 00249 } 00250 } 00251 else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==B_SPOT)){ 00252 data.lnRawOrder[1]=B_SPOT; 00253 } 00254 else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==B_SPOT)){ 00255 data.lnRawOrder[2]=B_SPOT; 00256 } 00257 data.lnRawRise[B_SPOT]=1; 00258 } 00259 void LineRawRanking_C(void){ 00260 data.lnRawLastRise=C_SPOT; 00261 LineRawCall_ALL(); 00262 if(data.lnRawRise[C_SPOT]==1) return; 00263 00264 if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==C_SPOT)){ 00265 data.lnRawOrder[0]=C_SPOT; 00266 } 00267 else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==C_SPOT)){ 00268 data.lnRawOrder[1]=C_SPOT; 00269 } 00270 else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==C_SPOT)){ 00271 data.lnRawOrder[2]=C_SPOT; 00272 } 00273 data.lnRawRise[C_SPOT]=1; 00274 } 00275 //ball 00276 uint8_t ReadBall(void){//1or0 00277 return (BallChecker.read()==0); 00278 //return (BallCheckerA.read_u16()<40000); 00279 } 00280 void JudgeBallHolding(void){ 00281 //ホールド判定 00282 if(sys.DriBlind==0){ 00283 if( 00284 ((data.ball==0)&&(sys.BallHoldFlag==0))|| 00285 ((data.ball==1)&&(sys.BallHoldFlag==1)) 00286 ){ 00287 sys.BallHoldJudgeFlag=0; 00288 Ball_judge.detach(); 00289 } 00290 if( 00291 ((data.ball==0)&&(sys.BallHoldFlag==1))|| 00292 ((data.ball==1)&&(sys.BallHoldFlag==0)) 00293 ){ 00294 if(sys.BallHoldJudgeFlag==0){ 00295 sys.BallHoldJudgeFlag=1; 00296 Ball_judge.attach(&JudgeBallHold, .25); 00297 } 00298 } 00299 /* 00300 if( 00301 !( 00302 (data.irNotice==IR_CLOSER)&& 00303 ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12)) 00304 ) 00305 ){ 00306 sys.BallHoldJudgeFlag=0; 00307 sys.BallHoldFlag=0; 00308 Ball_judge.detach(); 00309 } 00310 */ 00311 } 00312 else{ 00313 sys.BallHoldFlag=0; 00314 } 00315 } 00316 //readsensor 00317 void ReadCmps(void){ 00318 cmps_set.cmps = hmc.sample()/10.0; 00319 } 00320 void ReadPing(void){ 00321 uint16_t spi_data; 00322 //Ultra Sonic Wave 00323 spi_ss[1]=0; 00324 wait_us(200); 00325 00326 spi_data = spi.write(0xABCD); 00327 00328 wait_us(200); 00329 spi_ss[1]=1; 00330 00331 data.ping[0] = (spi_data&0x00FF)>>0; 00332 data.ping[1] = (spi_data&0xFF00)>>8; 00333 } 00334 void ReadPing2(void){ 00335 uint16_t spi_data; 00336 //Ultra Sonic Wave 00337 spi_ss[0]=0; 00338 wait_us(200); 00339 00340 spi_data = spi.write(0xABCD); 00341 00342 wait_us(200); 00343 spi_ss[0]=1; 00344 00345 data.ping[2] = (spi_data&0x00FF)>>0; 00346 data.ping[3] = (spi_data&0xFF00)>>8; 00347 } 00348 void ReadIr(void){ 00349 uint16_t spi_data; 00350 00351 //Ir 00352 spi_ss[3]=0; 00353 wait_us(200); 00354 00355 spi_data = spi.write(0xABCD); 00356 00357 wait_us(200); 00358 spi_ss[3]=1; 00359 /* 00360 //key/phaseL/phaseS/long/short/ 00361 // 2/ 3/ 3/ 4/ 4/ 00362 data.irKey = (spi_data&0xC000)>>14;//1100000000000000 00363 data.irValPhase[IR_LONG] = (spi_data&0x3800)>>11;//0011100000000000 00364 data.irValPhase[IR_SHORT] = (spi_data&0x0700)>>8;///0000011100000000 00365 data.irSpot[IR_LONG] = (spi_data&0x00F0)>>4;///0000000011110000 00366 data.irSpot[IR_SHORT] = (spi_data&0x000F)>>0;///0000000000001111 00367 */ 00368 00369 //key/phaseL/phaseS/diffL/position/ 00370 // 2/ 3/ 3/ 3/ 5/ 00371 data.irKey = (spi_data&0xC000)>>14;//1100000000000000 00372 data.irValPhase[IR_LONG] = (spi_data&0x3800)>>11;//0011100000000000 00373 data.irValPhase[IR_SHORT] = (spi_data&0x0700)>>8;///0000011100000000 00374 data.irDif[IR_LONG] = (spi_data&0x00E0)>>5;///0000000011100000 00375 data.irPosition = (spi_data&0x001F)>>0;///0000000000011111 00376 00377 if(data.irKey!=0x2) data.irNotice=IR_NONE; 00378 00379 00380 if(data.irValPhase[IR_SHORT]>=DIS_7){ 00381 if(data.irValPhase[IR_LONG]>=DIS_7){ 00382 data.irNotice=IR_NONE; 00383 } 00384 else{ 00385 if(data.irDif[IR_LONG]<=DIS_0){ 00386 data.irNotice=IR_NONE; 00387 } 00388 else{ 00389 data.irNotice=IR_FAR; 00390 } 00391 } 00392 } 00393 else{ 00394 if(data.irValPhase[IR_SHORT]>=DIS_3){ 00395 data.irNotice = IR_CLOSE; 00396 } 00397 else{ 00398 data.irNotice = IR_CLOSER; 00399 } 00400 } 00401 00402 /*if(data.irValPhase[IR_LONG]>=DIS_7){ 00403 data.irNotice=IR_NONE; 00404 } 00405 else{ 00406 if(data.irValPhase[IR_SHORT]>=DIS_7){ 00407 data.irNotice=IR_FAR; 00408 } 00409 else{ 00410 if(data.irValPhase[IR_LONG]>=DIS_4){ 00411 data.irNotice = IR_CLOSE; 00412 } 00413 else{ 00414 data.irNotice = IR_CLOSER; 00415 } 00416 } 00417 }*/ 00418 /* 00419 if(data.irNotice == IR_NONE) data.irPosition=11; 00420 if(data.irNotice == IR_FAR) data.irPosition=data.irSpot[IR_LONG]-1; 00421 if(data.irNotice == IR_CLOSE) data.irPosition=data.irSpot[IR_SHORT]+7; 00422 if(data.irNotice == IR_CLOSER) data.irPosition=data.irSpot[IR_SHORT]+7; 00423 */ 00424 //LED[1] = LED[0]; 00425 //LED[0] = !LED[0]; 00426 } 00427 //info 00428 void ValidInfo(void){ 00429 //LED=0xA; 00430 if(sys.InfoFlag==0){ 00431 sys.InfoFlag=1; 00432 } 00433 } 00434 void ReadInfo(void){ 00435 ReadIr(); 00436 PidUpdate(); 00437 ReadPing(); 00438 ReadPing2(); 00439 }
Generated on Wed Jul 13 2022 02:59:03 by
1.7.2