1504 line datchik perekrestok

//////////////////////////////////////////////
//     2019.12.24 woronin,  umkiedu@gmail.com
//     работа с двумя моторами езда по линии
//     Robot UMKI controller  K6_3
//     проезд перекрестков и дуг с малым радиусом
//////////////////////////////////////////////
int motorR1 = 2; // 2-й контакт определяет направление вращения  первого правого мотора
int motorL1 = 4; // 4-й контакт определяет направление вращения  первого левого мотора
int mLspeed = 3; // 3-й управляет вращением  левого мотора
int mRspeed = 5; // 5-й управляет вращением  правого мотора
const int sensorR1 = 11; // Подсоедниняем правый датчик к 11
const int sensorL1 = 10; // Подсоедниняем левый датчик к 10
const int sensorR2 = 9; // Подсоедниняем второй правый датчик к 9
const int sensorL2 = 8; // Подсоедниняем второй левый датчик к 8
const int dinamikPin = 12;// пин пищалки

byte speedF = 255, speedT = 120, perekrestok = 1;

void setup() {
  // определяем переменные
  pinMode(11, INPUT);
  pinMode(10, INPUT);
  pinMode(9, INPUT);
  pinMode(8, INPUT);
  Serial.begin(9600);
  pinMode(motorR1, OUTPUT);
  pinMode(mLspeed, OUTPUT);
  pinMode(motorL1, OUTPUT);
  pinMode(mLspeed, OUTPUT);
}
void forward() {
  digitalWrite(motorR1, LOW);
  analogWrite(mLspeed, speedT);
  digitalWrite(motorL1, LOW);
  analogWrite(mLspeed, speedT);
}
void finish () {
  digitalWrite(mLspeed, LOW);
  digitalWrite(mLspeed, LOW);
}
void right () {
  digitalWrite(motorR1, LOW);
  analogWrite(mLspeed, speedT);
  digitalWrite(motorL1, HIGH);
  analogWrite(mLspeed, speedT);
}
void left () {
  digitalWrite(motorR1, HIGH);
  analogWrite(mLspeed, speedT);
  digitalWrite(motorL1, LOW);
  analogWrite(mLspeed, speedT);
}

void turnForward() {
  forward();
  delay(600);
}

void midi(int zvuk) { // писк динамика с разным тоном
  tone(dinamikPin, 2 * zvuk, zvuk);
  delay(zvuk);
}

boolean goF = false, goL = false, goR = false, noGo = false, rotL = false, rotR = false, goL2 = false, goR2 = false;


void loop() {
  // Считываем показания с датчиков черной линии, с левого и с правого
  if (digitalRead(sensorR1) == 1 && digitalRead(sensorL1) == 1) {
    forward();
  } else if (digitalRead(sensorR1) == 0 && digitalRead(sensorL1) == 0) {
    goF = false;
    noGo = true;
    goL = false;
    goR = false;
  } else if (digitalRead(sensorL1) == 0) {
    Serial.println("Left");
    midi(500); // писк для отладки
    if (digitalRead(sensorR2) == 1) {
      long time = millis();
      while (digitalRead(sensorL1) == 0) {
        left();
      }
    } else if (digitalRead(sensorR2) == 0) {
      while (digitalRead(sensorR2) == 0) {
        left();
      }
    }
  }
}