File content as of revision 13:d6374484b953:

#include "mbed.h"
#include "M3Dpi.h"
#include "jsonReporter.h"
#include "binaryReporter.h"

const char M3DPI_ID = "1234567890";

M3Dpi robot;
Serial pc(USBTX,USBRX);
Reporter* report = new BinaryReporter(&pc, M3DPI_ID);

m3dpi::Distance distance;
m3dpi::Direction direction;
m3dpi::Rotation rotation;
m3dpi::Acceleration acceleration;


void setLeds(m3dpi::Distance distance)
{
    int colors[8] = {distance.front, distance.front_right, distance.right, distance.back_right, distance.back, distance.back_left, distance.left, distance.front_left};
    // shift distance value to get red colors
    for(int i = 0; i < 8; i++){
        colors[i] = colors[i] << 16;
    }
    robot.setLeds(colors);
}

void read_sensors_thread()
{
    robot.setStatus(Color::GREEN);
    ::distance = robot.getDistance();
    direction = robot.getDirection();
    rotation = robot.getRotation();
    acceleration = robot.getAcceleration();
    setLeds(::distance);
    robot.setStatus(Color::BLACK);
}

int main()
{            
    pc.baud(115200);
           
    while(true){
        Timer t; t.start();
        read_sensors_thread();
        report->distance(::distance);
        report->direction(direction);
        report->rotation(rotation);
        report->acceleration(acceleration);
        t.stop();pc.printf("Read and report time: %d us\r\n", t.read_us());
        //pc.printf("report send time: %d us\n", t.read_us());
        //report.time(robot.getTime());
        wait_ms(500);
    }
}