File content as of revision 2:284491e0f6bf:

#include "mbed.h"
#include "VL53L1X.h"
#include "CONFIG.h"
#define DEBUG
//#define DEBUG_DAC
#define BT_MODE
//#define ToF_MODE
#ifdef DEBUG
DigitalOut led(LED1);
#endif

Serial bt(BLUETOOTH_TX, BLUETOOTH_RX);
//Serial pc(USBTX, USBRX);

#ifdef ToF
VL53L1X tof(ToF_I2C_SDA, ToF_I2C_SCL);    //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8)

DigitalOut Xshut1(ToF_XShut_1);
DigitalOut Xshut2(ToF_XShut_2);
DigitalOut Xshut3(ToF_XShut_3);
#endif

DigitalOut reverseR(MOTOR_REVERSE_R);
DigitalOut reverseL(MOTOR_REVERSE_L);

AnalogOut throttleR(MOTOR_THROTTLE_R);
AnalogOut throttleL(MOTOR_THROTTLE_L);

Timer timer;
#ifdef BT_MODE
char rxChar = 0;
bool rxFlag = false;

void rxData(){
    rxChar = bt.getc();
    bt.putc(0);
    rxFlag = true;
}
#endif
int main()
{
#ifdef DEBUG_DAC
    while(1){
        throttleR = 1;
        throttleL = 1;
        wait(2);
        throttleR = 0;
        throttleL = 0;
        wait(2);
        
    }
#else
    #ifdef BT_MODE
        bt.baud(UART_BAUD);
        bt.attach(&rxData, Serial::RxIrq);
    #endif
    #ifdef ToF_MODE
    int dis1, dis2, dis3;
    
    reverseR = 0;
    reverseL = 0;
    
    bool wheelDirection = true;
    #endif
    while(1) {
 
    #ifdef ToF_MODE       
        Xshut1 = 1;
        Xshut2 = 0;
        Xshut3 = 0;
      
        tof.begin();
        tof.setDistanceMode();
        wait(ToF_SETUP_TIME);
        
        dis1 = tof.getDistance();
//        bt.printf("dis1: %d\n", tof.getDistance());
        
        Xshut1 = 0;
        Xshut2 = 1;
        Xshut3 = 0;
    
        tof.begin();
        tof.setDistanceMode();
        wait(ToF_SETUP_TIME);
        dis2 = tof.getDistance();  
        bt.printf("dis2: %d\n", tof.getDistance() );
        
    
        Xshut1 = 0;
        Xshut2 = 0;
        Xshut3 = 1;
        
        tof.begin();
        tof.setDistanceMode();
        wait(ToF_SETUP_TIME);

        dis3 = tof.getDistance();          
//        bt.printf("dis3: %d\n", tof.getDistance() );
        
        if( dis2 > HIGH_BOUND ){
            if(!wheelDirection){
                throttleL = 0;
                throttleR = 0;                
            reverseR = 0;
            reverseL = 0;
                wait(1.2f);
            wheelDirection = true;

            }

            throttleL = 0.9f;
            throttleR = 0.9f;
        }
        
        else if ( dis2 < LOW_BOUND ){
            if(wheelDirection){
                throttleL = 0;
                throttleR = 0;                

                reverseR = 1;
                reverseL = 1;
                wait(1.2f);
                wheelDirection = false;
            }

            wait(0.01f);            

            throttleL = 0.9f;
            throttleR = 0.9f;
        }
        else{
            throttleL = 0;
            throttleR = 0;            
        }
        #endif
    
        #ifdef BT_MODE
        if(rxFlag)
        {
            #ifdef DEBUG
            bt.puts("get\r\n");
            #endif
            
            switch(rxChar){
                case '1':
                    throttleR = throttleR + DAC_ONE_STEP;
                    break;
                    
                case '2':
                    throttleR = throttleR - DAC_ONE_STEP;
                    break;
                    
                case '3':
                    throttleL = throttleL + DAC_ONE_STEP;
                    break;
                    
                case '4':
                    throttleL = throttleL - DAC_ONE_STEP;
                    break;
                    
                case '5':
                    reverseR = !reverseR;
                    break;
                    
                case '6':
                    reverseL = !reverseL;
                    break;
                    
                default:    
                    reverseR = 0;
                    reverseL = 0;
                    throttleL = 0;
                    throttleR = 0;
                    break;
                
            }
            rxFlag = false;
        }
        #endif
    }
#endif
}