File content as of revision 4:97a11deb0ab7:

#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;
}
//void rxData2(){
//    rxChar = bt2.getc();
//    bt2.putc(0);            //convention
//    rxFlag = true;
//}
#endif
int main()
{
    throttleL = 0;
    throttleR = 0; 
    #ifdef BT_MODE
        bt1.baud(UART_BAUD);
        bt2.baud(UART_BAUD);
        bt1.attach(&rxData, Serial::RxIrq);
        //bt2.attach(&rxData2, Serial::RxIrq);
    #endif

    int dis1, dis2, dis3;
    
    reverseR = 0;
    reverseL = 0;
   
    bool wheelDirection = true;

    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.
        
        
    
        //bt1.putc(0);
       
        #endif
        //bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
      #ifdef BT_MODE
        if(rxFlag)
        {
            #ifdef DEBUG
//            bt1.puts("get\r\n");
            #endif
            
//            if(!(rxChar=='1'||rxChar=='2'||rxChar=='3'||rxChar=='4'||rxChar=='5'||rxChar=='6'))
//                rxChar=rxChar_prev;
            
            switch(rxChar){
                case '1':                                   // 전진
                    if(reverseR==1&&reverseL==1){
                        throttleR=0;
                        throttleL=0;
                        wait(0.1);
                    }
                    reverseR = 0;
                    reverseL = 0;
                    
                    //need to be delayed

                    throttleR = throttleR + DAC_ONE_STEP;
                    throttleL = throttleL + DAC_ONE_STEP;
//                    throttleR = 1.0f;
//                    throttleL = 1.0f;
                    break;
                    
                case '2':                                   // 후진
                    if(reverseR==0&&reverseL==0){
                        throttleR=0;
                        throttleL=0;
                        wait(0.1);
                    }
                    
                    reverseR = 1;
                    reverseL = 1;
                    
                    //need to be delayed
                    
                    throttleR = throttleR + DAC_ONE_STEP;
                    throttleL = throttleL + DAC_ONE_STEP;
                    break;
                    
                case '3':                                   // 좌회전
                    if(reverseR==1&&reverseL==1){
                        throttleR=0;
                        throttleL=0;
                        wait(0.1);
                        reverseR = 0;
                        reverseL = 0;
                    }

                    throttleR=0;
                    throttleL = throttleL + DAC_ONE_STEP;
                    break;
                    
                case '4':                                   // 우회전
                    if(reverseR==1&&reverseL==1){
                        throttleR=0;
                        throttleL=0;
                        wait(0.1);
                        reverseR = 0;
                        reverseL = 0;                        
                    }                
                    throttleL=0;                          
                    throttleR = throttleR + DAC_ONE_STEP;
                    break;
                    
                case '5':                                   // 정지
                        throttleR=0;
                        throttleL=0;
                    break;
                    
                case '6':
//                    reverseL = !reverseL;
                    break;
                    
                default:    
//                    reverseR = 0;
//                    reverseL = 0;
//                    throttleL = 0;
//                    throttleR = 0;
                    
                    break;
                
            }
            rxFlag = false;
        }
        #endif
    }
//#endif
}