💻 Программирование работы датчика расстояния

Рекомендуемые контрольные точки:
- 5 см (минимальная дальность)
- 10 см (ближняя зона)
- 20 см (рабочая зона)
- 50 см (средняя дальность)
- 100 см (дальняя зона)
- 200 см (максимальная дальность)

Требования к эталону:
- Погрешность эталонной линейки: ±1 мм
- Перпендикулярность поверхности
- Стабильность окружающих условий
- Отсутствие посторонних отражений

struct CalibrationPoint {
    float trueDistance;
    float measuredDistance;
    int sampleCount;
    float standardDeviation;
};

class CalibrationManager {
private:
    CalibrationPoint points[MAX_CAL_POINTS];
    int pointCount = 0;
    
public:
    void collectCalibrationData() {
        Serial.println("=== КАЛИБРОВКА ДАТЧИКА ===");
        
        float distances[] = {5, 10, 20, 50, 100, 200};
        int numDistances = sizeof(distances) / sizeof(distances[0]);
        
        for(int i = 0; i < numDistances; i++) {
            Serial.print("Установите препятствие на расстоянии ");
            Serial.print(distances[i]);
            Serial.println(" см и нажмите Enter");
            
            waitForUserInput();
            
            // Сбор множественных измерений
            float sum = 0;
            float sumSquares = 0;
            const int SAMPLES = 50;
            
            for(int j = 0; j < SAMPLES; j++) {
                float measurement = sensor.getRawDistance();
                sum += measurement;
                sumSquares += measurement * measurement;
                delay(20);
            }
            
            points[pointCount].trueDistance = distances[i];
            points[pointCount].measuredDistance = sum / SAMPLES;
            points[pointCount].sampleCount = SAMPLES;
            
            // Расчет стандартного отклонения
            float variance = (sumSquares / SAMPLES) - 
                           pow(points[pointCount].measuredDistance, 2);
            points[pointCount].standardDeviation = sqrt(variance);
            
            pointCount++;
            
            Serial.print("Среднее: ");
            Serial.print(points[pointCount-1].measuredDistance);
            Serial.print(" см, σ = ");
            Serial.println(points[pointCount-1].standardDeviation);
        }
    }
};

class LinearRegression {
public:
    struct RegressionResult {
        float slope;        // Коэффициент a
        float intercept;    // Коэффициент b
        float correlation;  // Коэффициент корреляции
        float rSquared;     // Коэффициент детерминации
    };
    
    RegressionResult calculateRegression(CalibrationPoint* points, int count) {
        float sumX = 0, sumY = 0, sumXY = 0, sumX2 = 0, sumY2 = 0;
        
        for(int i = 0; i < count; i++) {
            float x = points[i].measuredDistance;
            float y = points[i].trueDistance;
            
            sumX += x;
            sumY += y;
            sumXY += x * y;
            sumX2 += x * x;
            sumY2 += y * y;
        }
        
        float n = count;
        RegressionResult result;
        
        // Расчет коэффициентов
        result.slope = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX);
        result.intercept = (sumY - result.slope * sumX) / n;
        
        // Коэффициент корреляции
        float numerator = n * sumXY - sumX * sumY;
        float denominator = sqrt((n * sumX2 - sumX * sumX) * (n * sumY2 - sumY * sumY));
        result.correlation = numerator / denominator;
        result.rSquared = result.correlation * result.correlation;
        
        return result;
    }
};

