#include "mbed.h"

Serial pc(USBTX,USBRX);     // Tx, Rx Pin, default boud = 9600
I2C i2c(p9, p10);          // SDA, SCL

int main()
{
    char cmd[2];            // Command
    char echo[2];           // Echo result data (8bit)
    char echo2[2];
    int  addr  = 0xE0;      // Defalut I2C address of SRF08
    float  range = 0;         // Rage data (16bit, cm)
    float  range2 = 0;         // Rage data (16bit, cm)

    i2c.frequency(40000);
    while(1) {
        cmd[0] = 0x00;          // Register(Command)
        cmd[1] = 0x51;          // Ranging mode - Result in centimeters
        if(!i2c.write(addr,cmd,2)) {
            pc.printf("Raging ACK ");
            wait(0.17);
            cmd[0] = 0x02;          // Register(1st Echo)
            if(!i2c.write(addr,cmd,1,1)) {
                // Send I2C data (8bit*1)
                pc.printf("1st echo write ACK ");
                if(!i2c.read(addr,echo,2)) {
                    pc.printf("1st echo read ACK ");
                    int cm = ((echo[0]<<8)+echo[1]);
                    range = ((echo[0]<<8)+echo[1]);
                    pc.printf("Range : %d cm \n",cm);
                } else {
                    pc.printf("1st echo read NOT ACK\n");
                }

            } else {
                pc.printf("1st echo write NOT ACK\n");
            }
        } else {
            pc.printf("Raging NOT ACK\n");
        }
        wait(0.5);
    }
}