File content as of revision 3:cfb7cc54a3ba:

#include "mbed.h"
#include "VL53L1X.h"
#include "CONFIG.h"

#define DEBUG
#define BT_MODE

//#define ToF_MODE


#ifdef DEBUG
DigitalOut led(LED1);
#endif

Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX);        //input  (motor control)
Serial bt2(BLUETOOTH2_TX, BLUETOOTH2_RX);        //output (ToF result)

#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 = bt1.getc();
    bt1.putc(0);            //convention
    rxFlag = true;
}
#endif
int main()
{

    #ifdef BT_MODE
        bt1.baud(UART_BAUD);
        bt1.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     
        
        ///// first ToF
        Xshut1 = 1;
        Xshut2 = 0;
        Xshut3 = 0;
      
        tof.begin();
        tof.setDistanceMode();
        wait(ToF_SETUP_TIME);
        
        dis1 = tof.getDistance();
        
        
        ///// second ToF
        Xshut1 = 0;
        Xshut2 = 1;
        Xshut3 = 0;
    
        tof.begin();
        tof.setDistanceMode();
        wait(ToF_SETUP_TIME);
        dis2 = tof.getDistance();  
        
        
        ///// third ToF
        Xshut1 = 0;
        Xshut2 = 0;
        Xshut3 = 1;
        
        tof.begin();
        tof.setDistanceMode();
        wait(ToF_SETUP_TIME);

        dis3 = tof.getDistance();                   //return INTEGER
        //end of ToF com.
        
        bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
        
        // obstacle avoiding
//        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
//            bt1.puts("get\r\n");
            #endif
            
            switch(rxChar){
                case '1':                                   // 전진
                    reverseR = 0;
                    reverseL = 0;
                    
                    //need to be delayed

                    throttleR = throttleR + DAC_ONE_STEP;
                    throttleL = throttleL + DAC_ONE_STEP;
                    break;
                    
                case '2':                                   // 후진
                    reverseR = 1;
                    reverseL = 1;
                    
                    //need to be delayed
                    
                    throttleR = throttleR - DAC_ONE_STEP;
                    throttleL = throttleL + DAC_ONE_STEP;
                    break;
                    
                case '3':                                   // 좌회전
                    throttleL = throttleL + DAC_ONE_STEP;
                    break;
                    
                case '4':                                   // 우회진
                    throttleR = throttleR - 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
}