File content as of revision 1:d481b2912c2a:

#include "mbed.h"
#include "m3pi.h"

DigitalOut sensorLED(p8);
I2C colorSensor(p28, p27);
//Serial bt(p13, p14); // tx, rx

const int addr = 0x74;
m3pi m3pi;



int main() {

    char cmd[2];
    //sensorLED = 1;
    m3pi.locate(0,1);
    m3pi.printf("Start");
    wait(1);
    
    while(1) {


        cmd[0] = 0x00;
        cmd[1] = 0x01;
        colorSensor.write(addr,cmd,2);

        int result_low;
        int result_high;
        
        
        //Red
        cmd[0] = 0x40;
        wait(.1);
        colorSensor.write(addr,cmd,1);
        colorSensor.read(addr, cmd,1);
        result_low = (int) cmd[0];
        
        cmd[0] = 0x41;
        wait(.1);
        colorSensor.write(addr,cmd,1);
        colorSensor.read(addr, cmd,1);
        result_high = (int) cmd[0];
        
        int red = (result_high << 8) + result_low;
        
        //Green
        cmd[0] = 0x42;
        wait(.1);
        colorSensor.write(addr,cmd,1);
        colorSensor.read(addr, cmd,1);
        result_low = (int) cmd[0];
        
        cmd[0] = 0x43;
        wait(.1);
        colorSensor.write(addr,cmd,1);
        colorSensor.read(addr, cmd,1);
        result_high = (int) cmd[0];
        
        int green = (result_high << 8) + result_low;
        
        //Blue
        cmd[0] = 0x44;
        wait(.1);
        colorSensor.write(addr,cmd,1);
        colorSensor.read(addr, cmd,1);
        result_low = (int) cmd[0];
        
        cmd[0] = 0x45;
        wait(.1);
        colorSensor.write(addr,cmd,1);
        colorSensor.read(addr, cmd,1);
        result_high = (int) cmd[0];
        
        int blue = (result_high << 8) + result_low;

        // Print for debugging
        
        //bt.printf("R:%d",red);
        //bt.printf("G:%d",green);
        //bt.printf("B:%d",blue);
        
        m3pi.cls();
        m3pi.locate(0,1);
        m3pi.printf("R:%d",red);
        wait(1.5);
        
        m3pi.cls();
        m3pi.locate(0,1);
        m3pi.printf("G:%d",green);
        wait(1.5);
        
        m3pi.cls();
        m3pi.locate(0,1);
        m3pi.printf("B:%d",blue);    
        wait(1.5);    
    }
}