File content as of revision 1:5b2e1ee5e63e:

/*
 *main用LPC1768と通信するSensor用LPC1768のプログラム()
 *
 *現在は2つのLPC1768でSerial通信します．
 * 
 */
 
#include "mbed.h"
 
#include "IRM2121.h"
#include "Ping.h"
#include "MultiSerial.h"
#include "HMC6352.h"
#include "Watchdog.h"
 
#define PING_COUNT 5 //5回に1回にPing処理
#define IR_LEVEL 400 //irのしきい値
#define ADDRESS 0xAA//
#define DATA_NUM 9
 
uint8_t tx_data[DATA_NUM]={0};

IRM2121 Ir[12] = {p20,p19,p18,p17,p16,p15,  p12,p11,  p8,p7,p6,p5};

Ping Ping1(p22, p21);
Ping Ping2(p24, p23);
Ping Ping3(p26, p25);
Ping Ping4(p30, p29);

DigitalOut Led[4] = {LED1, LED2, LED3, LED4};

HMC6352 hmc6352(p28, p27);

MultiSerial Mbed(p13, p14);
//MultiSerial mbed(p9, p10);

Serial pc(USBTX,USBRX);
 
/*irm2121平均化*/
uint8_t IrMoving_ave(uint8_t num, int data){
    static unsigned int sum[10];
    static unsigned temp[6][10];
    
    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;
    
    return  sum[num]/10;   
}
 
void IrSort(unsigned int input[], uint8_t output[]){
    
    /*
     *1,2位の値とその場所の番号をアウトプットに格納する.
     *値が255以上だとボールを検知してないことになる。
     */
    unsigned int  near[2] = {1000,1000};
    uint8_t num[2] = {12,0}, i;
    static int sub[12]={0};
    
    for(i = 0; i < 12; i++ ){
        if(!input[i]){
            continue;
        }
        
        if(near[1] < input[i]+sub[i]){
            continue;
        }
        if(near[0] < input[i]+sub[i]){
            near[1] = input[i]+sub[i];
            num[1] = i;
            continue;
        }
        near[1] = near[0];
        num [1]  = num[0];
        near[0] = input[i]+sub[i];
        num [0]   = i;
    }
 
    if(num[0] == 12){//not found
        output[0] = 255;
        output[1] = 255;
        output[2] = 255;
        return;
    }
    if(num[1] == 12){
        num[1] = 0;
    }
    /*
    for(i = 0; i < 12; i++ ){
        
        if(i == num[0]){
            
            sub[i]++;
            continue;
        }
        if(sub[i]>0){
            
            sub[i]--;
        }
    }
    */
    output[0] = num[0]*12 + num[1];//rank1*12 + rank2
    output[1] = near[0] /10;//rank1
    output[2] = near[1] /10;//rank2
    
}
 
 
 
int main() {
    
    /*begin SetUp*/
    Watchdog g;
    g.Configure(1.0f);//1秒でWDT発生
    /*end   Setup*/
    
    
    /*Count*/
    uint8_t PingCk = 0;
    
    /*Data*/
    unsigned int    PingData[4] = {0};
    unsigned int    IrBase[12] = {0};
    uint8_t         IrData[3] = {0};
    uint16_t        Compass=0;
    
    /*Serial*/
    
    
    Mbed.start_write();
    Mbed.write_data(tx_data, ADDRESS, DATA_NUM);
    
    //Continuous mode, periodic set/reset, 20Hz measurement rate.
    hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
 
    while(1) {
        
        g.Service();
        
        PingCk ++;
        
        Led[0] = 1;//周回の初め    
        
        
        
        IrBase[0]  = Ir[0].Read();
        IrBase[1]  = Ir[1].Read();
        IrBase[2]  = Ir[2].Read();
        IrBase[3]  = Ir[3].Read();
        IrBase[4]  = Ir[4].Read();
        IrBase[5]  = Ir[5].Read();
        IrBase[6]  = Ir[6].Read();
        IrBase[7]  = Ir[7].Read();
        IrBase[8]  = Ir[8].Read();
        IrBase[9]  = Ir[9].Read();
        IrBase[10] = Ir[10].Read();
        IrBase[11] = Ir[11].Read();
        
        /*平均化
        IrTemp[0] = IrMoving_ave(0,IrTemp[0]);
        IrTemp[1] = IrMoving_ave(1,IrTemp[1]);
        IrTemp[2] = IrMoving_ave(2,IrTemp[2]);
        IrTemp[3] = IrMoving_ave(3,IrTemp[3]);
        IrTemp[4] = IrMoving_ave(4,IrTemp[4]);
        IrTemp[5] = IrMoving_ave(5,IrTemp[5]);
        */
        
        IrSort(IrBase,IrData);
        
        if(PingCk == 2){
            
            Ping1.Send();    
            wait_ms(30);
            PingData[0] = Ping1.Read_cm();
            if(PingData[0]>0xFF) PingData[0]=0xFF;
        }
        if(PingCk == 3){
            
            Ping2.Send();    
            wait_ms(30);
            PingData[1] = Ping2.Read_cm();
            if(PingData[1]>0xFF) PingData[1]=0xFF;
        }
        if(PingCk == 4){
            
            Ping3.Send();    
            wait_ms(30);
            PingData[2] = Ping3.Read_cm();
            if(PingData[2]>0xFF) PingData[2]=0xFF;
        }
        if(PingCk >= PING_COUNT){
            
            Ping4.Send();    
            wait_ms(30);
            PingData[3] = Ping4.Read_cm();
            if(PingData[3]>0xFF) PingData[3]=0xFF;
            
            PingCk = 0;
        }
        
        Compass = hmc6352.sample();
        
        //ex.
        tx_data[0] = IrData[0];
        tx_data[1] = IrData[1];
        tx_data[2] = IrData[2];
        tx_data[3] = PingData[0];
        tx_data[4] = PingData[1];
        tx_data[5] = PingData[2];
        tx_data[6] = PingData[3];
        tx_data[7] = (Compass & 0x00FF)>>0;
        tx_data[8] = (Compass & 0xFF00)>>8;
        
        
        for(int i=0; i<7; i++){
            pc.printf("%d\t", tx_data[i]);
        }
        
        pc.printf("%d\t", Compass);
        
        pc.printf("\n");
        
        
        Led[0] = 0;//周回の終わり
        
        wait_ms(1);
    }
}