CatPot 2015-2016
ジャパンオープン用のLPC824専用プログラム
Dependencies: mbed-src Ping SDFileSystem
Embed:
(wiki syntax)
Show/hide line numbers
format.cpp
00001 #include "mbed.h" 00002 #include "extern.h" 00003 00004 #if defined(ULTRA_SONIC) || defined(ULTRA_SONIC_2) 00005 00006 uint16_t packet, val, order; 00007 uint16_t front_dis=0, rear_dis=0; 00008 00009 void emergency(void){ 00010 led[1]=1; 00011 //front_dis = 0xAA; 00012 //rear_dis = 0xBB; 00013 val = nucleo.receive(); 00014 if(val==1){ 00015 order = nucleo.read(); 00016 if(order == 0xABCD){ 00017 packet = ((front_dis<<8)|(rear_dis))&0xFFFF;nucleo.reply(packet); 00018 } 00019 else{ 00020 packet = 0xFFFF;nucleo.reply(packet); 00021 } 00022 //pc.printf("SSSSSSSSSSSSSSSSS%d\r\n", order); 00023 } 00024 else{ 00025 //pc.printf("FFFF\r\n"); 00026 } 00027 wait_us(100); 00028 led[1]=0; 00029 } 00030 void Usw_System(void){ 00031 00032 nucleo.format(16, 3); 00033 nucleo.frequency(1000000); 00034 nucleo.reply(0x0000); 00035 00036 call.fall(&emergency); 00037 00038 //led[0]=led[1]=1; 00039 while(1){ 00040 00041 led[0]=1; 00042 front.Send(); 00043 wait_ms(30); 00044 front_dis = front.Read_cm(); 00045 if(front_dis>0xFE) front_dis=0xFE; 00046 00047 00048 rear.Send(); 00049 wait_ms(30); 00050 rear_dis = rear.Read_cm(); 00051 if(rear_dis>0xFE) rear_dis=0xFE; 00052 led[0]=0; 00053 wait_us(1000); 00054 //pc.printf("%d\t%d\t\r\n", front_dis, rear_dis); 00055 00056 //emergency(); 00057 } 00058 } 00059 #endif /*(ULTRA_SONIC) || (ULTRA_SONIC_2)*/ 00060 00061 00062 #ifdef IR_SENSOR 00063 00064 uint8_t ir_notice=0;//4bit 00065 uint8_t ir_val_phase[2]; 00066 uint8_t ir_dif_phase[2]; 00067 uint16_t ir_val_diff[2]; 00068 uint8_t ir_place;//0<=x<=20 00069 static uint8_t const ir_key=IR_KEY;//4bit 00070 uint8_t ir_position[2]={0};//4bit*2 00071 00072 void emergency(void){ 00073 00074 uint16_t order,packet,val; 00075 //ir_key=0xA; 00076 //ir_notice=0xC; 00077 //ir_position[1]=0xE; 00078 //ir_position[0]=0xF; 00079 val = nucleo.receive(); 00080 if(val==1){ 00081 order = nucleo.read(); 00082 if(order == 0xABCD){ 00083 //key/phaseL/phaseS/long_diff/position/ 00084 // 2/ 3/ 3/ 3/ 5/ 00085 packet = ( 00086 ((0xC000)&(ir_key<<14)) |//1100000000000000 00087 ((0x3800)&(ir_val_phase[IR_LONG]<<11)) |//0011100000000000 00088 ((0x0700)&(ir_val_phase[IR_SHORT]<<8)) |//0000011100000000 00089 ((0x00E0)&(ir_dif_phase[IR_LONG]<<5)) |//0000000011100000 00090 ((0x001F)&(ir_place<<0)) //0000000000011111 00091 )&0xFFFF; 00092 00093 nucleo.reply(packet); 00094 } 00095 else if(order == 0xBCDA){ 00096 //key/phaseL/phaseS/long/short/ 00097 // 2/ 3/ 3/ 4/ 4/ 00098 packet = ( 00099 ((0xC000)&(ir_key<<14)) |//1100000000000000 00100 ((0x3800)&(ir_val_phase[IR_LONG]<<11)) |//0011100000000000 00101 ((0x0700)&(ir_val_phase[IR_SHORT]<<8)) |//0000011100000000 00102 ((0x00F0)&(ir_position[IR_LONG]<<4)) |//0000000011110000 00103 ((0x000F)&(ir_position[IR_SHORT]<<0)) //0000000000001111 00104 )&0xFFFF; 00105 00106 nucleo.reply(packet); 00107 } 00108 else{ 00109 packet = 0xFFFF;nucleo.reply(packet); 00110 } 00111 //pc.printf("SSSSSSSSSSSSSSSSS%d\r\n", order); 00112 } 00113 else{ 00114 //pc.f("FFFF\r\n"); 00115 } 00116 } 00117 void Ir_System(void){ 00118 00119 uint8_t i;//ic 00120 uint8_t j;//ch 00121 00122 uint8_t num_long; 00123 uint8_t num_short; 00124 00125 uint16_t ir_val_long[8]; 00126 uint16_t ir_val_short[12]; 00127 00128 uint16_t ir_val; 00129 00130 uint8_t ir_posi; 00131 uint8_t ir_posi_long[8]; 00132 uint8_t ir_posi_short[12]; 00133 00134 uint16_t ave_buff_long[LONG_AVE]; 00135 uint16_t ave_buff_long2[LONG_AVE]; 00136 uint32_t ave_sum_long; 00137 uint32_t ave_sum_long2; 00138 //uint16_t ir_val_long_plus; 00139 //uint16_t ir_val_long_plus2; 00140 00141 00142 static uint8_t const ch_num[3]={8, 8, 4}; 00143 00144 static uint16_t const ir_dis_range[2][8]={ 00145 {0, 2000, 2250, 2500, 2600, 2700, 3000, 3500, },//IR_SHORT 0 00146 {0, 1000, 1100, 1200, 1300, 1500, 1700, 1900, },//IR_LONG 1 00147 }; 00148 00149 00150 static uint16_t const ir_dif_range[2][8]={ 00151 {0, 500, 750, 1000, 1250, 1500, 1600, 1800, },//IR_SHORT 0 00152 {0, 500, 600, 650, 700, 800, 900, 1000, },//IR_LONG 1 00153 }; 00154 00155 nucleo.format(16, 3); 00156 nucleo.frequency(1000000); 00157 nucleo.reply(0x0000); 00158 00159 ir.format(8, 3); 00160 ir.frequency(1000000); 00161 00162 //遠距離用電源 00163 supply.period(0.010);//T=10[ms] 00164 supply.write(0.9);//L...10%,H...90% 00165 00166 //Nucleoとの通信用のピン変化割り込み 00167 //Nucleoとの通信をする際はTeraTermへの出力は控えるべき 00168 call.fall(&emergency); 00169 00170 for(i=0,ave_sum_long =2100*LONG_AVE; i<LONG_AVE; i++) ave_buff_long[i]=2100; 00171 for(i=0,ave_sum_long2=2100*LONG_AVE; i<LONG_AVE; i++) ave_buff_long2[i]=2100; 00172 00173 for(i=0; i<IC_NUM; i++) cs[i]=1; 00174 00175 ir_val_phase[IR_SHORT] = DIS_7; 00176 ir_val_phase[IR_LONG] = DIS_7; 00177 while(1) { 00178 //全ての素子の値を検出 00179 num_short=0; 00180 num_long=0; 00181 for(i=0; i<IC_NUM; i++){//IC 00182 for(j=0; j<ch_num[i]; j++){//Ch 00183 ir_val = read_input(i, j); 00184 ir_posi = read_position(i, j); 00185 00186 if(i==0){ 00187 ir_val_long[num_long] = ir_val; 00188 ir_posi_long[num_long] = ir_posi; 00189 num_long++; 00190 00191 } 00192 else{ 00193 ir_val_short[num_short] = ir_val; 00194 ir_posi_short[num_short] = ir_posi; 00195 num_short++; 00196 } 00197 00198 } 00199 } 00200 //バブルソート 00201 BubbleSort(ir_val_short, ir_posi_short, 12); 00202 BubbleSort(ir_val_long, ir_posi_long, 8); 00203 00204 //移動平均 00205 ave_sum_long += ir_val_long[7]; 00206 ave_sum_long -= ave_buff_long[LONG_AVE-1]; 00207 for(i=LONG_AVE-1; i>0; i--){ 00208 ave_buff_long[i] = ave_buff_long[i-1]; 00209 } 00210 ave_buff_long[0] = ir_val_long[7]; 00211 00212 //ir_val_long_plus = ir_val_long[7]; 00213 ir_val_long[7] = ave_sum_long/LONG_AVE; 00214 00215 //移動平均2 00216 ave_sum_long2 += ir_val_long[0]; 00217 ave_sum_long2 -= ave_buff_long2[LONG_AVE-1]; 00218 for(i=LONG_AVE-1; i>0; i--){ 00219 ave_buff_long2[i] = ave_buff_long2[i-1]; 00220 } 00221 ave_buff_long2[0] = ir_val_long[0]; 00222 00223 //ir_val_long_plus2 = ir_val_long[0]; 00224 ir_val_long[0] = ave_sum_long2/LONG_AVE; 00225 00226 00227 //検出した最大値と最小値の差 00228 ir_val_diff[IR_SHORT] = ir_val_short[0]-ir_val_short[11];//After bubble 00229 ir_val_diff[IR_LONG] = abs(ir_val_long[0]-ir_val_long[7]);//After bubble 00230 //最もボールがあるとされる位置を代入 00231 ir_position[IR_SHORT] = ir_posi_short[11];//After bubble 00232 ir_position[IR_LONG] = ir_posi_long[7];//After bubble 00233 00234 /* 00235 #define IR_NOTE_NONE 0x0 00236 #define IR_NOTE_FAR 0x1 00237 #define IR_NOTE_CLOSE 0x2 00238 #define IR_NOTE_CLOSER 0x3 00239 */ 00240 //ボールが遠くにあるか，近くか，フィールドにないかを判断（8段階） 00241 00242 00243 //short 00244 00245 if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_7]){ 00246 ir_val_phase[IR_SHORT] = DIS_7; 00247 } 00248 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_6]){ 00249 ir_val_phase[IR_SHORT] = DIS_6; 00250 } 00251 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_5]){ 00252 ir_val_phase[IR_SHORT] = DIS_5; 00253 } 00254 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_4]){ 00255 ir_val_phase[IR_SHORT] = DIS_4; 00256 } 00257 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_3]){ 00258 ir_val_phase[IR_SHORT] = DIS_3; 00259 } 00260 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_2]){ 00261 ir_val_phase[IR_SHORT] = DIS_2; 00262 } 00263 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_1]){ 00264 ir_val_phase[IR_SHORT] = DIS_1; 00265 } 00266 else{//ir_val_short[11]>=0 00267 ir_val_phase[IR_SHORT] = DIS_0; 00268 } 00269 00270 //long 00271 00272 if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_7]){ 00273 ir_val_phase[IR_LONG] = DIS_7; 00274 } 00275 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_6]){ 00276 ir_val_phase[IR_LONG] = DIS_6; 00277 } 00278 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_5]){ 00279 ir_val_phase[IR_LONG] = DIS_5; 00280 } 00281 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_4]){ 00282 ir_val_phase[IR_LONG] = DIS_4; 00283 } 00284 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_3]){ 00285 ir_val_phase[IR_LONG] = DIS_3; 00286 } 00287 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_2]){ 00288 ir_val_phase[IR_LONG] = DIS_2; 00289 } 00290 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_1]){ 00291 ir_val_phase[IR_LONG] = DIS_1; 00292 } 00293 else{//ir_val_long[7]>=0 00294 ir_val_phase[IR_LONG] = DIS_0; 00295 } 00296 00297 //long_dif 00298 00299 if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_7]){ 00300 ir_dif_phase[IR_LONG] = DIS_7; 00301 } 00302 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_6]){ 00303 ir_dif_phase[IR_LONG] = DIS_6; 00304 } 00305 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_5]){ 00306 ir_dif_phase[IR_LONG] = DIS_5; 00307 } 00308 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_4]){ 00309 ir_dif_phase[IR_LONG] = DIS_4; 00310 } 00311 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_3]){ 00312 ir_dif_phase[IR_LONG] = DIS_3; 00313 } 00314 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_2]){ 00315 ir_dif_phase[IR_LONG] = DIS_2; 00316 } 00317 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_1]){ 00318 ir_dif_phase[IR_LONG] = DIS_1; 00319 } 00320 else{//ir_val_diff[IR_LONG]>=0 00321 ir_dif_phase[IR_LONG] = DIS_0; 00322 } 00323 00324 //ir_place 00325 if(ir_val_phase[IR_SHORT] >= DIS_7){ 00326 ir_place = ir_position[IR_LONG]-1; 00327 } 00328 else{ 00329 ir_place = ir_position[IR_SHORT]+7; 00330 } 00331 00332 //デバッグ用のTeraTermへの出力 00333 00334 /* 00335 pc.printf("val_short:%d\t", ir_val_short[11]);//最小検出値(近) 00336 pc.printf("val_long(AVE):%d\t", ir_val_long[7]);//最小検出値(遠) 00337 //pc.printf("val_long:%d\t", ir_val_long_plus);//最小検出値(遠)...平均なし 00338 00339 pc.printf("state_short:%d\t", ir_position[0]);//ボール位置(近) 00340 pc.printf("state_long:%d\t", ir_position[1]);//ボール位置(遠) 00341 //pc.printf("Diff_short:%d\t", ir_val_diff[IR_SHORT]);//検出した最大値と最小値の差(近) 00342 pc.printf("Diff_long:%d\t", ir_val_diff[IR_LONG]);//検出した最大値と最小値の差(遠) 00343 pc.printf("ir_val_phase[IR_SHORT]:%d\t", ir_val_phase[IR_SHORT]);//判定されたボールの距離の段階(遠) 00344 pc.printf("ir_val_phase[IR_LONG]:%d\t", ir_val_phase[IR_LONG]);//判定されたボールの距離の段階(遠) 00345 pc.printf("notice:%d\t", ir_notice);//判定されたボールの状況 00346 00347 pc.printf("ir_place:%d\t", ir_place); 00348 00349 00350 pc.printf("\r\n"); 00351 */ 00352 00353 /* 00354 pc.printf("%Value is "); 00355 pc.printf("Dirr_short is %d\t", ir_val_short[0]-ir_val_short[11]);//検出した最大値と最小値の差(近) 00356 pc.printf("Dirr_long is %d\t", ir_val_long[0]-ir_val_long[7]);//検出した最大値と最小値の差(遠) 00357 pc.printf("\r\n"); 00358 */ 00359 00360 //検出した値全てを出力(バブルソートすると位置が変わるので注意) 00361 /*pc.printf("%Value is "); 00362 for(i=0; i<8; i++){ 00363 pc.printf("%d\t", ir_val_long[i]);//遠 00364 } 00365 for(i=0; i<12; i++){ 00366 pc.printf("%d\t", ir_val_short[i]);//近 00367 } 00368 00369 pc.printf("\r\n"); 00370 */ 00371 } 00372 } 00373 uint16_t read_input(uint8_t ic, uint8_t channel) 00374 { 00375 uint16_t command_high = START_BIT | MODE_SINGLE | ((channel & 0x04) >> 2); 00376 uint16_t command_low = (channel & 0x03) << 6; 00377 00378 cs[ic] = 0; 00379 00380 ir.write(command_high); 00381 uint16_t high_byte = ir.write(command_low) & 0x0F; 00382 uint16_t low_byte = ir.write(0); 00383 00384 wait_us(1); 00385 cs[ic] = 1; 00386 00387 return (high_byte << 8) | low_byte;//0x0000...0xFFFF 00388 } 00389 uint8_t read_position(uint8_t ic, uint8_t channel){ 00390 //DigitalOut cs[IC_NUM] = {cs1, cs2, cs3}; 00391 static uint8_t const ir_location[3][8]= 00392 {//0,1,2...7(ch) 00393 {1, 2, 3, 4, 5, 6, 7, 8},//cs1,Long*8 00394 {1, 2, 3, 4, 5, 6, 7, 8},//cs2,Short*8 00395 {9, 10, 11, 12},//cs3,Short*4 00396 }; 00397 return ir_location[ic][channel]; 00398 } 00399 void BubbleSort(uint16_t *data, uint8_t *data2, uint8_t n)//降順にする 00400 { 00401 bool flag; 00402 uint8_t i, j;//inclement 00403 uint16_t temp;//temporary for value 00404 uint8_t temp2;//temporary for position 00405 00406 j=0; 00407 do{ 00408 j++; 00409 flag=0; 00410 for (i=0; i<n-j; i++) { 00411 if (data[i]<data[i+1]) { 00412 // 左右の並びがおかしければ入れ替える 00413 flag=1; 00414 temp=data[i]; 00415 data[i]=data[i+1]; 00416 data[i+1]=temp; 00417 00418 temp2=data2[i]; 00419 data2[i]=data2[i+1]; 00420 data2[i+1]=temp2; 00421 } 00422 } 00423 } while (flag==1); //入れ替えがある間，繰り返す． 00424 } 00425 #endif /*IR_SENSOR*/
Generated on Tue Jul 12 2022 16:27:28 by
1.7.2