void analyzeCalibrationQuality(RegressionResult result) {
    Serial.println("=== РЕЗУЛЬТАТЫ КАЛИБРОВКИ ===");
    Serial.print("Коэффициент масштабирования (a): ");
    Serial.println(result.slope, 4);
    Serial.print("Смещение (b): ");
    Serial.println(result.intercept, 4);
    Serial.print("Коэффициент корреляции (r): ");
    Serial.println(result.correlation, 4);
    Serial.print("Коэффициент детерминации (R²): ");
    Serial.println(result.rSquared, 4);
    
    // Оценка качества калибровки
    if (result.rSquared > 0.99) {
        Serial.println("✓ Отличная калибровка");
    } else if (result.rSquared > 0.95) {
        Serial.println("✓ Хорошая калибровка");
    } else if (result.rSquared > 0.90) {
        Serial.println("⚠️ Удовлетворительная калибровка");
    } else {
        Serial.println("❌ Плохая калибровка - требуется повтор");
    }
    
    // Формула калибровки
    Serial.println("\nФормула калибровки:");
    Serial.print("d_истинное = ");
    Serial.print(result.slope, 4);
    Serial.print(" × d_измеренное + ");
    Serial.println(result.intercept, 4);
}

class CalibratedSensor {
private:
    float calibrationSlope = 1.0;
    float calibrationIntercept = 0.0;
    bool isCalibrated = false;
    
    // Буфер для фильтрации
    float measurementBuffer[FILTER_SIZE];
    int bufferIndex = 0;
    bool bufferFull = false;
    
public:
    void setCalibrationParameters(float slope, float intercept) {
        calibrationSlope = slope;
        calibrationIntercept = intercept;
        isCalibrated = true;
        
        Serial.println("Калибровка применена");
        Serial.print("Формула: d = ");
        Serial.print(slope, 4);
        Serial.print(" × raw + ");
        Serial.println(intercept, 4);
    }
    
    float getCalibratedDistance() {
        // Получаем сырое измерение
        float rawDistance = getRawMeasurement();
        
        // Применяем медианный фильтр
        float filteredDistance = applyMedianFilter(rawDistance);
        
        // Применяем калибровку
        float calibratedDistance = calibrationSlope * filteredDistance + 
                                 calibrationIntercept;
        
        // Проверяем на валидность
        if (isValidMeasurement(calibratedDistance)) {
            return calibratedDistance;
        } else {
            return -1; // Невалидное измерение
        }
    }
    
private:
    float applyMedianFilter(float newValue) {
        // Добавляем новое значение в буфер
        measurementBuffer[bufferIndex] = newValue;
        bufferIndex = (bufferIndex + 1) % FILTER_SIZE;
        
        if (bufferIndex == 0) bufferFull = true;
        
        // Копируем буфер для сортировки
        float sortedBuffer[FILTER_SIZE];
        int actualSize = bufferFull ? FILTER_SIZE : bufferIndex;
        
        for(int i = 0; i < actualSize; i++) {
            sortedBuffer[i] = measurementBuffer[i];
        }
        
        // Сортировка пузырьком (для небольших массивов)
        for(int i = 0; i < actualSize - 1; i++) {
            for(int j = 0; j < actualSize - i - 1; j++) {
                if(sortedBuffer[j] > sortedBuffer[j + 1]) {
                    float temp = sortedBuffer[j];
                    sortedBuffer[j] = sortedBuffer[j + 1];
                    sortedBuffer[j + 1] = temp;
                }
            }
        }
        
        // Возвращаем медианное значение
        return sortedBuffer[actualSize / 2];
    }
    
    bool isValidMeasurement(float distance) {
        return (distance >= MIN_VALID_DISTANCE && 
                distance <= MAX_VALID_DISTANCE);
    }
};

class AutoCalibrationSystem {
private:
    struct ReferenceObject {
        float knownDistance;
        float measuredDistance;
        unsigned long timestamp;
    };
    
    ReferenceObject references[MAX_REFERENCES];
    int referenceCount = 0;
    
public:
    void enableAutomaticCalibration() {
        Serial.println("Автоматическая калибровка включена");
        Serial.println("Система будет обучаться на известных объектах");
    }
    
