1505 line datchik perekrestok

//////////////////////////////////////////////
//     2020.01.28 woronin,  umkiedu@gmail.com
//     работа с двумя моторами езда по линии
//     Robot UMKI controller  K6_3
//     проезд перекрестков и дуг с малым радиусом
//////////////////////////////////////////////
int L1;
int pinL1 = A0;
int R2;
int pinR2 = A3;
int R1;
int pinR1 = A1;
int L2;
int pinL2 = A2;
void setup() 
{
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  Serial.begin(9600);
}
void forward () {
  analogWrite(2, 1);
  analogWrite(3, 200);
  analogWrite(4, 1);
  analogWrite(5, 200);
}
void back() {
  analogWrite(2, 0);
  analogWrite(3, 0);
  analogWrite(4, 0);
  analogWrite(5, 0);
}
void left() {
  analogWrite(2, 1);
  analogWrite(3, 220);
  analogWrite(4, 0);
  analogWrite(5, 0);
}
void right() {
  analogWrite(2, 0);
  analogWrite(3, 0);
  analogWrite(4, 1);
  analogWrite(5, 220);
}
void turnleft() {
  left();
  delay(500);
  while(digitalRead(R1==1))
  {
    left();
    if( digitalRead(pinR1) == 0)
      break;
  }
}

void turnright() {
  right();
  delay(500);
  while(digitalRead(R1==1))
  {
    right();
    if( digitalRead(pinL1) == 0)
      break;
  }
}
void turnstraight() {
  forward();
  delay(300);
}
int perekrestok = 1;
void loop() 
{
  R2=digitalRead(pinR2);
  L1=digitalRead(pinL1);
  L2=digitalRead(pinL2);
  R1=digitalRead(pinR1);
  if((L1==0)&&(R1==0)) {
    back();
    delay(1);
  }
  if((L1==1)&&(R1==0)) {
    right();
    delay(1);
  }
  if((L1==0)&&(R1==1)) {
    left();
    delay(1);
  }
  if((L1==1)&&(R1==1))
  {
    forward();
    delay(1);
  }
  if (perekrestok  == 1) {
      if((R2 == 0 && L2 == 0)) { 
        turnstraight();
        perekrestok++;
      }
  } else if (perekrestok == 2) {
      if((L2==0)) { 
        turnleft();
        perekrestok++;
      }
  } else if(perekrestok == 3) {
      if((R2==0)) { 
        turnright();
        perekrestok++;
      }
  }  else if(perekrestok == 4) {
      if((L2==0)) { 
        turnleft();
        perekrestok++;
      }
  }
  
}