#include "mbed.h"
#include "TextLCD.h"
TextLCD lcd(p21, p22, p23, p24, p25, p26, p29,16,2);
I2C i2c(p9, p10); // sda, scl
Serial pc(USBTX, USBRX); // tx, rx

const int addr = 0x3C; // define the I2C Address
char cmd[3];
char read[8];
float x;
float y;
float z;
float echo;


int main() {
cmd[0] = 0x3C;
cmd[1] = 0x02;
cmd[2] = 0x00;
i2c.write(addr, cmd, 3);
wait(1);
while (1) {
lcd.cls();

i2c.read(addr, read, 8);
echo =((read[0] << 8)|read[1]);
x = echo;

echo =((read[2] << 8)|read[3]);
y = echo;

echo =((read[4] << 8)|read[5]);
z = echo;

lcd.printf("%.1f %.1f\n%.1f",x,y,z);
pc.printf("%.1f %.1f %.1f\n",x,y,z);
wait(1);
}

}