File content as of revision 2:fae63e87900e:

#include <mbed.h>
#include "rtos.h"
#include "Periphery/SupportSystem.h"
#include "Misc/SystemTimer.h"

Serial serialXbee(p28,p27);
Serial serialMinnow(p13, p14);
PwmOut drivePWM(p22);
PwmOut steerPWM(p21);

DigitalOut heartbeatLED(LED4);
DigitalOut ledTwo(LED1);


void hearbeat_thread(void const *args);

int main() {
    
    serialMinnow.baud(115200);
    
    SystemTimer *millis = new SystemTimer();
    I2C *i2c = new I2C(p9, p10);
    
    SupportSystem *supportSystem = new SupportSystem(0x80, i2c);
    
    drivePWM.period_ms(20);
    steerPWM.period_ms(20);
    
    Thread hearbeatThread(&hearbeat_thread);

    IMU_RegisterDataBuffer_t *imuRegisterDataBuffer;

    while(true){
    
        ledTwo = !ledTwo;
        imuRegisterDataBuffer = supportSystem->getImuRegisterDataBuffer();
        
        
        serialMinnow.printf("Distance X: %f\n\r", imuRegisterDataBuffer->distanceXRegister); 
        serialMinnow.printf("Squal: %d\n\r", imuRegisterDataBuffer->squal);     
        serialMinnow.printf("Uptime %d\n\r", supportSystem->readMaintenanceUptimeMillis());
        serialMinnow.printf("Error: %d\n\r", supportSystem->getTransmissionErrorCount());
        serialMinnow.printf("\n\r");
        
        wait(0.100);
    }
    
}


void hearbeat_thread(void const *args) {
    while(true) {
        heartbeatLED = !heartbeatLED;
        Thread::wait(200);
    }
}