#include <Servo.h>
Servo VerticalServo;
Servo  GorizontalServo;

int echoPin = 2;
int trigPin = 3;

#define ENA 4
#define ENB 11
#define IN1 7
#define IN2 6
#define IN3 5
#define IN4 8
#define RedIdeod 12
void setup()
{
  VerticalServo.attach(10);
  GorizontalServo.attach(9);
  VerticalServo.write(108);

  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop()
{

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  analogWrite(ENA, 255);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENB, 255);

  int leftDistance;
  int rightDistance;
  int duration, distation;

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distation = duration / 58;

  if (distation > 30) {
    GorizontalServo.write(56);
  }


  if ((distation > 0) && (distation < 300)) {
    Serial.print(distation);
    Serial.println(" cm");
    delay(100);
  }
  if (distation < 1) {
    Serial.print("Робот выключен, пожалуйста нажмите на выключатель ");
    Serial.println(" ");
    delay(100);
  }
  if ((distation < 30) && (distation > 0 )) {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);

    GorizontalServo.write(7);

    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    rightDistance = pulseIn(echoPin, HIGH);
    rightDistance = rightDistance / 58;
    delay(1000);

    GorizontalServo.write(120);

    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    leftDistance = pulseIn(echoPin, HIGH);
    leftDistance = leftDistance / 58;
    delay(1000);

    GorizontalServo.write(55);
    delay(1000);

    if (rightDistance > leftDistance) {
      digitalWrite(IN1, HIGH);
      digitalWrite(IN2, LOW);
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, HIGH);
      delay(380);
    }

    if (rightDistance < leftDistance) {
      digitalWrite(IN1, HIGH);
      digitalWrite(IN2, LOW);
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, HIGH);
      delay(380);
    }
  }
  if (distation == 0) {
   analogWrite(12, 255);
        delay(100);
    if (rightDistance == 0) {
     analogWrite(12, 255);
        delay(100);
      if (leftDistance == 0) {
        analogWrite(RedIdeod, 255);
        delay(100);
      }
    }
  }
}