    void addReferenceObject(float knownDistance) {
        if (referenceCount < MAX_REFERENCES) {
            references[referenceCount].knownDistance = knownDistance;
            references[referenceCount].measuredDistance = getCurrentMeasurement();
            references[referenceCount].timestamp = millis();
            referenceCount++;
            
            Serial.print("Добавлен эталонный объект: ");
            Serial.print(knownDistance);
            Serial.println(" см");
            
            // Автоматический перерасчет калибровки
            if (referenceCount >= 3) {
                recalculateCalibration();
            }
        }
    }
    
private:
    void recalculateCalibration() {
        // Используем только свежие данные (последние 10 минут)
        unsigned long currentTime = millis();
        int validReferences = 0;
        
        for(int i = 0; i < referenceCount; i++) {
            if (currentTime - references[i].timestamp < 600000) { // 10 минут
                validReferences++;
            }
        }
        
        if (validReferences >= 3) {
            LinearRegression regression;
            RegressionResult result = regression.calculateRegression(
                (CalibrationPoint*)references, validReferences);
            
            if (result.rSquared > 0.95) {
                sensor.setCalibrationParameters(result.slope, result.intercept);
                Serial.println("Автоматическая калибровка обновлена");
            }
        }
    }
};

class ObstacleAvoidanceController {
private:
    enum RobotState {
        MOVING_FORWARD,
        OBSTACLE_DETECTED,
        EMERGENCY_STOP,
        TURNING_RIGHT,
        TURNING_LEFT,
        BACKING_UP
    };
    
    RobotState currentState = MOVING_FORWARD;
    unsigned long stateChangeTime = 0;
    
    // Параметры безопасности
    const float EMERGENCY_DISTANCE = 5.0;   // см
    const float WARNING_DISTANCE = 15.0;    // см
    const float SAFE_DISTANCE = 30.0;       // см
    
    // Параметры движения
    const int NORMAL_SPEED = 150;           // 0-255
    const int SLOW_SPEED = 80;
    const int TURN_SPEED = 120;
    
public:
    void updateBehavior() {
        float distance = sensor.getCalibratedDistance();
        unsigned long currentTime = millis();
        
        // Конечный автомат поведения
        switch(currentState) {
            case MOVING_FORWARD:
                handleForwardMovement(distance);
                break;
                
            case OBSTACLE_DETECTED:
                handleObstacleDetection(distance, currentTime);
                break;
                
            case EMERGENCY_STOP:
                handleEmergencyStop(distance, currentTime);
                break;
                
            case TURNING_RIGHT:
                handleTurning(currentTime);
                break;
                
            case BACKING_UP:
                handleBackingUp(currentTime);
                break;
        }
        
        // Обновление индикации
        updateStatusLED();
        
        // Логирование состояния
        logRobotState(distance);
    }
    
private:
    void handleForwardMovement(float distance) {
        if (distance <= EMERGENCY_DISTANCE) {
            emergencyStop();
            changeState(EMERGENCY_STOP);
        } else if (distance <= WARNING_DISTANCE) {
            slowDown();
            changeState(OBSTACLE_DETECTED);
        } else if (distance <= SAFE_DISTANCE) {
            moveForwardSlow();
        } else {
            moveForwardNormal();
        }
    }
    
    void handleObstacleDetection(float distance, unsigned long currentTime) {
        if (distance <= EMERGENCY_DISTANCE) {
            emergencyStop();
            changeState(EMERGENCY_STOP);
        } else if (distance > SAFE_DISTANCE) {
            // Препятствие исчезло
            changeState(MOVING_FORWARD);
        } else {
            // Начинаем маневр объезда
            if (currentTime - stateChangeTime > 500) { // Задержка для стабилизации
                startObstacleAvoidance();
            }
        }
    }
    
    void handleEmergencyStop(float distance, unsigned long currentTime) {
        stop();
        
        if (distance > WARNING_DISTANCE) {
            // Безопасно возобновить движение
            changeState(MOVING_FORWARD);
        } else if (currentTime - stateChangeTime > 1000) {
            // Попытка объезда через 1 секунду
            changeState(BACKING_UP);
        }
    }
    
