🤖 Умный робот с множественными датчиками

    [Робот сверху]
    
    [O]     [O]    ← Колеса
     |       |
  [●]         [●]   ← Датчики линии
     \       /
      \     /
       [═══]        ← Черная линия

// Подключение датчиков к Arduino
const int TRIG_PIN = 7;        // Ультразвук - trigger
const int ECHO_PIN = 6;        // Ультразвук - echo
const int LINE_LEFT_PIN = A0;  // Левый датчик линии
const int LINE_RIGHT_PIN = A1; // Правый датчик линии
const int MOTOR_LEFT_A = 3;    // Левый мотор - направление
const int MOTOR_LEFT_B = 4;    // Левый мотор - направление
const int MOTOR_RIGHT_A = 5;   // Правый мотор - направление  
const int MOTOR_RIGHT_B = 8;   // Правый мотор - направление

// Инициализация системы
void setup() {
    Serial.begin(9600);
    initializeSensors();
    initializeMotors();
    calibrateSensors();
}

// Главный цикл
void loop() {
    updateSensors();      // Читаем все датчики
    makeDecision();       // Принимаем решение
    executeAction();      // Выполняем действие
    delay(20);           // 50 Hz update rate
}

// Структура для хранения данных датчиков
struct SensorData {
    int leftLine;         // Левый датчик линии (0-1023)
    int rightLine;        // Правый датчик линии (0-1023)
    float distance;       // Расстояние в см
    bool lineDetected;    // Обнаружена ли линия
    bool obstacleAhead;   // Есть ли препятствие впереди
};

SensorData sensors;

void updateSensors() {
    // Читаем датчики линии
    sensors.leftLine = analogRead(LINE_LEFT_PIN);
    sensors.rightLine = analogRead(LINE_RIGHT_PIN);
    
    // Читаем ультразвуковой датчик
    sensors.distance = readUltrasonic();
    
    // Определяем состояния
    sensors.lineDetected = (sensors.leftLine < 300) || (sensors.rightLine < 300);
    sensors.obstacleAhead = sensors.distance < 20.0; // 20 см безопасная зона
}

float readUltrasonic() {
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    
    long duration = pulseIn(ECHO_PIN, HIGH);
    return duration * 0.034 / 2; // Перевод в сантиметры
}

enum RobotState {
    FOLLOWING_LINE,     // Следование по линии
    AVOIDING_OBSTACLE,  // Обход препятствия
    SEARCHING_LINE,     // Поиск потерянной линии
    STOPPED            // Остановка (аварийная ситуация)
};

RobotState currentState = FOLLOWING_LINE;

void makeDecision() {
    switch (currentState) {
        case FOLLOWING_LINE:
            if (sensors.obstacleAhead) {
                currentState = AVOIDING_OBSTACLE;
                Serial.println("Switching to OBSTACLE AVOIDANCE");
            } else if (!sensors.lineDetected) {
                currentState = SEARCHING_LINE;
                Serial.println("LINE LOST - Searching...");
            }
            break;
            
        case AVOIDING_OBSTACLE:
            if (!sensors.obstacleAhead && sensors.lineDetected) {
                currentState = FOLLOWING_LINE;
                Serial.println("Obstacle cleared - Following line");
            }
            break;
            
        case SEARCHING_LINE:
            if (sensors.lineDetected) {
                currentState = FOLLOWING_LINE;
                Serial.println("Line found - Following");
            }
            break;
            
        case STOPPED:
            // Требуется ручное вмешательство
            break;
    }
}

void executeAction() {
    switch (currentState) {
        case FOLLOWING_LINE:
            followLine();
            break;
            
        case AVOIDING_OBSTACLE:
            avoidObstacle();
            break;
            
        case SEARCHING_LINE:
            searchLine();
            break;
            
        case STOPPED:
            stopMotors();
            break;
    }
}

void followLine() {
    int leftSensor = sensors.leftLine;
    int rightSensor = sensors.rightLine;
    
    // PID-регулятор для точного следования
    int error = leftSensor - rightSensor;
    static int lastError = 0;
    static int integral = 0;
    
    float Kp = 0.5;  // Пропорциональный коэффициент
    float Ki = 0.01; // Интегральный коэффициент  
    float Kd = 0.1;  // Дифференциальный коэффициент
    
    integral += error;
    int derivative = error - lastError;
    
    int correction = Kp * error + Ki * integral + Kd * derivative;
    
    int baseSpeed = 150;  // Базовая скорость
    int leftSpeed = baseSpeed + correction;
    int rightSpeed = baseSpeed - correction;
    
    // Ограничиваем скорости
    leftSpeed = constrain(leftSpeed, 0, 255);
    rightSpeed = constrain(rightSpeed, 0, 255);
    
    setMotorSpeeds(leftSpeed, rightSpeed);
    lastError = error;
}

class AdaptivePID {
private:
    float kp, ki, kd;
    float integral, lastError;
    float maxIntegral = 1000;
    
public:
    AdaptivePID(float p, float i, float d) : kp(p), ki(i), kd(d) {
        integral = 0;
        lastError = 0;
    }
    
    float calculate(float error, float speed) {
        // Адаптация коэффициентов в зависимости от скорости
        float adaptiveKp = kp * (1.0 + speed / 255.0);
        float adaptiveKd = kd * (1.0 + speed / 255.0);
        
        integral += error;
        integral = constrain(integral, -maxIntegral, maxIntegral);
        
        float derivative = error - lastError;
        
        float output = adaptiveKp * error + ki * integral + adaptiveKd * derivative;
        
        lastError = error;
        return output;
    }
};

