File content as of revision 0:e742eb5a0f3e:

#include "mbed.h"
#include "HMC6352.h"
#include "Ping.h"
#include "common.h"

#define ALL_IR 11
#define BREAK_TIME_IR 833
#define ERROR_IR 0x9F6 //=2550
#define NOT_F 255
#define BUT_WAIT        0.3 //s
#define ENTER   0
#define EXIT    1
Serial sensor(p13,p14);
Serial pc(USBTX,USBRX);
HMC6352 compass(p9,p10);
Ping PING_F(p5);    /* 超音波距離センサのピンを設定 */
Ping PING_L(p6);
Ping PING_B(p7);
Ping PING_R(p8);
Timer timer_ir;
DigitalIn CalibEnterButton(p15);



DigitalIn ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30,p12};
DigitalOut myled[4] = {LED1,LED2,LED3,LED4};

extern void micon_tx();
uint8_t compass_data[2] = {0};
uint8_t ping_data[4] = {0};//DENGER: VALUE is N/A  end
uint8_t ir_min = NOT_F, ir_num = NOT_F;//data format
uint8_t ir_main = 0;
const uint8_t flag_case[ALL_IR] = {1,1,1,1,1,1,1,1,1,1,1};// use ir_flag

unsigned int moving_ave(uint8_t num, unsigned int data){
    static unsigned int sum[ALL_IR] = {0};
    static unsigned int temp[ALL_IR][10] = {0};
    
    sum[num] -= temp[num][9];
    sum[num] += data;
    temp[num][9] = temp[num][8];
    temp[num][8] = temp[num][7];
    temp[num][7] = temp[num][6];
    temp[num][6] = temp[num][5];
    temp[num][5] = temp[num][4];
    temp[num][4] = temp[num][3];
    temp[num][3] = temp[num][2];
    temp[num][2] = temp[num][1];
    temp[num][1] = temp[num][0];
    temp[num][0] = data;
    //sum[num]+=modulate[num];
    
    //return ir_data[num] = sum[num]/100;
    return sum[num]/10;//return ir_value
}

unsigned int ir_function (uint8_t num){
    uint8_t flag = 0;
    unsigned int memory = 0;
    unsigned int data = 0;
       
    flag = flag_case[num];
    timer_ir.start();
    if(ir[num]){
        while(ir[num]){
            if(timer_ir.read_us() >= BREAK_TIME_IR){
                flag = 0;
                break;
            }
        }
    }
    timer_ir.stop();
    timer_ir.reset();
    
    if(flag){
        timer_ir.start();
        while(!ir[num]){
            if(timer_ir.read_us() >= BREAK_TIME_IR){
                break;
            }
        }
        memory = timer_ir.read_us();
        while(1){
            if(!ir[num]){
                data = (timer_ir.read_us() - memory);
                timer_ir.stop();
                timer_ir.reset();
                //wait(0.01);
                return data;
            }
            if((timer_ir.read_us()-memory)>2550){
                break;
            }
        }
    }
    timer_ir.stop();
    timer_ir.reset();
    
    return ERROR_IR;
}
void ir_fun(unsigned int ir_data[]){
    uint8_t num;
    uint8_t i;//count
    unsigned int min;
    
    min = ERROR_IR;
    num = NOT_F;
    
    for (i = 3;i < (ALL_IR+2) ;i++){
        if(i ==10){
            if(min > ir_data[0]){
                min = ir_data[0];
                num = 0;
            }
        }else if(i == 11){
            if(min > ir_data[1]){
                min = ir_data[1];
                num = 1;
            }
        }else if(i == 12){
            if(min > ir_data[2]){
                min = ir_data[2];
                num = 2;
            }
        }else{
            if(min > ir_data[i]){
            min = ir_data[i];
            num = i;
            }
       }
    }
    if(num == 2){
        if(ir_data[5]<ir_data[1]) num = 3;
    }
    if(num == 4){
        if(ir_data[1]<ir_data[5]) num = 3;
    }
    
    ir_min = min/10;
    ir_num = num;


}
int main(void){
    
    int compassdef = 0,compass_A = 0;/*compass_A = original_value,used compass_data*/
    uint8_t i = 0;
    unsigned int ir_data[ALL_IR] = {0};  //DENGER: VALUE is N/A

    
    
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    compassdef = (compass.sample() / 10);
    sensor.attach(&micon_tx,Serial::TxIrq);//送信空き割り込み設定
    sensor.putc(1);                         //送信開始+
    
    CalibEnterButton.mode(PullUp);
        
    if(!CalibEnterButton){
            myled[3] =1;
            compass.setCalibrationMode(ENTER);
            while(!CalibEnterButton);
            compass.setCalibrationMode(EXIT);
            wait(BUT_WAIT);
            myled[3] = 0;
         }
    while(1){
        compass_A = ((compass.sample() / 10) + 540 - compassdef) % 360;
        if(compass_A >255 ){
            compass_data[0] = 255;
            compass_data[1] = compass_A - 255 ;
        }else{
            compass_data[0] = compass_A;
            compass_data[1] = 0;
        }
         /* 超音波発射 */
        PING_F.Send();
        PING_L.Send();
        PING_B.Send();
        PING_R.Send();
 
        wait_ms(30);    // 待つ
 
        /* 結果から距離を算出 */
        ping_data[FRONT] = PING_F.Read_cm() / 2;  // FRONT距離を記録 [cm]
        ping_data[LEFT]  = PING_L.Read_cm() / 2;  // LEFT距離を記録 [cm]
        ping_data[BACK]  = PING_B.Read_cm() / 2;  // BACK距離を記録 [cm]
        ping_data[RIGHT] = PING_R.Read_cm() / 2;  // RIGHT距離を記録 [cm]
        
            // PING_F.Send();  wait_ms(30);  Ping_F=PING_F.Read_cm()/2;
            // PING_R.Send();  wait_ms(30);  Ping_R=PING_R.Read_cm()/2;
            // PING_B.Send();  wait_ms(30);  Ping_B=PING_B.Read_cm()/2;
            // PING_L.Send();  wait_ms(30);  Ping_L=PING_L.Read_cm()/2;
            /* アクセス可能になるまで待機してから、値を代入 */
            /*PING_slots.wait();
                Ping_data[0] = Ping_F;
                Ping_data[1] = Ping_R;
                Ping_data[2] = Ping_B;
                Ping_data[3] = Ping_L;
            PING_slots.release();
            */
            //wait(0.1);
        

        for( i = 0; i < ALL_IR; i++){/*赤外線データの読み取り*/
             ir_data[i] = moving_ave(i,ir_function(i));
        }
        
        ir_fun(ir_data);
        ir_main = ir_data[10]/10;
        //
        //pc.printf("%d\t%d\n",ir_function(8), (moving_ave(8,ir_function(8))));
        //pc.printf("%d\n",ir_data[8]);
        //pc.printf("compass: %d\n",compass_A);//check finish
        //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping_data[0],ping_data[1],ping_data[2],ping_data[3]);
        //pc.printf("ir_min:%d\tir_num:%d\n",ir_min,ir_num);
        //pc.printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d 9:%d \n",ir_data[0],ir_data[1],ir_data[2],ir_data[3],ir_data[4],ir_data[5],ir_data[6],ir_data[7],ir_data[8],ir_data[9]);

        
    }
    
    
    
}