    void startObstacleAvoidance() {
        // Проверяем свободное пространство справа
        if (checkRightSide()) {
            changeState(TURNING_RIGHT);
        } else {
            // Отъезжаем назад и пробуем снова
            changeState(BACKING_UP);
        }
    }
    
    bool checkRightSide() {
        // Поворачиваем датчик направо (если есть сервопривод)
        if (servoAttached) {
            servo.write(45); // Поворот на 45° вправо
            delay(500);
            
            float rightDistance = sensor.getCalibratedDistance();
            
            servo.write(90); // Возврат в центр
            delay(300);
            
            return rightDistance > SAFE_DISTANCE;
        } else {
            // Если сервопривода нет, используем эвристику
            return true; // Предполагаем, что справа свободно
        }
    }
};

class WallFollowingController {
private:
    const float TARGET_WALL_DISTANCE = 20.0; // см
    const float WALL_TOLERANCE = 3.0;        // см
    
    // PID-регулятор для плавного следования
    float pidKp = 2.0;
    float pidKi = 0.1;
    float pidKd = 0.5;
    
    float previousError = 0;
    float integralError = 0;
    unsigned long lastUpdateTime = 0;
    
public:
    void followWall() {
        float wallDistance = sensor.getCalibratedDistance();
        unsigned long currentTime = millis();
        float deltaTime = (currentTime - lastUpdateTime) / 1000.0; // секунды
        
        if (deltaTime > 0) {
            // Расчет ошибки
            float error = TARGET_WALL_DISTANCE - wallDistance;
            
            // PID-регулятор
            integralError += error * deltaTime;
            float derivative = (error - previousError) / deltaTime;
            
            float pidOutput = pidKp * error + 
                            pidKi * integralError + 
                            pidKd * derivative;
            
            // Ограничение выходного сигнала
            pidOutput = constrain(pidOutput, -100, 100);
            
            // Применение коррекции к движению
            int leftSpeed = NORMAL_SPEED + pidOutput;
            int rightSpeed = NORMAL_SPEED - pidOutput;
            
            // Ограничение скоростей
            leftSpeed = constrain(leftSpeed, 0, 255);
            rightSpeed = constrain(rightSpeed, 0, 255);
            
            // Управление моторами
            setMotorSpeeds(leftSpeed, rightSpeed);
            
            // Обновление переменных
            previousError = error;
            lastUpdateTime = currentTime;
            
            // Отладочная информация
            Serial.print("Ошибка: ");
            Serial.print(error);
            Serial.print(", PID: ");
            Serial.print(pidOutput);
            Serial.print(", Скорости: L=");
            Serial.print(leftSpeed);
            Serial.print(", R=");
            Serial.println(rightSpeed);
        }
    }
    
    void tuneParameters(float kp, float ki, float kd) {
        pidKp = kp;
        pidKi = ki;
        pidKd = kd;
        
        // Сброс интегральной составляющей
        integralError = 0;
        
        Serial.println("PID параметры обновлены:");
        Serial.print("Kp="); Serial.print(pidKp);
        Serial.print(", Ki="); Serial.print(pidKi);
        Serial.print(", Kd="); Serial.println(pidKd);
    }
};

class SimpleMapper {
private:
    struct MapPoint {
        float x, y;           // Координаты в см
        float distance;       // Расстояние до препятствия
        bool obstacle;        // Наличие препятствия
        unsigned long timestamp;
    };
    
    MapPoint map[MAX_MAP_POINTS];
    int mapSize = 0;
    
    float robotX = 0, robotY = 0;     // Позиция робота
    float robotAngle = 0;             // Ориентация робота (радианы)
    
public:
    void updateMap() {
        float distance = sensor.getCalibratedDistance();
        
        if (distance > 0 && distance < MAX_MAPPING_RANGE) {
            // Расчет координат препятствия
            float obstacleX = robotX + distance * cos(robotAngle);
            float obstacleY = robotY + distance * sin(robotAngle);
            
            // Добавление точки на карту
            addMapPoint(obstacleX, obstacleY, distance, true);
            
            // Также отмечаем свободное пространство
            markFreeSpace(robotX, robotY, obstacleX, obstacleY);
        }
    }
    
