File content as of revision 7:1f4ef899e43e:

#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[2] = {32,32};
    if(bt1.readable()){
    for(int i=0; i<2; i++){
        temp[i] = bt1.getc();
    }
        X_position = temp[1];
        Y_position = temp[0];
    }
}
#endif
float map(int in, float inMin, float inMax, float outMin, float outMax) {
  // check it's within the range
  if (inMin<inMax) { 
    if (in <= inMin) 
      return outMin;
    if (in >= inMax)
      return outMax;
  } else {  // cope with input range being backwards.
    if (in >= inMin) 
      return outMin;
    if (in <= inMax)
      return outMax;
  }
  // calculate how far into the range we are
  float scale = (in-inMin)/(inMax-inMin);
  // calculate the output.
  return outMin + scale*(outMax-outMin);
}
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;
    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
        bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
        
        #ifdef JOYSTICK_MODE        

        if (Y_position < 32) {
    // Convert the declining Y-axis readings for going backward from 470 to 0 into 0 to 255 value for the PWM signal for increasing the motor speed
                        reverseR = 1;
                        reverseL = 1;
    throttleR = map(Y_position, 32, 0, 0, 1);
    throttleL = map(Y_position, 32, 0, 0, 1);
        }
        else if (Y_position > 32) {
    // Convert the increasing Y-axis readings for going forward from 550 to 1023 into 0 to 255 value for the PWM signal for increasing the motor speed
                        reverseR = 0;
                        reverseL = 0;
        throttleR = map(Y_position, 32, 64, 0, 1);
        throttleL = map(Y_position, 32, 64, 0, 1);
        }
  // If joystick stays in middle the motors are not moving
        else {
        throttleR = 0;
        throttleL = 0;
        }
        
        // X-axis used for left and right control
        if (X_position < 32) {
    // Convert the declining X-axis readings from 470 to 0 into increasing 0 to 255 value
        int xMapped = map(X_position, 32, 0, 0, 0.3);
    // Move to left - decrease left motor speed, increase right motor speed
        throttleL = throttleL - xMapped;
        throttleR = throttleR + xMapped;
    // Confine the range from 0 to 255
        if (throttleL < 0) {
        throttleL = 0;
        }
        if (throttleR > 255) {
        throttleR = 255;
        }
        }
        if (X_position > 32) {
    // Convert the increasing X-axis readings from 550 to 1023 into 0 to 255 value
        int xMapped = map(X_position, 32, 64, 0, 0.3);
    // Move right - decrease right motor speed, increase left motor speed
        throttleL = throttleL + xMapped;
        throttleR = throttleR - xMapped;
    // Confine the range from 0 to 255
        if (throttleL > 255) {
        throttleL = 255;
        }
        if (throttleR < 0) {
        throttleR = 0;
        }
        }
        
        if (throttleR < 0.4) {
        throttleR = 0;
        }
        if (throttleL < 0.4) {
        throttleL = 0;
        }
        #endif
        
        #ifdef BUTTON_MODE

        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
}