ジャパンオープン用のLPC824専用プログラム
Dependencies: mbed-src Ping SDFileSystem
main/format.cpp
- Committer:
- lilac0112_1
- Date:
- 2016-03-03
- Revision:
- 5:d666b328c093
- Parent:
- 4:a0755903bd6e
- Child:
- 6:80c2f4fce2eb
File content as of revision 5:d666b328c093:
#include "mbed.h" #include "extern.h" #if defined(ULTRA_SONIC) || defined(ULTRA_SONIC_2) uint16_t packet, val, order; uint16_t front_dis=0, rear_dis=0; void emergency(void){ led[1]=1; //front_dis = 0xAA; //rear_dis = 0xBB; val = nucleo.receive(); if(val==1){ order = nucleo.read(); if(order == 0xABCD){ packet = ((front_dis<<8)|(rear_dis))&0xFFFF;nucleo.reply(packet); } else{ packet = 0xFFFF;nucleo.reply(packet); } //pc.printf("SSSSSSSSSSSSSSSSS%d\r\n", order); } else{ //pc.printf("FFFF\r\n"); } wait_us(100); led[1]=0; } void Usw_System(void){ nucleo.format(16, 3); nucleo.frequency(1000000); nucleo.reply(0x0000); call.fall(&emergency); //led[0]=led[1]=1; while(1){ led[0]=1; front.Send(); wait_ms(30); front_dis = front.Read_cm(); if(front_dis>0xFE) front_dis=0xFE; rear.Send(); wait_ms(30); rear_dis = rear.Read_cm(); if(rear_dis>0xFE) rear_dis=0xFE; led[0]=0; wait_us(1000); //pc.printf("%d\t%d\t\r\n", front_dis, rear_dis); //emergency(); } } #endif /*(ULTRA_SONIC) || (ULTRA_SONIC_2)*/ #ifdef IR_SENSOR uint8_t ir_notice=0;//4bit uint8_t ir_val_phase[2]; uint8_t ir_dif_phase[2]; uint16_t ir_val_diff[2]; uint8_t ir_place;//0<=x<=20 static uint8_t const ir_key=IR_KEY;//4bit uint8_t ir_position[2]={0};//4bit*2 void emergency(void){ uint16_t order,packet,val; //ir_key=0xA; //ir_notice=0xC; //ir_position[1]=0xE; //ir_position[0]=0xF; val = nucleo.receive(); if(val==1){ order = nucleo.read(); if(order == 0xABCD){ //key/phaseL/phaseS/long_diff/position/ // 2/ 3/ 3/ 3/ 5/ packet = ( ((0xC000)&(ir_key<<14)) |//1100000000000000 ((0x3800)&(ir_val_phase[IR_LONG]<<11)) |//0011100000000000 ((0x0700)&(ir_val_phase[IR_SHORT]<<8)) |//0000011100000000 ((0x00E0)&(ir_dif_phase[IR_LONG]<<5)) |//0000000011100000 ((0x001F)&(ir_place<<0)) //0000000000011111 )&0xFFFF; nucleo.reply(packet); } else if(order == 0xBCDA){ //key/phaseL/phaseS/long/short/ // 2/ 3/ 3/ 4/ 4/ packet = ( ((0xC000)&(ir_key<<14)) |//1100000000000000 ((0x3800)&(ir_val_phase[IR_LONG]<<11)) |//0011100000000000 ((0x0700)&(ir_val_phase[IR_SHORT]<<8)) |//0000011100000000 ((0x00F0)&(ir_position[IR_LONG]<<4)) |//0000000011110000 ((0x000F)&(ir_position[IR_SHORT]<<0)) //0000000000001111 )&0xFFFF; nucleo.reply(packet); } else{ packet = 0xFFFF;nucleo.reply(packet); } //pc.printf("SSSSSSSSSSSSSSSSS%d\r\n", order); } else{ //pc.f("FFFF\r\n"); } } void Ir_System(void){ uint8_t i;//ic uint8_t j;//ch uint8_t num_long; uint8_t num_short; uint16_t ir_val_long[8]; uint16_t ir_val_short[12]; uint16_t ir_val; uint8_t ir_posi; uint8_t ir_posi_long[8]; uint8_t ir_posi_short[12]; uint16_t ave_buff_long[LONG_AVE]; uint16_t ave_buff_long2[LONG_AVE]; uint32_t ave_sum_long; uint32_t ave_sum_long2; uint16_t ir_val_long_plus; uint16_t ir_val_long_plus2; static uint8_t const ch_num[3]={8, 8, 4}; static uint16_t const ir_dis_range[2][8]={ {0, 2000, 2250, 2500, 2600, 2700, 3000, 3900, },//IR_SHORT 0 {0, 1000, 1100, 1200, 1300, 1500, 1800, 2000, },//IR_LONG 1 }; static uint16_t const ir_dif_range[2][8]={ {0, 500, 750, 1000, 1250, 1500, 1750, 2000, },//IR_SHORT 0 {0, 500, 600, 650, 700, 800, 900, 1000, },//IR_LONG 1 }; nucleo.format(16, 3); nucleo.frequency(1000000); nucleo.reply(0x0000); ir.format(8, 3); ir.frequency(1000000); //遠距離用電源 supply.period(0.010);//T=10[ms] supply.write(0.9);//L...10%,H...90% //Nucleoとの通信用のピン変化割り込み //Nucleoとの通信をする際はTeraTermへの出力は控えるべき call.fall(&emergency); for(i=0,ave_sum_long =2100*LONG_AVE; i<LONG_AVE; i++) ave_buff_long[i]=2100; for(i=0,ave_sum_long2=2100*LONG_AVE; i<LONG_AVE; i++) ave_buff_long2[i]=2100; for(i=0; i<IC_NUM; i++) cs[i]=1; ir_val_phase[IR_SHORT] = DIS_7; ir_val_phase[IR_LONG] = DIS_7; while(1) { //全ての素子の値を検出 num_short=0; num_long=0; for(i=0; i<IC_NUM; i++){//IC for(j=0; j<ch_num[i]; j++){//Ch ir_val = read_input(i, j); ir_posi = read_position(i, j); if(i==0){ ir_val_long[num_long] = ir_val; ir_posi_long[num_long] = ir_posi; num_long++; } else{ ir_val_short[num_short] = ir_val; ir_posi_short[num_short] = ir_posi; num_short++; } } } //バブルソート BubbleSort(ir_val_short, ir_posi_short, 12); BubbleSort(ir_val_long, ir_posi_long, 8); //移動平均 ave_sum_long += ir_val_long[7]; ave_sum_long -= ave_buff_long[LONG_AVE-1]; for(i=LONG_AVE-1; i>0; i--){ ave_buff_long[i] = ave_buff_long[i-1]; } ave_buff_long[0] = ir_val_long[7]; ir_val_long_plus = ir_val_long[7]; ir_val_long[7] = ave_sum_long/LONG_AVE; //移動平均2 ave_sum_long2 += ir_val_long[0]; ave_sum_long2 -= ave_buff_long2[LONG_AVE-1]; for(i=LONG_AVE-1; i>0; i--){ ave_buff_long2[i] = ave_buff_long2[i-1]; } ave_buff_long2[0] = ir_val_long[0]; ir_val_long_plus2 = ir_val_long[0]; ir_val_long[0] = ave_sum_long2/LONG_AVE; //検出した最大値と最小値の差 ir_val_diff[IR_SHORT] = ir_val_short[0]-ir_val_short[11];//After bubble ir_val_diff[IR_LONG] = abs(ir_val_long[0]-ir_val_long[7]);//After bubble //最もボールがあるとされる位置を代入 ir_position[IR_SHORT] = ir_posi_short[11];//After bubble ir_position[IR_LONG] = ir_posi_long[7];//After bubble /* #define IR_NOTE_NONE 0x0 #define IR_NOTE_FAR 0x1 #define IR_NOTE_CLOSE 0x2 #define IR_NOTE_CLOSER 0x3 */ //ボールが遠くにあるか,近くか,フィールドにないかを判断(8段階) //short if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_7]){ ir_val_phase[IR_SHORT] = DIS_7; } else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_6]){ ir_val_phase[IR_SHORT] = DIS_6; } else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_5]){ ir_val_phase[IR_SHORT] = DIS_5; } else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_4]){ ir_val_phase[IR_SHORT] = DIS_4; } else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_3]){ ir_val_phase[IR_SHORT] = DIS_3; } else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_2]){ ir_val_phase[IR_SHORT] = DIS_2; } else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_1]){ ir_val_phase[IR_SHORT] = DIS_1; } else{//ir_val_short[11]>=0 ir_val_phase[IR_SHORT] = DIS_0; } //long if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_7]){ ir_val_phase[IR_LONG] = DIS_7; } else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_6]){ ir_val_phase[IR_LONG] = DIS_6; } else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_5]){ ir_val_phase[IR_LONG] = DIS_5; } else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_4]){ ir_val_phase[IR_LONG] = DIS_4; } else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_3]){ ir_val_phase[IR_LONG] = DIS_3; } else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_2]){ ir_val_phase[IR_LONG] = DIS_2; } else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_1]){ ir_val_phase[IR_LONG] = DIS_1; } else{//ir_val_long[7]>=0 ir_val_phase[IR_LONG] = DIS_0; } //long_dif if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_7]){ ir_dif_phase[IR_LONG] = DIS_7; } else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_6]){ ir_dif_phase[IR_LONG] = DIS_6; } else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_5]){ ir_dif_phase[IR_LONG] = DIS_5; } else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_4]){ ir_dif_phase[IR_LONG] = DIS_4; } else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_3]){ ir_dif_phase[IR_LONG] = DIS_3; } else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_2]){ ir_dif_phase[IR_LONG] = DIS_2; } else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_1]){ ir_dif_phase[IR_LONG] = DIS_1; } else{//ir_val_diff[IR_LONG]>=0 ir_dif_phase[IR_LONG] = DIS_0; } //ir_place if(ir_val_phase[IR_SHORT] >= DIS_7){ ir_place = ir_position[IR_SHORT]+7; } else{ ir_place = ir_position[IR_LONG]-1; } //デバッグ用のTeraTermへの出力 pc.printf("val_short:%d\t", ir_val_short[11]);//最小検出値(近) pc.printf("val_long(AVE):%d\t", ir_val_long[7]);//最小検出値(遠) //pc.printf("val_long:%d\t", ir_val_long_plus);//最小検出値(遠)...平均なし pc.printf("state_short:%d\t", ir_position[0]);//ボール位置(近) pc.printf("state_long:%d\t", ir_position[1]);//ボール位置(遠) //pc.printf("Diff_short:%d\t", ir_val_diff[IR_SHORT]);//検出した最大値と最小値の差(近) pc.printf("Diff_long:%d\t", ir_val_diff[IR_LONG]);//検出した最大値と最小値の差(遠) pc.printf("ir_val_phase[IR_SHORT]:%d\t", ir_val_phase[IR_SHORT]);//判定されたボールの距離の段階(遠) pc.printf("ir_val_phase[IR_LONG]:%d\t", ir_val_phase[IR_LONG]);//判定されたボールの距離の段階(遠) //pc.printf("notice:%d\t", ir_notice);//判定されたボールの状況 pc.printf("\r\n"); /* pc.printf("%Value is "); pc.printf("Dirr_short is %d\t", ir_val_short[0]-ir_val_short[11]);//検出した最大値と最小値の差(近) pc.printf("Dirr_long is %d\t", ir_val_long[0]-ir_val_long[7]);//検出した最大値と最小値の差(遠) pc.printf("\r\n"); */ //検出した値全てを出力(バブルソートすると位置が変わるので注意) /*pc.printf("%Value is "); for(i=0; i<8; i++){ pc.printf("%d\t", ir_val_long[i]);//遠 } for(i=0; i<12; i++){ pc.printf("%d\t", ir_val_short[i]);//近 } pc.printf("\r\n"); */ } } uint16_t read_input(uint8_t ic, uint8_t channel) { uint16_t command_high = START_BIT | MODE_SINGLE | ((channel & 0x04) >> 2); uint16_t command_low = (channel & 0x03) << 6; cs[ic] = 0; ir.write(command_high); uint16_t high_byte = ir.write(command_low) & 0x0F; uint16_t low_byte = ir.write(0); wait_us(1); cs[ic] = 1; return (high_byte << 8) | low_byte;//0x0000...0xFFFF } uint8_t read_position(uint8_t ic, uint8_t channel){ //DigitalOut cs[IC_NUM] = {cs1, cs2, cs3}; static uint8_t const ir_location[3][8]= {//0,1,2...7(ch) {1, 2, 3, 4, 5, 6, 7, 8},//cs1,Long*8 {1, 2, 3, 4, 5, 6, 7, 8},//cs2,Short*8 {9, 10, 11, 12},//cs3,Short*4 }; return ir_location[ic][channel]; } void BubbleSort(uint16_t *data, uint8_t *data2, uint8_t n)//降順にする { bool flag; uint8_t i, j;//inclement uint16_t temp;//temporary for value uint8_t temp2;//temporary for position j=0; do{ j++; flag=0; for (i=0; i<n-j; i++) { if (data[i]<data[i+1]) { // 左右の並びがおかしければ入れ替える flag=1; temp=data[i]; data[i]=data[i+1]; data[i+1]=temp; temp2=data2[i]; data2[i]=data2[i+1]; data2[i+1]=temp2; } } } while (flag==1); //入れ替えがある間,繰り返す. } #endif /*IR_SENSOR*/