Level 1: "Вижу стену → поворачиваю"     ← Реактивно

Level 2: "Вижу стену → смотрю по сторонам →  
          выбираю лучший путь"           ← Умно!

     ┌──────────────┐
     │              │
     │     🤖 →     │  Датчик смотрит вперёд
     │         ┌────┘  Угол не видит!
     │         │
     └─────────┘

     ┌────┐  ┌────┐
     │    │  │    │
     │    │🤖│    │  Проход есть, но робот
     │    │  │    │  не знает ширину!
     └────┘  └────┘

     ┌───────────────┐
     │ ↓ ← ← ← ←     │
     │               │
     │ → → → → → ↓   │  Робот ходит
     │ ↑           ↓ │  по кругу!
     │ ← ← ← ← ← ←   │
     └───────────────┘

           Передний
              │
              ▼
         ┌────────┐
    Левый│   🤖   │Правый
    ─────►        ◄─────
         └────────┘

       Передний
          │
    ╱     │     ╲
   45°    │    45°
    ╲     │     ╱
     ┌────┴────┐
     │    🤖    │
     │ L     R │  ← Боковые
     └─────────┘

int front = getDistance(FRONT);
int left = getDistance(LEFT);
int right = getDistance(RIGHT);

if (front > SAFE_DIST) {
  // Путь свободен — едем!
  goForward();
}
else if (left > right) {
  // Слева свободнее — поворачиваем влево
  turnLeft();
}
else {
  // Справа свободнее — поворачиваем вправо
  turnRight();
}

Ситуация 1: Путь свободен
┌─────────────┐
│             │
│     ↑       │   front=100, left=50, right=50
│    🤖       │   → goForward()
└─────────────┘

Ситуация 2: Стена впереди, слева свободнее
┌──────┐      │
│      │      │   front=20, left=80, right=30
│      │  🤖 →│   → turnLeft()
└──────┘      │

Ситуация 3: Стена впереди, справа свободнее
│      ┌──────┐
│      │      │   front=20, left=30, right=80
│←  🤖 │      │   → turnRight()
│      └──────┘

// Пины ультразвуковых датчиков
const int TRIG_F = 2, ECHO_F = 3;   // Передний
const int TRIG_L = 4, ECHO_L = 5;   // Левый
const int TRIG_R = 6, ECHO_R = 7;   // Правый

const int SAFE_DIST = 30;  // см
const int CRITICAL_DIST = 15;

void loop() {
  int front = measure(TRIG_F, ECHO_F);
  int left = measure(TRIG_L, ECHO_L);
  int right = measure(TRIG_R, ECHO_R);
  
  if (front > SAFE_DIST) {
    // Плавная коррекция на ходу
    int correction = (left - right) / 10;
    setMotors(BASE_SPEED + correction, BASE_SPEED - correction);
  }
  else if (front > CRITICAL_DIST) {
    // Притормаживаем и выбираем направление
    setMotors(SLOW_SPEED, SLOW_SPEED);
    
    if (left > right + 10) prepareLeft();
    else if (right > left + 10) prepareRight();
  }
  else {
    // Экстренный манёвр!
    stop();
    if (left > right) emergencyLeft();
    else emergencyRight();
  }
  
  delay(50);
}

#include <Servo.h>
Servo headServo;

int scan() {
  int distances[5];
  int angles[] = {0, 45, 90, 135, 180};
  
  for (int i = 0; i < 5; i++) {
    headServo.write(angles[i]);
    delay(200);
    distances[i] = measureDistance();
  }
  
  // Находим направление с max расстоянием
  int maxIndex = 0;
  for (int i = 1; i < 5; i++) {
    if (distances[i] > distances[maxIndex]) {
      maxIndex = i;
    }
  }
  
  return angles[maxIndex];  // Куда ехать
}

const int TARGET_DIST = 20;  // Желаемое расстояние до стены
const float Kp = 2.0;

void wallFollow() {
  int rightDist = measure(TRIG_R, ECHO_R);
  
  int error = rightDist - TARGET_DIST;
  int correction = Kp * error;
  
  // Если далеко от стены — поворачиваем к ней
  // Если близко — отворачиваемся
  setMotors(BASE_SPEED - correction, BASE_SPEED + correction);
}

enum Direction { FORWARD, LEFT, RIGHT, BACK };
Direction history[10];
int historyIndex = 0;

void recordTurn(Direction dir) {
  history[historyIndex] = dir;
  historyIndex = (historyIndex + 1) % 10;
}

bool isLooping() {
  // Если последние 4 поворота — LLLL или RRRR
  // Значит, мы в тупике!
  int sameCount = 0;
  for (int i = 0; i < 4; i++) {
    if (history[i] == history[0]) sameCount++;
  }
  return (sameCount >= 4);
}

void loop() {
  // ...
  if (isLooping()) {
    // Разворачиваемся и пробуем другой путь
    turnAround();
    recordTurn(BACK);
  }
}

enum State { DRIVING, SCANNING, TURNING, REVERSING };
State currentState = DRIVING;

void loop() {
  switch (currentState) {
    case DRIVING:
      if (obstacleAhead()) {
        currentState = SCANNING;
      } else {
        driveForward();
      }
      break;
      
    case SCANNING:
      int bestAngle = scan();
      if (bestAngle < 60) currentState = REVERSING;
      else currentState = TURNING;
      break;
      
    case TURNING:
      if (turnComplete()) currentState = DRIVING;
      break;
      
    case REVERSING:
      reverse();
      if (clearBehind()) currentState = SCANNING;
      break;
  }
}