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

 1Рекомендуемые контрольные точки:
 2- 5 см (минимальная дальность)
 3- 10 см (ближняя зона)
 4- 20 см (рабочая зона)
 5- 50 см (средняя дальность)
 6- 100 см (дальняя зона)
 7- 200 см (максимальная дальность)
 8
 9Требования к эталону:
10- Погрешность эталонной линейки: ±1 мм
11- Перпендикулярность поверхности
12- Стабильность окружающих условий
13- Отсутствие посторонних отражений

 1struct CalibrationPoint {
 2    float trueDistance;
 3    float measuredDistance;
 4    int sampleCount;
 5    float standardDeviation;
 6};
 7
 8class CalibrationManager {
 9private:
10    CalibrationPoint points[MAX_CAL_POINTS];
11    int pointCount = 0;
12    
13public:
14    void collectCalibrationData() {
15        Serial.println("=== КАЛИБРОВКА ДАТЧИКА ===");
16        
17        float distances[] = {5, 10, 20, 50, 100, 200};
18        int numDistances = sizeof(distances) / sizeof(distances[0]);
19        
20        for(int i = 0; i < numDistances; i++) {
21            Serial.print("Установите препятствие на расстоянии ");
22            Serial.print(distances[i]);
23            Serial.println(" см и нажмите Enter");
24            
25            waitForUserInput();
26            
27            // Сбор множественных измерений
28            float sum = 0;
29            float sumSquares = 0;
30            const int SAMPLES = 50;
31            
32            for(int j = 0; j < SAMPLES; j++) {
33                float measurement = sensor.getRawDistance();
34                sum += measurement;
35                sumSquares += measurement * measurement;
36                delay(20);
37            }
38            
39            points[pointCount].trueDistance = distances[i];
40            points[pointCount].measuredDistance = sum / SAMPLES;
41            points[pointCount].sampleCount = SAMPLES;
42            
43            // Расчет стандартного отклонения
44            float variance = (sumSquares / SAMPLES) - 
45                           pow(points[pointCount].measuredDistance, 2);
46            points[pointCount].standardDeviation = sqrt(variance);
47            
48            pointCount++;
49            
50            Serial.print("Среднее: ");
51            Serial.print(points[pointCount-1].measuredDistance);
52            Serial.print(" см, σ = ");
53            Serial.println(points[pointCount-1].standardDeviation);
54        }
55    }
56};

 1class LinearRegression {
 2public:
 3    struct RegressionResult {
 4        float slope;        // Коэффициент a
 5        float intercept;    // Коэффициент b
 6        float correlation;  // Коэффициент корреляции
 7        float rSquared;     // Коэффициент детерминации
 8    };
 9    
10    RegressionResult calculateRegression(CalibrationPoint* points, int count) {
11        float sumX = 0, sumY = 0, sumXY = 0, sumX2 = 0, sumY2 = 0;
12        
13        for(int i = 0; i < count; i++) {
14            float x = points[i].measuredDistance;
15            float y = points[i].trueDistance;
16            
17            sumX += x;
18            sumY += y;
19            sumXY += x * y;
20            sumX2 += x * x;
21            sumY2 += y * y;
22        }
23        
24        float n = count;
25        RegressionResult result;
26        
27        // Расчет коэффициентов
28        result.slope = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX);
29        result.intercept = (sumY - result.slope * sumX) / n;
30        
31        // Коэффициент корреляции
32        float numerator = n * sumXY - sumX * sumY;
33        float denominator = sqrt((n * sumX2 - sumX * sumX) * (n * sumY2 - sumY * sumY));
34        result.correlation = numerator / denominator;
35        result.rSquared = result.correlation * result.correlation;
36        
37        return result;
38    }
39};

 1void analyzeCalibrationQuality(RegressionResult result) {
 2    Serial.println("=== РЕЗУЛЬТАТЫ КАЛИБРОВКИ ===");
 3    Serial.print("Коэффициент масштабирования (a): ");
 4    Serial.println(result.slope, 4);
 5    Serial.print("Смещение (b): ");
 6    Serial.println(result.intercept, 4);
 7    Serial.print("Коэффициент корреляции (r): ");
 8    Serial.println(result.correlation, 4);
 9    Serial.print("Коэффициент детерминации (R²): ");
10    Serial.println(result.rSquared, 4);
11    
12    // Оценка качества калибровки
13    if (result.rSquared > 0.99) {
14        Serial.println("✓ Отличная калибровка");
15    } else if (result.rSquared > 0.95) {
16        Serial.println("✓ Хорошая калибровка");
17    } else if (result.rSquared > 0.90) {
18        Serial.println("⚠️ Удовлетворительная калибровка");
19    } else {
20        Serial.println("❌ Плохая калибровка - требуется повтор");
21    }
22    
23    // Формула калибровки
24    Serial.println("\nФормула калибровки:");
25    Serial.print("d_истинное = ");
26    Serial.print(result.slope, 4);
27    Serial.print(" × d_измеренное + ");
28    Serial.println(result.intercept, 4);
29}

 1class CalibratedSensor {
 2private:
 3    float calibrationSlope = 1.0;
 4    float calibrationIntercept = 0.0;
 5    bool isCalibrated = false;
 6    
 7    // Буфер для фильтрации
 8    float measurementBuffer[FILTER_SIZE];
 9    int bufferIndex = 0;
10    bool bufferFull = false;
11    
12public:
13    void setCalibrationParameters(float slope, float intercept) {
14        calibrationSlope = slope;
15        calibrationIntercept = intercept;
16        isCalibrated = true;
17        
18        Serial.println("Калибровка применена");
19        Serial.print("Формула: d = ");
20        Serial.print(slope, 4);
21        Serial.print(" × raw + ");
22        Serial.println(intercept, 4);
23    }
24    
25    float getCalibratedDistance() {
26        // Получаем сырое измерение
27        float rawDistance = getRawMeasurement();
28        
29        // Применяем медианный фильтр
30        float filteredDistance = applyMedianFilter(rawDistance);
31        
32        // Применяем калибровку
33        float calibratedDistance = calibrationSlope * filteredDistance + 
34                                 calibrationIntercept;
35        
36        // Проверяем на валидность
37        if (isValidMeasurement(calibratedDistance)) {
38            return calibratedDistance;
39        } else {
40            return -1; // Невалидное измерение
41        }
42    }
43    
44private:
45    float applyMedianFilter(float newValue) {
46        // Добавляем новое значение в буфер
47        measurementBuffer[bufferIndex] = newValue;
48        bufferIndex = (bufferIndex + 1) % FILTER_SIZE;
49        
50        if (bufferIndex == 0) bufferFull = true;
51        
52        // Копируем буфер для сортировки
53        float sortedBuffer[FILTER_SIZE];
54        int actualSize = bufferFull ? FILTER_SIZE : bufferIndex;
55        
56        for(int i = 0; i < actualSize; i++) {
57            sortedBuffer[i] = measurementBuffer[i];
58        }
59        
60        // Сортировка пузырьком (для небольших массивов)
61        for(int i = 0; i < actualSize - 1; i++) {
62            for(int j = 0; j < actualSize - i - 1; j++) {
63                if(sortedBuffer[j] > sortedBuffer[j + 1]) {
64                    float temp = sortedBuffer[j];
65                    sortedBuffer[j] = sortedBuffer[j + 1];
66                    sortedBuffer[j + 1] = temp;
67                }
68            }
69        }
70        
71        // Возвращаем медианное значение
72        return sortedBuffer[actualSize / 2];
73    }
74    
75    bool isValidMeasurement(float distance) {
76        return (distance >= MIN_VALID_DISTANCE && 
77                distance <= MAX_VALID_DISTANCE);
78    }
79};

 1class AutoCalibrationSystem {
 2private:
 3    struct ReferenceObject {
 4        float knownDistance;
 5        float measuredDistance;
 6        unsigned long timestamp;
 7    };
 8    
 9    ReferenceObject references[MAX_REFERENCES];
10    int referenceCount = 0;
11    
12public:
13    void enableAutomaticCalibration() {
14        Serial.println("Автоматическая калибровка включена");
15        Serial.println("Система будет обучаться на известных объектах");
16    }
17    
18    void addReferenceObject(float knownDistance) {
19        if (referenceCount < MAX_REFERENCES) {
20            references[referenceCount].knownDistance = knownDistance;
21            references[referenceCount].measuredDistance = getCurrentMeasurement();
22            references[referenceCount].timestamp = millis();
23            referenceCount++;
24            
25            Serial.print("Добавлен эталонный объект: ");
26            Serial.print(knownDistance);
27            Serial.println(" см");
28            
29            // Автоматический перерасчет калибровки
30            if (referenceCount >= 3) {
31                recalculateCalibration();
32            }
33        }
34    }
35    
36private:
37    void recalculateCalibration() {
38        // Используем только свежие данные (последние 10 минут)
39        unsigned long currentTime = millis();
40        int validReferences = 0;
41        
42        for(int i = 0; i < referenceCount; i++) {
43            if (currentTime - references[i].timestamp < 600000) { // 10 минут
44                validReferences++;
45            }
46        }
47        
48        if (validReferences >= 3) {
49            LinearRegression regression;
50            RegressionResult result = regression.calculateRegression(
51                (CalibrationPoint*)references, validReferences);
52            
53            if (result.rSquared > 0.95) {
54                sensor.setCalibrationParameters(result.slope, result.intercept);
55                Serial.println("Автоматическая калибровка обновлена");
56            }
57        }
58    }
59};

  1class ObstacleAvoidanceController {
  2private:
  3    enum RobotState {
  4        MOVING_FORWARD,
  5        OBSTACLE_DETECTED,
  6        EMERGENCY_STOP,
  7        TURNING_RIGHT,
  8        TURNING_LEFT,
  9        BACKING_UP
 10    };
 11    
 12    RobotState currentState = MOVING_FORWARD;
 13    unsigned long stateChangeTime = 0;
 14    
 15    // Параметры безопасности
 16    const float EMERGENCY_DISTANCE = 5.0;   // см
 17    const float WARNING_DISTANCE = 15.0;    // см
 18    const float SAFE_DISTANCE = 30.0;       // см
 19    
 20    // Параметры движения
 21    const int NORMAL_SPEED = 150;           // 0-255
 22    const int SLOW_SPEED = 80;
 23    const int TURN_SPEED = 120;
 24    
 25public:
 26    void updateBehavior() {
 27        float distance = sensor.getCalibratedDistance();
 28        unsigned long currentTime = millis();
 29        
 30        // Конечный автомат поведения
 31        switch(currentState) {
 32            case MOVING_FORWARD:
 33                handleForwardMovement(distance);
 34                break;
 35                
 36            case OBSTACLE_DETECTED:
 37                handleObstacleDetection(distance, currentTime);
 38                break;
 39                
 40            case EMERGENCY_STOP:
 41                handleEmergencyStop(distance, currentTime);
 42                break;
 43                
 44            case TURNING_RIGHT:
 45                handleTurning(currentTime);
 46                break;
 47                
 48            case BACKING_UP:
 49                handleBackingUp(currentTime);
 50                break;
 51        }
 52        
 53        // Обновление индикации
 54        updateStatusLED();
 55        
 56        // Логирование состояния
 57        logRobotState(distance);
 58    }
 59    
 60private:
 61    void handleForwardMovement(float distance) {
 62        if (distance <= EMERGENCY_DISTANCE) {
 63            emergencyStop();
 64            changeState(EMERGENCY_STOP);
 65        } else if (distance <= WARNING_DISTANCE) {
 66            slowDown();
 67            changeState(OBSTACLE_DETECTED);
 68        } else if (distance <= SAFE_DISTANCE) {
 69            moveForwardSlow();
 70        } else {
 71            moveForwardNormal();
 72        }
 73    }
 74    
 75    void handleObstacleDetection(float distance, unsigned long currentTime) {
 76        if (distance <= EMERGENCY_DISTANCE) {
 77            emergencyStop();
 78            changeState(EMERGENCY_STOP);
 79        } else if (distance > SAFE_DISTANCE) {
 80            // Препятствие исчезло
 81            changeState(MOVING_FORWARD);
 82        } else {
 83            // Начинаем маневр объезда
 84            if (currentTime - stateChangeTime > 500) { // Задержка для стабилизации
 85                startObstacleAvoidance();
 86            }
 87        }
 88    }
 89    
 90    void handleEmergencyStop(float distance, unsigned long currentTime) {
 91        stop();
 92        
 93        if (distance > WARNING_DISTANCE) {
 94            // Безопасно возобновить движение
 95            changeState(MOVING_FORWARD);
 96        } else if (currentTime - stateChangeTime > 1000) {
 97            // Попытка объезда через 1 секунду
 98            changeState(BACKING_UP);
 99        }
100    }
101    
102    void startObstacleAvoidance() {
103        // Проверяем свободное пространство справа
104        if (checkRightSide()) {
105            changeState(TURNING_RIGHT);
106        } else {
107            // Отъезжаем назад и пробуем снова
108            changeState(BACKING_UP);
109        }
110    }
111    
112    bool checkRightSide() {
113        // Поворачиваем датчик направо (если есть сервопривод)
114        if (servoAttached) {
115            servo.write(45); // Поворот на 45° вправо
116            delay(500);
117            
118            float rightDistance = sensor.getCalibratedDistance();
119            
120            servo.write(90); // Возврат в центр
121            delay(300);
122            
123            return rightDistance > SAFE_DISTANCE;
124        } else {
125            // Если сервопривода нет, используем эвристику
126            return true; // Предполагаем, что справа свободно
127        }
128    }
129};

 1class WallFollowingController {
 2private:
 3    const float TARGET_WALL_DISTANCE = 20.0; // см
 4    const float WALL_TOLERANCE = 3.0;        // см
 5    
 6    // PID-регулятор для плавного следования
 7    float pidKp = 2.0;
 8    float pidKi = 0.1;
 9    float pidKd = 0.5;
10    
11    float previousError = 0;
12    float integralError = 0;
13    unsigned long lastUpdateTime = 0;
14    
15public:
16    void followWall() {
17        float wallDistance = sensor.getCalibratedDistance();
18        unsigned long currentTime = millis();
19        float deltaTime = (currentTime - lastUpdateTime) / 1000.0; // секунды
20        
21        if (deltaTime > 0) {
22            // Расчет ошибки
23            float error = TARGET_WALL_DISTANCE - wallDistance;
24            
25            // PID-регулятор
26            integralError += error * deltaTime;
27            float derivative = (error - previousError) / deltaTime;
28            
29            float pidOutput = pidKp * error + 
30                            pidKi * integralError + 
31                            pidKd * derivative;
32            
33            // Ограничение выходного сигнала
34            pidOutput = constrain(pidOutput, -100, 100);
35            
36            // Применение коррекции к движению
37            int leftSpeed = NORMAL_SPEED + pidOutput;
38            int rightSpeed = NORMAL_SPEED - pidOutput;
39            
40            // Ограничение скоростей
41            leftSpeed = constrain(leftSpeed, 0, 255);
42            rightSpeed = constrain(rightSpeed, 0, 255);
43            
44            // Управление моторами
45            setMotorSpeeds(leftSpeed, rightSpeed);
46            
47            // Обновление переменных
48            previousError = error;
49            lastUpdateTime = currentTime;
50            
51            // Отладочная информация
52            Serial.print("Ошибка: ");
53            Serial.print(error);
54            Serial.print(", PID: ");
55            Serial.print(pidOutput);
56            Serial.print(", Скорости: L=");
57            Serial.print(leftSpeed);
58            Serial.print(", R=");
59            Serial.println(rightSpeed);
60        }
61    }
62    
63    void tuneParameters(float kp, float ki, float kd) {
64        pidKp = kp;
65        pidKi = ki;
66        pidKd = kd;
67        
68        // Сброс интегральной составляющей
69        integralError = 0;
70        
71        Serial.println("PID параметры обновлены:");
72        Serial.print("Kp="); Serial.print(pidKp);
73        Serial.print(", Ki="); Serial.print(pidKi);
74        Serial.print(", Kd="); Serial.println(pidKd);
75    }
76};

 1class SimpleMapper {
 2private:
 3    struct MapPoint {
 4        float x, y;           // Координаты в см
 5        float distance;       // Расстояние до препятствия
 6        bool obstacle;        // Наличие препятствия
 7        unsigned long timestamp;
 8    };
 9    
10    MapPoint map[MAX_MAP_POINTS];
11    int mapSize = 0;
12    
13    float robotX = 0, robotY = 0;     // Позиция робота
14    float robotAngle = 0;             // Ориентация робота (радианы)
15    
16public:
17    void updateMap() {
18        float distance = sensor.getCalibratedDistance();
19        
20        if (distance > 0 && distance < MAX_MAPPING_RANGE) {
21            // Расчет координат препятствия
22            float obstacleX = robotX + distance * cos(robotAngle);
23            float obstacleY = robotY + distance * sin(robotAngle);
24            
25            // Добавление точки на карту
26            addMapPoint(obstacleX, obstacleY, distance, true);
27            
28            // Также отмечаем свободное пространство
29            markFreeSpace(robotX, robotY, obstacleX, obstacleY);
30        }
31    }
32    
33    void addMapPoint(float x, float y, float dist, bool isObstacle) {
34        if (mapSize < MAX_MAP_POINTS) {
35            map[mapSize].x = x;
36            map[mapSize].y = y;
37            map[mapSize].distance = dist;
38            map[mapSize].obstacle = isObstacle;
39            map[mapSize].timestamp = millis();
40            mapSize++;
41        }
42    }
43    
44    void printMap() {
45        Serial.println("=== КАРТА ОКРУЖЕНИЯ ===");
46        for(int i = 0; i < mapSize; i++) {
47            Serial.print("Точка ");
48            Serial.print(i);
49            Serial.print(": (");
50            Serial.print(map[i].x);
51            Serial.print(", ");
52            Serial.print(map[i].y);
53            Serial.print(") - ");
54            Serial.println(map[i].obstacle ? "Препятствие" : "Свободно");
55        }
56    }
57    
58    bool isPathClear(float targetX, float targetY) {
59        // Простая проверка на наличие препятствий на пути
60        for(int i = 0; i < mapSize; i++) {
61            if (map[i].obstacle) {
62                float distToObstacle = sqrt(pow(map[i].x - targetX, 2) + 
63                                          pow(map[i].y - targetY, 2));
64                if (distToObstacle < ROBOT_RADIUS + SAFETY_MARGIN) {
65                    return false;
66                }
67            }
68        }
69        return true;
70    }
71};

 1class AdaptiveBehavior {
 2private:
 3    struct BehaviorPattern {
 4        float situation[4];    // [distance, speed, turn_angle, success_rate]
 5        int action;           // Выбранное действие
 6        float reward;         // Награда за действие
 7        int usage_count;      // Количество использований
 8    };
 9    
10    BehaviorPattern patterns[MAX_PATTERNS];
11    int patternCount = 0;
12    
13    // Q-learning параметры
14    float learningRate = 0.1;
15    float discountFactor = 0.9;
16    float explorationRate = 0.2;
17    
18public:
19    int selectAction(float distance, float speed, float turnAngle) {
20        float situation[4] = {distance, speed, turnAngle, 0};
21        
22        // Exploration vs Exploitation
23        if (random(100) < explorationRate * 100) {
24            // Исследование: случайное действие
25            return random(NUM_ACTIONS);
26        } else {
27            // Эксплуатация: лучшее известное действие
28            return getBestAction(situation);
29        }
30    }
31    
32    void updatePattern(float* situation, int action, float reward) {
33        // Поиск соответствующего паттерна
34        int patternIndex = findOrCreatePattern(situation, action);
35        
36        if (patternIndex >= 0) {
37            // Q-learning обновление
38            float oldQ = patterns[patternIndex].reward;
39            float newQ = oldQ + learningRate * (reward - oldQ);
40            
41            patterns[patternIndex].reward = newQ;
42            patterns[patternIndex].usage_count++;
43            
44            // Адаптация exploration rate
45            explorationRate = max(0.05, explorationRate * 0.995);
46        }
47    }
48    
49private:
50    int getBestAction(float* situation) {
51        float bestReward = -1000;
52        int bestAction = 0;
53        
54        for(int i = 0; i < patternCount; i++) {
55            if (isSimilarSituation(patterns[i].situation, situation)) {
56                if (patterns[i].reward > bestReward) {
57                    bestReward = patterns[i].reward;
58                    bestAction = patterns[i].action;
59                }
60            }
61        }
62        
63        return bestAction;
64    }
65    
66    bool isSimilarSituation(float* pattern, float* current) {
67        float threshold = 0.2; // 20% допуск
68        
69        for(int i = 0; i < 3; i++) {
70            float difference = abs(pattern[i] - current[i]) / max(pattern[i], current[i]);
71            if (difference > threshold) {
72                return false;
73            }
74        }
75        return true;
76    }
77};

  1class TestingFramework {
  2private:
  3    struct TestResult {
  4        String testName;
  5        bool passed;
  6        float score;
  7        String details;
  8        unsigned long duration;
  9    };
 10    
 11    TestResult results[MAX_TESTS];
 12    int testCount = 0;
 13    
 14public:
 15    void runFullTestSuite() {
 16        Serial.println("=== КОМПЛЕКСНОЕ ТЕСТИРОВАНИЕ ===");
 17        unsigned long startTime = millis();
 18        
 19        // 1. Тесты датчика
 20        runSensorTests();
 21        
 22        // 2. Тесты калибровки
 23        runCalibrationTests();
 24        
 25        // 3. Тесты алгоритмов
 26        runAlgorithmTests();
 27        
 28        // 4. Интеграционные тесты
 29        runIntegrationTests();
 30        
 31        // 5. Тесты производительности
 32        runPerformanceTests();
 33        
 34        unsigned long totalTime = millis() - startTime;
 35        generateTestReport(totalTime);
 36    }
 37    
 38private:
 39    void runSensorTests() {
 40        Serial.println("\n--- ТЕСТИРОВАНИЕ ДАТЧИКА ---");
 41        
 42        // Тест 1: Базовая функциональность
 43        addTestResult("Базовая функциональность", 
 44                     testBasicSensorFunction());
 45        
 46        // Тест 2: Точность измерений
 47        addTestResult("Точность измерений", 
 48                     testMeasurementAccuracy());
 49        
 50        // Тест 3: Стабильность показаний
 51        addTestResult("Стабильность показаний", 
 52                     testMeasurementStability());
 53        
 54        // Тест 4: Время отклика
 55        addTestResult("Время отклика", 
 56                     testResponseTime());
 57    }
 58    
 59    bool testBasicSensorFunction() {
 60        int successCount = 0;
 61        const int ATTEMPTS = 10;
 62        
 63        for(int i = 0; i < ATTEMPTS; i++) {
 64            float distance = sensor.getCalibratedDistance();
 65            if (distance > 0 && distance < 500) {
 66                successCount++;
 67            }
 68            delay(100);
 69        }
 70        
 71        float successRate = (float)successCount / ATTEMPTS;
 72        Serial.print("Успешных измерений: ");
 73        Serial.print(successRate * 100);
 74        Serial.println("%");
 75        
 76        return successRate > 0.9; // 90% успешности
 77    }
 78    
 79    bool testMeasurementAccuracy() {
 80        Serial.println("Поместите препятствие на 30 см от датчика");
 81        delay(3000);
 82        
 83        float sum = 0;
 84        const int SAMPLES = 20;
 85        
 86        for(int i = 0; i < SAMPLES; i++) {
 87            sum += sensor.getCalibratedDistance();
 88            delay(50);
 89        }
 90        
 91        float average = sum / SAMPLES;
 92        float error = abs(average - 30.0);
 93        
 94        Serial.print("Измеренное расстояние: ");
 95        Serial.println(average);
 96        Serial.print("Ошибка: ");
 97        Serial.println(error);
 98        
 99        return error < 3.0; // Ошибка менее 3 см
100    }
101    
102    bool testMeasurementStability() {
103        float readings[50];
104        
105        for(int i = 0; i < 50; i++) {
106            readings[i] = sensor.getCalibratedDistance();
107            delay(20);
108        }
109        
110        float mean = calculateMean(readings, 50);
111        float stdDev = calculateStdDev(readings, 50, mean);
112        
113        Serial.print("Стандартное отклонение: ");
114        Serial.println(stdDev);
115        
116        return stdDev < 2.0; // Отклонение менее 2 см
117    }
118};