File content as of revision 8:27d6afa7cb7c:

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

#define DEBUG
#define BT_MODE

//#define BUTTON_MODE
#define JOYSTICK_MODE


#define ToF_MODE
#define ToF

#ifdef DEBUG
//DigitalOut led(LED1);
#endif

//reserved pin
//AnalogIn t1(SPI2_SCK);
//AnalogIn t2(SPI2_NSS);
//AnalogIn t3(SPI2_MISO);
//AnalogIn t4(SPI2_MOSI);
//AnalogIn t5(SPI3_SCK);
//AnalogIn t6(SPI3_NSS);
//AnalogIn t7(SPI3_MISO);
//AnalogIn t8(SPI3_MOSI);
//AnalogIn t9(USART3_TX);
//AnalogIn t10(USART3_RX);
//AnalogIn   t11(ADC2_t);
//AnalogIn   t12(ADC3_1_t);
//AnalogIn t13(ADC3_2_t);
//AnalogIn t14(TIM1_t);
//AnalogIn t15(TIM4_t);
//AnalogIn t16(TIM9_t);
//AnalogIn t17(TIM11_t);






Serial bt1(BLUETOOTH1_TX, BLUETOOTH1_RX);        //input  (motor control)

#ifdef ToF
Serial bt2(BLUETOOTH2_TX, BLUETOOTH2_RX);        //output (ToF result)

VL53L1X tof1(ToF1_I2C_SDA, ToF1_I2C_SCL);    //F303K8(PB_7, PB_6), F072RB(PB_9, PB_8)
VL53L1X tof2(ToF2_I2C_SDA, ToF2_I2C_SCL);
VL53L1X tof3(ToF3_I2C_SDA, ToF3_I2C_SCL);

//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;
#endif

#ifdef BUTTON_MODE

void rxData(){
    char temp = bt1.getc();
    bt1.putc(0);            //convention
    if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
        rxChar = temp;
        rxFlag = 1;
        }
}

#endif

#ifdef JOYSTICK_MODE
int x_position = 0;
int y_position = 0;

