Quant BT controll for BT RC Controller

#include <SoftwareSerial.h>

//Режим работы
// 1 - Bleutooth
// 2 - Labyrinth
// 3 - BlackLine
int mode = 2;

// определяем пины для двигателей
// 0 - скорость
const int MOTOR_LEFT_0_PIN = 5;
const int MOTOR_LEFT_1_PIN = 6;
const int MOTOR_LEFT_2_PIN = 7;
const int MOTOR_RIGHT_0_PIN = 10;
const int MOTOR_RIGHT_1_PIN = 8;
const int MOTOR_RIGHT_2_PIN = 9;

int MOTOR_SPEED = 50;
const int MOTOR_SPEED_KOF = 4;

const int pinSensorA5 = A5; //Left
const int pinSensorA6 = A6; //Center
const int pinSensorA7 = A7; //Right

//Пороговые значения для лабиринта
const int threshold1 = 25;
const int threshold2 = 40;
const int threshold3 = 25;

int sensorA5;
int sensorA6;
int sensorA7;

SoftwareSerial bt(2, 3); //rx, tx

void Forward() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT_1_PIN, LOW);
  digitalWrite(MOTOR_LEFT_2_PIN, HIGH);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_RIGHT_1_PIN, HIGH);
  digitalWrite(MOTOR_RIGHT_2_PIN, LOW);
  Serial.println("Forward");
}

void Stop() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, 0);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, 0);
  Serial.println("Stop");
}

void Backward() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT_1_PIN, HIGH);
  digitalWrite(MOTOR_LEFT_2_PIN, LOW);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_RIGHT_1_PIN, LOW);
  digitalWrite(MOTOR_RIGHT_2_PIN, HIGH);
  Serial.println("Forward");
}

void Right() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED / MOTOR_SPEED_KOF);

  Serial.println("Right");
}

void Left() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED / MOTOR_SPEED_KOF  );

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED);

  Serial.println("Left");
}

void sensorRead() {
  sensorA5 = analogRead(pinSensorA5);
  sensorA6 = analogRead(pinSensorA6);
  sensorA7 = analogRead(pinSensorA7);

  Serial.println("Left: "); Serial.print(sensorA5);
  Serial.println("Center: "); Serial.print(sensorA6);
  Serial.println("RIght: "); Serial.print(sensorA7);
}

void setup() {

  bt.begin(9600);
  Serial.begin(9600);
}

char answer;

void loop() {
  sensorRead();
//  delay(1000);
  if (bt.available())
  {
    answer = bt.read();
    Serial.println(answer);

    if (answer == 'X') // set mode = 1
    {
      mode = 2;
    }
    else if (answer == 'x') // set mode = 2
    {
      mode = 1;
    }

    if (mode == 1)
    {
      if (answer == 'F') {
        Forward();
      }
      else if (answer == 'B') {
        Backward();
      }
      else if (answer == 'L') {
        Left();
      }
      else if (answer == 'R') {
        Right();
      }
      else if (answer == 'V') {
        tone(4, 500);
        digitalWrite(4, HIGH);
      }
      else if (answer == 'v') {
        noTone(4);
        digitalWrite(4, LOW);
      }
      else if (answer == 'S') {
        Stop();
      }
      else if (isDigit(answer)) {
        MOTOR_SPEED = map(atoi(&answer), 0, 10, 0, 255);
      }

      if (sensorA5 >= 100 || sensorA6 >= 90 || sensorA7 >= 100) {
        Backward();
        delay(300);
        Forward();
      }
    }
  }
  if (mode == 2)
  { // sensorA5 sensorA6 sensorA7
    if (sensorA5 < threshold1 && sensorA6 < threshold2 && sensorA7 < threshold3) {
      // Нет препятствий, двигаемся вперед
      Forward();
    } else if (sensorA5 > threshold1 && sensorA6 < threshold2 && sensorA7 < threshold3) {
      // Препятствие слева, поворачиваем вправо
      Left();
    } else if (sensorA5 < threshold1 && sensorA6 < threshold2 && sensorA7 > threshold3) {
      // Препятствие справа, поворачиваем влево
      Right();
    } else if (sensorA5 > threshold1 && sensorA6 > threshold2 && sensorA7 < threshold3) {
      // Препятствие спереди, поворачиваем влево
      //Left();
    } else {
      // Препятствия с обеих сторон или со всех сторон, поворачиваем вправо
      Right();
    }
  }
}