// пин управления скоростью мотора (с подержкой ШИМ)
#define SPEED   6
// пин выбора направления движения мотора
#define DIR     5
#define PIN_TRIG 12
#define PIN_ECHO 11

long duration, midduration, cm;
unsigned long time;
#include <Servo.h>
#define SERVO_PIN 7
Servo yservo;
int pos = 0;
void setup() 
{
  // инициализация переменных
  cm = 10; 
  duration = cm * 2 * 29.1;
  midduration =  cm * 2 * 29.1;
  yservo.attach(7);
  // пины в режим выхода
  pinMode(DIR, OUTPUT); 
  pinMode(SPEED, OUTPUT);
  // Инициализируем взаимодействие по последовательному порту
  Serial.begin(115200);
  //Определяем вводы и выводы
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
} 
 
void loop() 
{
  time = millis();
  // пускаем двигатель вперед
  digitalWrite(DIR, LOW);
  analogWrite(SPEED, 100);     
  // пока высота больше порога
  while (cm>6)
  { 
    // измерить высоту
    cm = visorrun();
    Serial.print(cm);
    Serial.print("\n");
    delay(20);
  }
  time = millis() - time;
  // остановка
  analogWrite(SPEED,0);
  delay(100);
  // отъезд назад
   digitalWrite(DIR, HIGH);
  analogWrite(SPEED, 100);
  delay(800);
  analogWrite(SPEED, 0);
  delay(100);
  // открыть отсек
  yservo.write(105);
  delay(100);
  yservo.write(97);
  // проехать вперед для подбора груза
  digitalWrite(DIR, LOW);
  analogWrite(SPEED, 100);
      delay(1400);
  // остановиться
  analogWrite(SPEED, 0);
  delay(100);
  // закрыть отсек
  yservo.write(90);
  delay(100);
  yservo.write(97);
  // ехать на базу
  digitalWrite(DIR, HIGH);
  analogWrite(SPEED, 100);
  delay(time + 600);
  analogWrite(SPEED, 0);
  while(1);
}
int visorrun() { 
  // Сначала генерируем короткий импульс длительностью 2-5 микросекунд.
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(5);
  digitalWrite(PIN_TRIG, HIGH);
  // Выставив высокий уровень сигнала, ждем около 10 микросекунд. В этот момент датчик будет посылать сигналы с частотой 40 КГц.
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  //  Время задержки акустического сигнала на эхолокаторе.
  duration = pulseIn(PIN_ECHO, HIGH);
  // усреднить значение для сглаживания
  midduration = (duration + 3 * midduration) / 4; 
  // Теперь осталось преобразовать время в расстояние
  return ((midduration / 2) / 29.1);
}
