File content as of revision 8:7b9d70751bdf:

#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,   3500,   },//IR_SHORT 0
        {0, 1000,   1100,   1200,   1300,   1500,   1700,   1900,   },//IR_LONG 1
    };
    
    
    static uint16_t const ir_dif_range[2][8]={
        {0, 500,   750,   1000,   1250,   1500,   1600,   1800,   },//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_LONG]-1;
        }
        else{
            ir_place = ir_position[IR_SHORT]+7;
        }
        
        //デバッグ用の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("ir_place:%d\t", ir_place);
        
        
        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*/