File content as of revision 5:bc5081f0c063:

/*--------------------------------------------------------------------------------
Filename: main.cpp
Description: Main program loop
--------------------------------------------------------------------------------*/
#include "mbed.h"
#include "TOF.h"
#include "Motor.h"
#include "power.h"
#include "LED.h"

Serial pc(SERIAL_TX, SERIAL_RX, 9600);    // set-up serial to pc

//TOF chip shutdown signals
DigitalOut TOF1(PC_8); 
DigitalOut TOF4(PC_11); 
DigitalOut TOF6(PC_12); 
DigitalOut TOF7(PD_2);  

//Class define
cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF6,TOF7);  //Define TOF sensor class and initialise devices
cPower cPower(VBATT, V5, V3);
cRBG_LED cRBG_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);

int main()
{
    uint8_t TOFRange[4]; //Array to store TOF measurements 0-sensor1, 1-sensor4, 2-sensor6, 3-sensor7
    float power_levels[3]; //Array to voltage levels
    
    while(1){
        
        //Perform TOF measurements
        TOFRange[0] = serviceTOF(VL6180X, ADDR1);
        TOFRange[1] = serviceTOF(VL6180X, ADDR4);
        TOFRange[2] = serviceTOF(VL6180X, ADDR6);
        TOFRange[3] = serviceTOF(VL6180X, ADDR7);
        
        Check_for_obstacles(TOFRange);
         
        //Send range to pc by serial
        //pc.printf("TOF1:%d TOF3: %d TOF5: %d TOF7: %d\r\n", TOFRange[0], TOFRange[1], TOFRange[2],TOFRange[3]);
        
        power_levels[0] = cPower.monitor_battery();
        power_levels[1] = cPower.monitor_5v_line();
        power_levels[2] = cPower.monitor_3v3_line();
        printf("BATT:%4.2f, 5V:%4.2f, 3v3:%4.2f\n\r", power_levels[0], power_levels[1], power_levels[2]);
        
        cRBG_LED.yellow_led();    

    }
}