#include <Servo.h>

Servo myservo;

#define turn 12 //сервопривод
#define Trig 8 //ультразвуковой датчик
#define Echo 9 //ультразвуковой датчик
const int in1 = 2; // IN4 pin 2 //драйвер двигателя
const int in2 = 4; // IN3 pin 4 //драйвер двигателя
const int in3 = 5; // IN2 pin 5 //драйвер двигателя
const int in4 = 7; // IN1 pin 7 //драйвер двигателя
int ENB1 = A1; //драйвер двигателя
int ENA2 = A2; //драйвер двигателя
int leftSensor = 11;
int rightSensor = 13;

int zero = 90;
int right = 45;
int left = 135;

void setup()
{
Serial.begin(9600);
pinMode(turn,OUTPUT); //выход на серво
pinMode(Trig, OUTPUT); // выход
pinMode(Echo, INPUT); // вход
pinMode(in1, OUTPUT); // выход на L298n
pinMode(in2, OUTPUT); // выход на L298n
pinMode(in3, OUTPUT); // выход на L298n
pinMode(in4, OUTPUT); // выход на L298n
pinMode (ENB1, OUTPUT);
pinMode (ENA2, OUTPUT);
pinMode(leftSensor, INPUT);
pinMode(rightSensor, INPUT);
myservo.attach(12);
myservo.write(zero);
}
unsigned int impulseTime=0;
unsigned int distance_sm=0;
void loop()
{
digitalWrite(Trig, HIGH);
delayMicroseconds(10); // 10 микросекунд
digitalWrite(Trig, LOW);
impulseTime=pulseIn(Echo, HIGH); // замеряем длину импульса
distance_sm=impulseTime/58; // переводим в сантиметры
if (distance_sm<20) // если расстояние более 20 сантиметров
{
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
analogWrite(ENB1, 0);
analogWrite(ENA2, 0);
myservo.write(zero);
}
else
{
  Serial.print(digitalRead(leftSensor));
  Serial.println(" ");
  Serial.print(digitalRead(rightSensor));
  if (digitalRead(leftSensor) == 0 && digitalRead(rightSensor) == 0) {
      analogWrite(ENB1, 255);
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      digitalWrite(in3, LOW);
      digitalWrite(in4, HIGH);
      analogWrite(ENA2, 255);
  } else if (digitalRead(leftSensor) == 1 && digitalRead(rightSensor) == 0) {
      analogWrite(ENB1, 120);
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      digitalWrite(in3, LOW);
      digitalWrite(in4, HIGH);
      analogWrite(ENA2, 120);
      myservo.write(left);
  } else if (digitalRead(leftSensor) == 0 && digitalRead(rightSensor) == 1) {
    analogWrite(ENB1, 120);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    analogWrite(ENA2, 120);
    myservo.write(right);
  }
}
}