帮我看一下Arduino的代码问题

想做一个蓝牙的测距小车，用的是osoyoo家的小车，程序代码在下面。蓝牙和测距的代码各自分开都是可以实行的，但是加在一起蓝牙就控制不了小车了，不明白自己的编程那里写错了，望大佬指导。

这是测距的代码（最下面的是总代码）

#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C  lcd (0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
const int TrigPin = A0;
const int EchoPin = A1;
float cm;

void setup()
{
  Serial.begin(9600);
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);

}
void loop()
{
  digitalWrite(TrigPin, LOW); /
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);
  cm = pulseIn(EchoPin, HIGH) / 58.0; //
  cm = (int(cm * 100.0)) / 100.0; //
  delay(1000);
  lcd.init(); // initialize the lcd 
  lcd.backlight(); //Open the backlight
  lcd.print("Distance:"); 
  lcd.setCursor(0, 1);
  lcd.print(cm);
  lcd.print("cm");}

在void loop加入了do_Distance_Tick()以后蓝牙功能就无法实现了

#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,16,2); // set the LCD address to 0x27 for a 16 chars and 2 line display
const int TrigPin = A0;
const int EchoPin = A1;
1. float cm;
#define IN1  7   //K1、K2 motor direction
#define IN2  8     //K1、K2 motor direction
#define IN3  9    //K3、K4 motor direction
#define IN4  10   //K3、K4 motor direction
#define ENA  5    // Needs to be a PWM pin to be able to control motor speed ENA
#define ENB  6    // Needs to be a PWM pin to be able to control motor speed ENA
#define M_SPEED1   150  //motor speed
#define M_SPEED2   150  //motor speed
#define MAX_PACKETSIZE 32    //Serial receive buffer
char buffUART[MAX_PACKETSIZE];
unsigned int buffUARTIndex = 0;
unsigned long preUARTTick = 0;
bool stopFlag = true;
bool JogFlag = false;
uint16_t JogTimeCnt = 0;
uint32_t JogTime=0;
enum DS
{
  MANUAL_DRIVE,
  AUTO_DRIVE_LF, //line follow
}Drive_Status=MANUAL_DRIVE;
enum DN
{ 
  GO_ADVANCE, 
  GO_LEFT, 
  GO_RIGHT,
  GO_BACK,
  STOP_STOP,
  DEF
}Drive_Num=DEF;

/*motor control*/
void go_back(int t)  //motor rotate clockwise -->robot go ahead
{
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4,HIGH);
  delay(t);
}
void go_ahead(int t) //motor rotate counterclockwise -->robot go back
{
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4,LOW); 
  delay(t);
}
void go_stop() //motor brake -->robot stop
{
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4,LOW); 
}
void turn_left(int t)  //left motor rotate clockwise and right motor rotate counterclockwise -->robot turn right
{
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  delay(t);
}
void turn_right(int t) //left motor rotate counterclockwise and right motor rotate clockwise -->robot turn left
{
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  delay(t);
}
/*set motor speed */
void set_motorspeed(int lspeed,int rspeed) //change motor speed
{
  analogWrite(ENA,lspeed);//lspeed:0-255
  analogWrite(ENB,rspeed);//rspeed:0-255   
}

//WiFi / Bluetooth through the serial control
void do_Uart_Tick()
{

  char Uart_Date=0;
  if(Serial.available()) 
  {
    size_t len = Serial.available();
    uint8_t sbuf[len + 1];
    sbuf[len] = 0x00;
    Serial.readBytes(sbuf, len);
    //parseUartPackage((char*)sbuf);
    memcpy(buffUART + buffUARTIndex, sbuf, len);//ensure that the serial port can read the entire frame of data
    buffUARTIndex += len;
    preUARTTick = millis();
    if(buffUARTIndex >= MAX_PACKETSIZE - 1) 
    {
      buffUARTIndex = MAX_PACKETSIZE - 2;
      preUARTTick = preUARTTick - 200;
    }
  }
  if(buffUARTIndex > 0 && (millis() - preUARTTick >= 100))//APP send flag to modify the obstacle avoidance parameters
  { //data ready
    buffUART[buffUARTIndex] = 0x00;
    Uart_Date=buffUART[0];
    buffUARTIndex = 0;
  }
  switch (Uart_Date)    //serial control instructions
  {
    case '2':
            Drive_Status=MANUAL_DRIVE; Drive_Num=GO_ADVANCE;Serial.println("forward"); break;
    case '4':
            Drive_Status=MANUAL_DRIVE;Drive_Num=GO_LEFT;Serial.println("turn left");break;
    case '6':
            Drive_Status=MANUAL_DRIVE;Drive_Num=GO_RIGHT;Serial.println("turn right");break;
    case '8':
            Drive_Status=MANUAL_DRIVE;Drive_Num=GO_BACK;Serial.println("go back");break;
    case '5':
           Drive_Status=MANUAL_DRIVE;Drive_Num=STOP_STOP;Serial.println("stop");break;
    default:break;
  }
}
//robot motor control
void do_Drive_Tick()
{
  if(Drive_Status == MANUAL_DRIVE)
  {
    switch (Drive_Num) 
    {
      case GO_ADVANCE:
          set_motorspeed(255,255);
          go_ahead(15);
          JogFlag = true;
          JogTimeCnt = 3;
          JogTime=millis();
          break;
      case GO_LEFT: 
          set_motorspeed(100,100);
          turn_left(2);
          JogFlag = true;
          JogTimeCnt = 1;
          JogTime=millis();
          break;
      case GO_RIGHT:  
           set_motorspeed(100,100);
           turn_right(2);
          JogFlag = true;
          JogTimeCnt = 1;
          JogTime=millis();
          break;
      case GO_BACK: 
          set_motorspeed(150,150);
          go_back(10);
          JogFlag = true;
          JogTimeCnt = 1;
          JogTime=millis();
          break;
      case STOP_STOP: 
          go_stop();
          JogTime = 0;
          break;
      default:
          break;
    }
    Drive_Num=DEF;
  //keep the car running for 100ms
    if(millis()-JogTime>=100)
    {
      JogTime=millis();
      if(JogFlag == true) 
      {
        stopFlag = false;
        if(JogTimeCnt <= 0) 
        {
          JogFlag = false; stopFlag = true;
        }
        JogTimeCnt--;
      }
      if(stopFlag == true) 
      {
        JogTimeCnt=0;
        go_stop();
      }
    }
  }

}
void do_Distance_Tick()
{
  digitalWrite(TrigPin, LOW); /TrigPin
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);
  cm = pulseIn(EchoPin, HIGH) / 58.0; //
  cm = (int(cm * 100.0)) / 100.0; //小数点
  delay(1000);
  lcd.init(); // initialize the lcd 
  lcd.backlight(); //Open the backlight
  lcd.print("Distance:"); 
  lcd.setCursor(0, 1);
  lcd.print(cm);
  lcd.print("cm");
}
void setup() {

  /******L298N******/
  pinMode(IN1, OUTPUT); 
  pinMode(IN2, OUTPUT); 
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT); 
  pinMode(ENA, OUTPUT);  
  pinMode(ENB, OUTPUT);
  go_stop();
  Serial.begin(9600);
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);

}
void loop() {
  do_Uart_Tick();
  do_Drive_Tick();
  do_Distance_Tick();
}