float predictCollisionTime(float distance, float speed) {
    if (speed <= 0) return INFINITY;
    return distance / speed;
}

void smartObstacleAvoidance() {
    float currentSpeed = getCurrentSpeed();
    float timeToCollision = predictCollisionTime(sensors.distance, currentSpeed);
    
    if (timeToCollision < 2.0) {  // 2 секунды до столкновения
        // Начинаем маневр заранее
        executeAvoidanceManeuver();
    }
}

void basicLineFollowing() {
    bool leftOnLine = (sensors.leftLine < 300);
    bool rightOnLine = (sensors.rightLine < 300);
    
    if (leftOnLine && rightOnLine) {
        // Оба датчика на линии - едем прямо
        setMotorSpeeds(150, 150);
    } else if (leftOnLine && !rightOnLine) {
        // Линия слева - поворачиваем влево
        setMotorSpeeds(100, 200);
    } else if (!leftOnLine && rightOnLine) {
        // Линия справа - поворачиваем вправо  
        setMotorSpeeds(200, 100);
    } else {
        // Линия потеряна - останавливаемся
        setMotorSpeeds(0, 0);
        currentState = SEARCHING_LINE;
    }
}

void avoidObstacle() {
    static int avoidanceStep = 0;
    static unsigned long stepStartTime = 0;
    
    switch (avoidanceStep) {
        case 0: // Остановка
            stopMotors();
            delay(500);
            avoidanceStep = 1;
            stepStartTime = millis();
            break;
            
        case 1: // Поворот влево на 90°
            setMotorSpeeds(-150, 150);  // Поворот на месте
            if (millis() - stepStartTime > 800) {  // ~90°
                avoidanceStep = 2;
                stepStartTime = millis();
            }
            break;
            
        case 2: // Движение вперед
            setMotorSpeeds(150, 150);
            if (millis() - stepStartTime > 1000) {  // Проезжаем препятствие
                avoidanceStep = 3;
                stepStartTime = millis();
            }
            break;
            
        case 3: // Поворот вправо на 90°
            setMotorSpeeds(150, -150);
            if (millis() - stepStartTime > 800) {
                avoidanceStep = 4;
                stepStartTime = millis();
            }
            break;
            
        case 4: // Движение до линии
            setMotorSpeeds(150, 150);
            if (sensors.lineDetected) {
                avoidanceStep = 5;
                stepStartTime = millis();
            }
            break;
            
        case 5: // Поворот вправо для выхода на линию
            setMotorSpeeds(150, -150);
            if (millis() - stepStartTime > 800) {
                avoidanceStep = 0;  // Сброс
                currentState = FOLLOWING_LINE;
            }
            break;
    }
}

void searchLine() {
    static unsigned long searchStartTime = 0;
    static bool searchInitialized = false;
    static int searchRadius = 100;  // мс поворота
    static int searchDirection = 1; // 1 = право, -1 = лево
    
    if (!searchInitialized) {
        searchStartTime = millis();
        searchInitialized = true;
    }
    
    // Поворот с увеличивающимся радиусом
    if (searchDirection > 0) {
        setMotorSpeeds(150, -150);  // Поворот вправо
    } else {
        setMotorSpeeds(-150, 150);  // Поворот влево
    }
    
    if (millis() - searchStartTime > searchRadius) {
        searchDirection *= -1;  // Меняем направление
        searchRadius += 50;     // Увеличиваем радиус поиска
        searchStartTime = millis();
        
        // Ограничиваем время поиска
        if (searchRadius > 2000) {
            currentState = STOPPED;  // Линия безнадежно потеряна
            Serial.println("Line search failed - STOPPING");
        }
    }
    
    // Если нашли линию
    if (sensors.lineDetected) {
        searchInitialized = false;
        searchRadius = 100;  // Сброс параметров
        currentState = FOLLOWING_LINE;
        Serial.println("Line found!");
    }
}

class QLearningRobot {
private:
    float qTable[NUM_STATES][NUM_ACTIONS];
    float learningRate = 0.1;
    float discountFactor = 0.9;
    float explorationRate = 0.1;
    
    enum State {
        ON_LINE_CLEAR = 0,
        ON_LINE_OBSTACLE = 1,
        OFF_LINE_CLEAR = 2,
        OFF_LINE_OBSTACLE = 3
    };
    
    enum Action {
        MOVE_FORWARD = 0,
        TURN_LEFT = 1,
        TURN_RIGHT = 2,
        REVERSE = 3
    };
    
public:
    int selectAction(int state) {
        if (random(100) < explorationRate * 100) {
            return random(NUM_ACTIONS);  // Исследование
        } else {
            return getBestAction(state);  // Использование знаний
        }
    }
    
    void updateQ(int state, int action, float reward, int nextState) {
        float maxQ = getMaxQ(nextState);
        float oldQ = qTable[state][action];
        
        qTable[state][action] = oldQ + learningRate * 
            (reward + discountFactor * maxQ - oldQ);
    }
    
    float calculateReward() {
        float reward = 0;
        
        if (sensors.lineDetected) reward += 10;  // Награда за следование линии
        if (sensors.obstacleAhead) reward -= 50; // Штраф за риск столкновения
        if (getCurrentSpeed() > 100) reward += 1; // Награда за скорость
        
        return reward;
    }
};