    void addMapPoint(float x, float y, float dist, bool isObstacle) {
        if (mapSize < MAX_MAP_POINTS) {
            map[mapSize].x = x;
            map[mapSize].y = y;
            map[mapSize].distance = dist;
            map[mapSize].obstacle = isObstacle;
            map[mapSize].timestamp = millis();
            mapSize++;
        }
    }
    
    void printMap() {
        Serial.println("=== КАРТА ОКРУЖЕНИЯ ===");
        for(int i = 0; i < mapSize; i++) {
            Serial.print("Точка ");
            Serial.print(i);
            Serial.print(": (");
            Serial.print(map[i].x);
            Serial.print(", ");
            Serial.print(map[i].y);
            Serial.print(") - ");
            Serial.println(map[i].obstacle ? "Препятствие" : "Свободно");
        }
    }
    
    bool isPathClear(float targetX, float targetY) {
        // Простая проверка на наличие препятствий на пути
        for(int i = 0; i < mapSize; i++) {
            if (map[i].obstacle) {
                float distToObstacle = sqrt(pow(map[i].x - targetX, 2) + 
                                          pow(map[i].y - targetY, 2));
                if (distToObstacle < ROBOT_RADIUS + SAFETY_MARGIN) {
                    return false;
                }
            }
        }
        return true;
    }
};

class AdaptiveBehavior {
private:
    struct BehaviorPattern {
        float situation[4];    // [distance, speed, turn_angle, success_rate]
        int action;           // Выбранное действие
        float reward;         // Награда за действие
        int usage_count;      // Количество использований
    };
    
    BehaviorPattern patterns[MAX_PATTERNS];
    int patternCount = 0;
    
    // Q-learning параметры
    float learningRate = 0.1;
    float discountFactor = 0.9;
    float explorationRate = 0.2;
    
public:
    int selectAction(float distance, float speed, float turnAngle) {
        float situation[4] = {distance, speed, turnAngle, 0};
        
        // Exploration vs Exploitation
        if (random(100) < explorationRate * 100) {
            // Исследование: случайное действие
            return random(NUM_ACTIONS);
        } else {
            // Эксплуатация: лучшее известное действие
            return getBestAction(situation);
        }
    }
    
    void updatePattern(float* situation, int action, float reward) {
        // Поиск соответствующего паттерна
        int patternIndex = findOrCreatePattern(situation, action);
        
        if (patternIndex >= 0) {
            // Q-learning обновление
            float oldQ = patterns[patternIndex].reward;
            float newQ = oldQ + learningRate * (reward - oldQ);
            
            patterns[patternIndex].reward = newQ;
            patterns[patternIndex].usage_count++;
            
            // Адаптация exploration rate
            explorationRate = max(0.05, explorationRate * 0.995);
        }
    }
    
private:
    int getBestAction(float* situation) {
        float bestReward = -1000;
        int bestAction = 0;
        
        for(int i = 0; i < patternCount; i++) {
            if (isSimilarSituation(patterns[i].situation, situation)) {
                if (patterns[i].reward > bestReward) {
                    bestReward = patterns[i].reward;
                    bestAction = patterns[i].action;
                }
            }
        }
        
        return bestAction;
    }
    
    bool isSimilarSituation(float* pattern, float* current) {
        float threshold = 0.2; // 20% допуск
        
        for(int i = 0; i < 3; i++) {
            float difference = abs(pattern[i] - current[i]) / max(pattern[i], current[i]);
            if (difference > threshold) {
                return false;
            }
        }
        return true;
    }
};

