File content as of revision 2:6197e1cf2cd1:

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

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

//TOF chip shutdown signals
DigitalOut TOF1(PC_8);
DigitalOut TOF3(PC_10);
DigitalOut TOF5(PC_12);
DigitalOut TOF7(PG_2);

cAdafruit_VL6180X VL6180X(TOF1,TOF3,TOF5,TOF7);  //Define TOF sensor class and initialise devices
cMotor LMotor(M1_PWM, M1_IN1, M1_IN2), RMotor(M2_PWM, M2_IN1, M2_IN2);

/*--------------------------------------------------------------------------------
Function name: ServiceTOF
Input Parameters: address - address of target TOF sensor
Output Parameters: range - distance measurement in mm
Description: performs measurement routine on a given sensor
----------------------------------------------------------------------------------*/
uint8_t serviceTOF(uint8_t address){
        
        uint8_t range = 0;
        
        // poll the VL6180X till new sample ready
        VL6180X.Poll_Range(address);

        // read range result
        range = VL6180X.Read_Range(address);
        
        // clear the interrupt on VL6180X
        VL6180X.Clear_Interrupts(address);
        
        return range;
}


/*--------------------------------------------------------------------------------
Function name: main
Input Parameters: N/A
Output Parameters: N/A
Description: Main programming loop
----------------------------------------------------------------------------------*/
int main()
{
    uint8_t TOFRange[4]; //Array to store TOF measurements 0-sensor1, 1-sensor3, 2-sensor5, 3-sensor7
    
    while(1) {
        //LMotor.Backwards(0.2);
        //RMotor.Backwards(0.2);    
        
        
        //Perform TOF measurements
        TOFRange[0] = serviceTOF(ADDR1);
        TOFRange[1] = serviceTOF(ADDR2);
        TOFRange[2] = serviceTOF(ADDR3);
        TOFRange[3] = serviceTOF(ADDR4);
                
        //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]);
        
        //Short delay
        wait(0.1);
        
    }
}