File content as of revision 5:8ef79eebbc97:

#include "mbed.h"

#include "Sensor.h"
#include "Motor.h"

// Set up serial to pc
Serial pc(SERIAL_TX, SERIAL_RX); 

// Set up I²C on the STM32 NUCLEO-401RE 
#define addr1   (0x29) 
#define addr2   (0x2A)  
#define addr3   (0x2B)
#define addr4   (0x2C)

// VL6180x sensors
Sensor sensor1(I2C_SDA, I2C_SCL, PC_9);
Sensor sensor2(I2C_SDA, I2C_SCL, PC_11);
Sensor sensor3(I2C_SDA, I2C_SCL, PD_2);
Sensor sensor4(I2C_SDA, I2C_SCL, PG_3);

// Motors
Motor motor1(PC_6, PB_15, PB_13);
Motor motor2(PA_15, PC_7, PB_4);

void CheckObstacle()
{
    // BY DEFAULT, WHEN OBSTACLE, TURN TO THE RIGHT
    
    // When obstacle ahead
    if ( (sensor1.getIsObstacle()))
    {
        //Turn to the right
        motor1.turnRight();
        motor2.turnRight();
    }
    // When obstacle ahead and to the right 
    else if ( (sensor1.getIsObstacle()) && (sensor2.getIsObstacle()) )
    {
        //Turn to the left
        motor1.turnLeft();
        motor2.turnLeft();
    }
    // No obstacle
    else
    {
        motor1.moveForward();
        motor2.moveForward();
    }
}

int main() 
{
    int range1; 
    int range2;
    int range3;
    int range4; 

    // load settings onto VL6180X sensors 
    sensor2.init();
    // change default address of sensor 2
    sensor2.changeAddress(addr2);
    
    sensor3.init();
    // change default address of sensor 3
    sensor3.changeAddress(addr3);

    sensor4.init();
    // change default address of sensor 4
    sensor4.changeAddress(addr4);
    
    sensor1.init();
  
    while (1)
    {            
        range1 = sensor1.read();
        range2 = sensor2.read();
        range3 = sensor3.read();
        range4 = sensor4.read();         

        pc.printf("Range one = %d | range two = %d | range three = %d | range four = %d\r\n",range1, range2, range3, range4); 
        
        // TODO : better name for this method ?? 
        CheckObstacle();
              
        wait_ms(10); 
    } 
}