class TestingFramework {
private:
    struct TestResult {
        String testName;
        bool passed;
        float score;
        String details;
        unsigned long duration;
    };
    
    TestResult results[MAX_TESTS];
    int testCount = 0;
    
public:
    void runFullTestSuite() {
        Serial.println("=== КОМПЛЕКСНОЕ ТЕСТИРОВАНИЕ ===");
        unsigned long startTime = millis();
        
        // 1. Тесты датчика
        runSensorTests();
        
        // 2. Тесты калибровки
        runCalibrationTests();
        
        // 3. Тесты алгоритмов
        runAlgorithmTests();
        
        // 4. Интеграционные тесты
        runIntegrationTests();
        
        // 5. Тесты производительности
        runPerformanceTests();
        
        unsigned long totalTime = millis() - startTime;
        generateTestReport(totalTime);
    }
    
private:
    void runSensorTests() {
        Serial.println("\n--- ТЕСТИРОВАНИЕ ДАТЧИКА ---");
        
        // Тест 1: Базовая функциональность
        addTestResult("Базовая функциональность", 
                     testBasicSensorFunction());
        
        // Тест 2: Точность измерений
        addTestResult("Точность измерений", 
                     testMeasurementAccuracy());
        
        // Тест 3: Стабильность показаний
        addTestResult("Стабильность показаний", 
                     testMeasurementStability());
        
        // Тест 4: Время отклика
        addTestResult("Время отклика", 
                     testResponseTime());
    }
    
    bool testBasicSensorFunction() {
        int successCount = 0;
        const int ATTEMPTS = 10;
        
        for(int i = 0; i < ATTEMPTS; i++) {
            float distance = sensor.getCalibratedDistance();
            if (distance > 0 && distance < 500) {
                successCount++;
            }
            delay(100);
        }
        
        float successRate = (float)successCount / ATTEMPTS;
        Serial.print("Успешных измерений: ");
        Serial.print(successRate * 100);
        Serial.println("%");
        
        return successRate > 0.9; // 90% успешности
    }
    
    bool testMeasurementAccuracy() {
        Serial.println("Поместите препятствие на 30 см от датчика");
        delay(3000);
        
        float sum = 0;
        const int SAMPLES = 20;
        
        for(int i = 0; i < SAMPLES; i++) {
            sum += sensor.getCalibratedDistance();
            delay(50);
        }
        
        float average = sum / SAMPLES;
        float error = abs(average - 30.0);
        
        Serial.print("Измеренное расстояние: ");
        Serial.println(average);
        Serial.print("Ошибка: ");
        Serial.println(error);
        
        return error < 3.0; // Ошибка менее 3 см
    }
    
    bool testMeasurementStability() {
        float readings[50];
        
        for(int i = 0; i < 50; i++) {
            readings[i] = sensor.getCalibratedDistance();
            delay(20);
        }
        
        float mean = calculateMean(readings, 50);
        float stdDev = calculateStdDev(readings, 50, mean);
        
        Serial.print("Стандартное отклонение: ");
        Serial.println(stdDev);
        
        return stdDev < 2.0; // Отклонение менее 2 см
    }
};

class DebugLogger {
private:
    bool loggingEnabled = true;
    int logLevel = LOG_INFO;
    
    enum LogLevel {
        LOG_ERROR = 0,
        LOG_WARNING = 1,
        LOG_INFO = 2,
        LOG_DEBUG = 3
    };
    
public:
    void logSensorReading(float distance, bool isValid) {
        if (logLevel >= LOG_DEBUG) {
            Serial.print("[SENSOR] Distance: ");
            Serial.print(distance);
            Serial.print(" cm, Valid: ");
            Serial.println(isValid ? "Yes" : "No");
        }
    }
    
    void logRobotState(RobotState state, float distance) {
        if (logLevel >= LOG_INFO) {
            Serial.print("[STATE] ");
            Serial.print(millis());
            Serial.print(" ms: ");
            Serial.print(getStateName(state));
            Serial.print(", Distance: ");
            Serial.println(distance);
        }
    }
    
