안드로이드 앱에서 명령을 내려 아두이노 rc카를 조종하는 걸 만들고 있습니다.

안드로이드 소스에서는 bluetoothSPP를 이용하여 아두이노와 블루투스 통신을 합니다.

if (blockName == "GO") {
    Bluetooth.send("G" + num1, false);

}

예를들어 블록에 GO와 시간초에 num1이 들어가면 아두이노에 "G" 와 num1을 전달해주는데요.

아두이노 코드는 이렇습니다.

#include<SoftwareSerial.h>
#include <Servo.h>
#define TRIG 9
#define ECHO 10

int blueTx=6;   //Tx (보내는핀 설정)at
int blueRx=7;   //Rx (받는핀 설정)
SoftwareSerial BT(blueTx,blueRx);

Servo servo;

int motor1_input1=2;
int motor1_input2=3;
int motor2_input1=4;
int motor2_input2=5;

int servo_motor=8;
const int init_servo_angle = 90;
const int left_servo_angle = 100;
const int right_servo_angle = 40;

bool Mode;
bool check;

char input;
int input2;
int sec;

void setup() {
  BT.begin(9600);
  Serial.begin(9600);
  
  pinMode(motor1_input1,OUTPUT);
  pinMode(motor1_input2,OUTPUT);
  pinMode(motor2_input1,OUTPUT);
  pinMode(motor2_input2,OUTPUT);
  
  servo.attach(servo_motor);
  servo.write(init_servo_angle);

  // 초음파 센서
  pinMode(TRIG,OUTPUT);
  pinMode(ECHO, INPUT);
           
  Mode = false; //false(수동), true(자동)

 
 
}

void loop() {
 
 long duration, distance;
 digitalWrite(TRIG, LOW);
 delayMicroseconds(2);
 digitalWrite(TRIG, HIGH);
 delayMicroseconds(10);
 digitalWrite(TRIG, LOW); 
 duration = pulseIn (ECHO, HIGH);  
 distance = duration * 17 / 1000;
  

  if(BT.available()){
    

    input = BT.read();
     if(input == 'A' || input == 'M' || input == 'C'){
      switch(input){
        case 'A': //자동주행       
          break; 
            
        case 'M': //수동주행
          Mode=false;
          break; 
            
        case 'C': //블록코딩 
          Mode=true;
          break;                          
      }
    }else if(Mode==false){
      Manual_Drive(input);
    }
    else if(Mode==true){
      
       sec =(int)BT.read()-48;
                     
        if(sec<0){
          input2=0;
        }
        else if(sec>9){
          input2=9;
        }
        else{
          input2=sec;
        }
        Block_Coding(input,input2); 
    }
    
  }
 }

 
void GO(){
  
  digitalWrite(motor1_input1,HIGH);
  digitalWrite(motor1_input2,LOW);
  digitalWrite(motor2_input1,HIGH);
  digitalWrite(motor2_input2,LOW);
  servo.write(init_servo_angle);
}
void BACK(){
  digitalWrite(motor1_input1,LOW);
  digitalWrite(motor1_input2,HIGH);   
  digitalWrite(motor2_input1,LOW);
  digitalWrite(motor2_input2,HIGH);   
  servo.write(init_servo_angle);
}
void Left(){
  digitalWrite(motor1_input1,LOW);
  digitalWrite(motor1_input2,LOW);   
  digitalWrite(motor2_input1,LOW);
  digitalWrite(motor2_input2,LOW); 
  servo.write(left_servo_angle);
}
void Right(){
  digitalWrite(motor1_input1,LOW);
  digitalWrite(motor1_input2,LOW);   
  digitalWrite(motor2_input1,LOW);
  digitalWrite(motor2_input2,LOW); 
  servo.write(right_servo_angle);
}
void F_L(){
  digitalWrite(motor1_input1,HIGH);
  digitalWrite(motor1_input2,LOW);
  digitalWrite(motor2_input1,HIGH);
  digitalWrite(motor2_input2,LOW);
  servo.write(left_servo_angle);
}
void F_R(){
  digitalWrite(motor1_input1,HIGH);
  digitalWrite(motor1_input2,LOW);
  digitalWrite(motor2_input1,HIGH);
  digitalWrite(motor2_input2,LOW);
  servo.write(right_servo_angle);
}
void B_L(){
  digitalWrite(motor1_input1,LOW);
  digitalWrite(motor1_input2,HIGH);   
  digitalWrite(motor2_input1,LOW);
  digitalWrite(motor2_input2,HIGH);   
  servo.write(left_servo_angle);
}
void B_R(){
  digitalWrite(motor1_input1,LOW);
  digitalWrite(motor1_input2,HIGH);   
  digitalWrite(motor2_input1,LOW);
  digitalWrite(motor2_input2,HIGH);   
  servo.write(right_servo_angle);
}
void STOP(){
  digitalWrite(motor1_input1,LOW);
  digitalWrite(motor1_input2,LOW);    
  digitalWrite(motor2_input1,LOW);
  digitalWrite(motor2_input2,LOW); 
  servo.write(init_servo_angle);
}



void Manual_Drive(char input){
  Serial.println(input);
      switch(input){
        case 'F':
          GO();
          break;

        case 'B':
          BACK();
          break;

        case 'L':
          Left();
          break;

        case 'R':
          Right();
          break;

        case 'Q':   // F+L
          F_L();
          break;

        case 'W':   // F+R
          F_R();
          break;

        case 'E':   // B+L
          B_L();
          break;

        case 'T':   // B+R
          B_R();
          break;

        case 'S':
          STOP();
          break;
      }   
}

void Block_Coding(char input, int input2){
    Serial.println(input);
    Serial.println(input2);
    
    switch(input){
      case 'G':
        GO();
        delay(1000*input2);
        STOP();
        break;  
           
      case 'L':
        F_L();
        delay(1000*input2);
        STOP();
        break;
  
      case 'R':
        F_R();  
        delay(1000*input2);
        STOP();
        break;
        
      case 'K':
        BACK();
        delay(1000*input2);
        STOP();
        break;
    }
}

그런데 현재 이 소스로는 Block_Coding 메소드는 잘 동작을 하는데  Manual_Drive 메소드는 조금씩 끊겨서 rc카가 움직입니다.

근데 여기서  
duration = pulseIn (ECHO, HIGH);  
 distance = duration * 17 / 1000;

이 소스를 주석처리하고 실행하면 Manual_Drive 메소드는 제대로 실행되는데 Block_Coding 메소드가 작동이 되지를 않습니다.

이 문제를 어떻게 해결해야 할지 글도 길고 막연하지만 도움 부탁드립니다..