---

#include <ESP32Servo.h>

// Пины сервопривода и ультразвукового датчика
#define SERVO_PIN    13
#define TRIG_PIN     12
#define ECHO_PIN     14

// Границы движения серво (0-180°, но ограничим ±30° от центра)
#define CENTER_POS    90    // центральная позиция (0° отклонения)
#define MAX_OFFSET    30    // максимальное отклонение от центра в градусах
#define MIN_ANGLE     (CENTER_POS - MAX_OFFSET)  // 60°
#define MAX_ANGLE     (CENTER_POS + MAX_OFFSET)  // 120°

// Параметры сканирования
#define SCAN_STEP      2    // шаг сканирования в градусах
#define SCAN_DELAY    50    // задержка после поворота (мс)

Servo myServo;
int currentAngle = CENTER_POS;   // текущий угол
int targetAngle = CENTER_POS;    // целевой угол (за объектом)

void setup() {
  Serial.begin(115200);
  
  // Настройка пинов ультразвукового датчика
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  // Подключение серво
  myServo.attach(SERVO_PIN);
  
  // Стартовая позиция — центр (90°)
  myServo.write(CENTER_POS);
  currentAngle = CENTER_POS;
  delay(500);
  
  Serial.println("Радар запущен. Начальная позиция: центр.");
  Serial.print("Диапазон движения: от ");
  Serial.print(MIN_ANGLE);
  Serial.print("° до ");
  Serial.print(MAX_ANGLE);
  Serial.println("°");
}

void loop() {
  // Поиск объекта — медленное сканирование в пределах MIN_ANGLE..MAX_ANGLE
  targetAngle = findClosestObject();
  
  // Плавно поворачиваем серво к цели
  moveToAngle(targetAngle);
  
  delay(50);
}

// Функция сканирования: возвращает угол, где обнаружен ближайший объект
int findClosestObject() {
  float minDistance = 200.0;   // максимальная дистанция (см)
  int bestAngle = CENTER_POS;
  
  // Сканируем от MIN_ANGLE до MAX_ANGLE
  for (int angle = MIN_ANGLE; angle <= MAX_ANGLE; angle += SCAN_STEP) {
    myServo.write(angle);
    delay(SCAN_DELAY);
    
    float distance = getDistance();
    
    // Если объект ближе 100 см — считаем
    if (distance > 2 && distance < 100) {
      Serial.print("Угол: ");
      Serial.print(angle);
      Serial.print("°  Расстояние: ");
      Serial.print(distance);
      Serial.println(" см");
      
      if (distance < minDistance) {
        minDistance = distance;
        bestAngle = angle;
      }
    }
  }
  
  if (minDistance < 100) {
    Serial.print("Ближайший объект найден на угле ");
    Serial.print(bestAngle);
    Serial.print("°, дистанция ");
    Serial.print(minDistance);
    Serial.println(" см");
    return bestAngle;
  } else {
    // Если ничего не найдено, остаёмся в центре
    return CENTER_POS;
  }
}

// Плавное перемещение серво к целевому углу
void moveToAngle(int target) {
  // Ограничиваем целевой угол допустимыми пределами
  target = constrain(target, MIN_ANGLE, MAX_ANGLE);
  
  // Движение с шагом 1° для плавности
  while (currentAngle != target) {
    if (currentAngle < target) currentAngle++;
    else currentAngle--;
    
    myServo.write(currentAngle);
    delay(10);
  }
}

// Измерение расстояния через HC-SR04
float getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // таймаут 30 мс
  if (duration == 0) return 200.0; // нет эха
  
  float distance = duration * 0.034 / 2; // в см
  return distance;
}