    void logAlgorithmDecision(String algorithm, String decision, float confidence) {
        if (logLevel >= LOG_DEBUG) {
            Serial.print("[ALGORITHM] ");
            Serial.print(algorithm);
            Serial.print(" decided: ");
            Serial.print(decision);
            Serial.print(" (confidence: ");
            Serial.print(confidence);
            Serial.println(")");
        }
    }
    
    void logError(String component, String error) {
        if (logLevel >= LOG_ERROR) {
            Serial.print("[ERROR] ");
            Serial.print(component);
            Serial.print(": ");
            Serial.println(error);
        }
    }
    
    void generatePerformanceReport() {
        Serial.println("\n=== ОТЧЕТ О ПРОИЗВОДИТЕЛЬНОСТИ ===");
        Serial.print("Время работы: ");
        Serial.print(millis() / 1000);
        Serial.println(" секунд");
        
        Serial.print("Частота измерений: ");
        Serial.print(measurementCount * 1000.0 / millis());
        Serial.println(" Гц");
        
        Serial.print("Успешных измерений: ");
        Serial.print((float)validMeasurements / measurementCount * 100);
        Serial.println("%");
    }
};

class ScenarioTester {
public:
    void testEmergencyStop() {
        Serial.println("\n=== ТЕСТ ЭКСТРЕННОЙ ОСТАНОВКИ ===");
        
        // Эмуляция приближения к препятствию
        robot.setSpeed(200);
        robot.moveForward();
        
        // Симуляция постепенного приближения
        for(float distance = 50; distance >= 5; distance -= 2) {
            sensor.simulateReading(distance);
            controller.updateBehavior();
            
            if (robot.isStopped() && distance <= EMERGENCY_DISTANCE) {
                Serial.println("✓ Экстренная остановка сработала корректно");
                return;
            }
            
            delay(100);
        }
        
        Serial.println("❌ Экстренная остановка НЕ сработала!");
    }
    
    void testObstacleAvoidance() {
        Serial.println("\n=== ТЕСТ ОБЪЕЗДА ПРЕПЯТСТВИЙ ===");
        
        // Установка препятствия спереди
        sensor.simulateReading(10); // 10 см до препятствия
        
        unsigned long startTime = millis();
        bool obstacleAvoided = false;
        
        while(millis() - startTime < 10000) { // 10 секунд на маневр
            controller.updateBehavior();
            
            // Проверяем, удалось ли объехать
            if (robot.getX() != 0 || robot.getY() != 0) {
                obstacleAvoided = true;
                break;
            }
            
            delay(50);
        }
        
        if (obstacleAvoided) {
            Serial.println("✓ Препятствие успешно объехано");
        } else {
            Serial.println("❌ Не удалось объехать препятствие");
        }
    }
    
    void testNoiseHandling() {
        Serial.println("\n=== ТЕСТ УСТОЙЧИВОСТИ К ШУМАМ ===");
        
        float baseDistance = 30.0;
        int validReadings = 0;
        
        for(int i = 0; i < 100; i++) {
            // Добавляем случайный шум ±20%
            float noise = random(-20, 21) / 100.0;
            float noisyDistance = baseDistance * (1 + noise);
            
            sensor.simulateReading(noisyDistance);
            float filteredDistance = sensor.getCalibratedDistance();
            
            // Проверяем, что фильтр справляется с шумом
            if (abs(filteredDistance - baseDistance) < 5.0) {
                validReadings++;
            }
            
            delay(10);
        }
        
        float accuracy = (float)validReadings / 100 * 100;
        Serial.print("Точность при наличии шума: ");
        Serial.print(accuracy);
        Serial.println("%");
        
        if (accuracy > 80) {
            Serial.println("✓ Система устойчива к шумам");
        } else {
            Serial.println("❌ Требуется улучшение фильтрации");
        }
    }
};