File content as of revision 1:ed00e9c4ba6e:

#include "mbed.h"
#include "TextLCD.h"
#include "main.h"

int ave5point(int source)
{
    static int tmp[5] = {0};
    static int sum = 0;
    
    sum -= tmp[4];
    sum += source;
    tmp[4] = tmp[3];
    tmp[3] = tmp[2];
    tmp[2] = tmp[1];
    tmp[1] = tmp[0];
    tmp[0] = source;
    
    for(uint8_t i = 0; i < 5; i++){
        if((tmp[i] > 180) || (tmp[i] < 10)){
            return 0xFF;
        }
    }
    
    return (int)(sum / 5.0);

}

void init()
{
    device.baud(BAUD_RATE);
    device.printf("START");
    device2.printf("START"); 
    device.attach(&dev_tx,Serial::TxIrq);
    device.attach(&dev_rx,Serial::RxIrq);
    device2.attach(&dev2_tx,Serial::TxIrq);
    device2.attach(&dev2_rx,Serial::RxIrq);
    Button.mode(PullUp);
    
    timer2.start();
    
    lcd.cls();
}

int main() 
{
    uint8_t flag = 0,state = 0;

    init();
    
    for(;;){
        if((!Button) && (!flag)){
            wait(1);
            flag = 1;
            state = 1;
        }else if((!Button) && (flag)){
            wait(1);
            flag = 0;
            state = 0;
        }
        
        mbedleds = 0;
        
        Ultrasonic();
        IR_Position();
        AveDistance = ave5point(Distance);
        
        lcd.cls();
        
        if(state){
            lcd.locate(0,0);
            lcd.printf("R:%d,F:%d",(int)ultrasonicValue[1],(int)ultrasonicValue[0]);
            lcd.locate(0,1);
            lcd.printf("L:%d,B:%d",(int)ultrasonicValue[3],(int)ultrasonicValue[2]);
        }else{
            lcd.locate(0,0);
            lcd.printf("%d,%d",direction,data[2]);//data[2]
            lcd.locate(0,1);
            lcd.printf("%d,%d",AveDistance,data2[0]);
        }
        /*
        pc.printf("R:%d,\tF:%d\t",(int)ultrasonicValue[1],(int)ultrasonicValue[0]);
        pc.printf("L:%d,\tB:%d\n",(int)ultrasonicValue[3],(int)ultrasonicValue[2]);
        */
        /*
        lcd.locate(0,0);
        lcd.printf("%d",(data[0] << 8) + data[1]);
        lcd.locate(0,1);
        lcd.printf("%d",data[2]);
        */
        /*
        lcd.locate(0,0);
        lcd.printf("%d,%d",direction,data[3]);//data[2]
        lcd.locate(0,1);
        lcd.printf("%d",AveDistance);
        */
        /*
        lcd.locate(0,0);
        lcd.printf("%d",data[0]);
        lcd.locate(0,1);
        lcd.printf("%d",data[1]);*/
    }
}