void rxData(){
    char temp[5];
//    int x_temp=0,y_temp=0;
    if('('==bt1.getc()){
            for(int i=1;i<6&&temp[i-2]!=',';i++){
                temp[i-1]=bt1.getc();
            }
            x_position=atoi(temp);
            for(int i=1;i<6&&temp[i-2]!=')';i++){
                temp[i-1]=bt1.getc();
            }
            y_position=atoi(temp);
//            bt2.printf("X:%d,Y:%d\n",x_temp,y_temp);
    }
}
#endif
int main()
{
    throttleL = 0;
    throttleR = 0; 
    #ifdef BT_MODE
        bt1.baud(UART_BAUD);
        bt1.attach(&rxData, Serial::RxIrq);     
    #endif
//    #ifdef ToF_MODE
        bt2.baud(UART_BAUD);
//        bt2.attach(&rxData2, Serial::RxIrq);
//    #endif
    int dis1, dis2, dis3;
    
    reverseR = 0;
    reverseL = 0;
   
//    bool wheelDirection = true;


    #ifdef ToF_MODE
 ///// first ToF
//      
        tof1.begin();
        tof1.setDistanceMode(2);
        while(!tof1.newDataReady());
        
        
        ///// second ToF
    
        tof2.begin();
        tof2.setDistanceMode(2);
        while(!tof2.newDataReady());
//        
//        ///// third ToF
        tof3.begin();
        tof3.setDistanceMode(2);
        while(!tof3.newDataReady());
        
        #endif
               //return INTEGER
    float speed = MIN_SPEED;
    bool forward=1;
    float speed_joystick =0,direction=0;   
    while(1) {
 
    #ifdef ToF_MODE     
        
        ///// first ToF
//        tof1.startMeasurement();
        while(!tof1.newDataReady());
        dis1 = tof1.getDistance();
//        
//        tof2.startMeasurement();
        ///// second ToF
        while(!tof2.newDataReady());
        dis2 = tof2.getDistance();  
        
//        
//        ///// third ToF
//        tof3.startMeasurement();
        while(!tof3.newDataReady());
        dis3 = tof3.getDistance();                   //return INTEGER
//        //end of ToF com.
        
        
    
        //bt1.putc(0);
       
        #endif
        
        
        #ifdef JOYSTICK_MODE     
            bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
            

            speed_joystick = (float)y_position/250;
            direction = (float)x_position/2000;
            //bt2.printf("X:%d,Y:%d\n",x_position,y_position);
            //bt2.printf("speed:%1.3f,dir:%1.3f\n",speed,direction);
            if(speed_joystick>0&&forward==0){
                forward=1;
                throttleR=0;
                throttleL=0;
                wait(TIME_DELAY_FOR_REVERSE);
                reverseR = 0;
                reverseL = 0;
                wait(1.0);
                }
            else if(speed_joystick<0&&forward==1){
                throttleR=0;
                throttleL=0;                
                forward=0;
                wait(TIME_DELAY_FOR_REVERSE);
                reverseR = 1;
                reverseL = 1;                
                wait(1.0);
                }
            if(direction<0){//left
                throttleR = abs(speed_joystick)+SPEED_CORRECTION+0.01f;
                throttleL = abs(speed_joystick)-SPEED_CORRECTION+direction;
            }
            else{//right
                throttleR = speed_joystick-direction;
                throttleL = speed_joystick+SPEED_CORRECTION;               
                }
                
        
        #endif
        
        #ifdef BUTTON_MODE
        bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
        if(rxFlag)
        {
            rxFlag=0;

            switch(rxChar){
                case '1':                                   // 전진
                    if(reverseR==1&&reverseL==1){
                        speed=MIN_SPEED;
                        throttleR=speed;
                        throttleL=speed+SPEED_CORRECTION;
                        wait(TIME_DELAY_FOR_REVERSE);
                        reverseR = 0;
                        reverseL = 0;
                    }

                    //need to be delayed
                    if(speed <= MAX_SPEED-DAC_ONE_STEP)
                        speed = speed + DAC_ONE_STEP;
                    else 
                        speed = MAX_SPEED;
                    throttleR = speed;
                    throttleL = speed+SPEED_CORRECTION;
//                    bt1.printf("Speed : %f\n",throttleR);
//                    throttleR = 1.0f;
//                    throttleL = 1.0f;
                    break;
                    
                case '2':                                   // 후진
                    if(reverseR==0&&reverseL==0){
                        speed = MIN_SPEED;
                        throttleR=0;
                        throttleL=0;
                        wait(TIME_DELAY_FOR_REVERSE);
                        reverseR = 1;
                        reverseL = 1;
                    }
                                       
                    //need to be delayed
                    if(speed <= MAX_SPEED-DAC_ONE_STEP)
                        speed = speed + DAC_ONE_STEP;
                    else 
                        speed = MAX_SPEED;
                    throttleR = speed;
                    throttleL = speed+SPEED_CORRECTION;                    
//                    throttleR = throttleR + DAC_ONE_STEP;
//                    throttleL = throttleL + DAC_ONE_STEP;
                    break;
                    
                case '3':                                   // 좌회전
                    if(reverseR==1&&reverseL==1){
                        speed = MIN_SPEED;
                        throttleR=0;
                        throttleL=0;
                        wait(TIME_DELAY_FOR_REVERSE);
                        reverseR = 0;
                        reverseL = 0;
                    }
                    if(speed > MIN_SPEED+DAC_ONE_STEP)
                        speed = speed - DAC_ONE_STEP;
                    else 
                        speed = MIN_SPEED;
                    throttleL=speed+SPEED_CORRECTION;
//                    throttleR = speed+SPEED_CORRECTION;
                    break;
                    
                case '4':                                   // 우회전
                    if(reverseR==1&&reverseL==1){
                        speed = MIN_SPEED;
                        throttleR=0;
                        throttleL=0;
                        wait(TIME_DELAY_FOR_REVERSE);
                        reverseR = 0;
                        reverseL = 0;                        
                    }                
                    if(speed > MIN_SPEED+DAC_ONE_STEP)
                        speed = speed - DAC_ONE_STEP;
                    else 
                        speed = MIN_SPEED;
                    throttleR=speed;               
//                    throttleR=0;                          
//                    throttleL = speed;
                    break;
                    
                case '5':                                   // 정지
                        speed = MIN_SPEED;
                        throttleR=0;
                        throttleL=0;
                    break;
                    
                case '6':
//                    reverseL = !reverseL;
                    if(reverseR==1&&reverseL==1){
                        speed = MIN_SPEED;
                        throttleR=0;
                        throttleL=0;
                        wait(TIME_DELAY_FOR_REVERSE);
                        reverseR = 0;
                        reverseL = 0;
                    }
                    if(speed <= MAX_SPEED-DAC_ONE_STEP)
                        speed = speed + DAC_ONE_STEP;
                    else 
                        speed = MAX_SPEED;
//                    throttleL=speed+SPEED_CORRECTION;
                    throttleR = speed;
                    break;
                case '7':
                    if(reverseR==1&&reverseL==1){
                        speed = MIN_SPEED;
                        throttleR=0;
                        throttleL=0;
                        wait(TIME_DELAY_FOR_REVERSE);
                        reverseR = 0;
                        reverseL = 0;                        
                    }                
                    if(speed <= MAX_SPEED-DAC_ONE_STEP)
                        speed = speed + DAC_ONE_STEP;
                    else 
                        speed = MAX_SPEED;
//                    throttleR=speed;               
//                    throttleR=0;                          
                    throttleL = speed;
                    break;
                default:    
//                    reverseR = 0;
//                    reverseL = 0;
//                    throttleL = 0;
//                    throttleR = 0;
                    
                    break;
                
            }

        }
        #endif
    }
//#endif
}