🔋 Лаборатория энергооптимизации

[Аккумулятор 7.4V] → [Датчик тока INA219] → [Контроллер] → [Моторы]
                             ↓
                    [Компьютер с Arduino IDE]
                             ↓
                    [График в реальном времени]

#include <Wire.h>
#include <Adafruit_INA219.h>

class EnergyMeter {
private:
    Adafruit_INA219 ina219;
    float totalEnergy = 0;      // Общая потребленная энергия (Дж)
    unsigned long lastTime = 0;
    
public:
    void initialize() {
        ina219.begin();
        Serial.begin(9600);
        Serial.println("=== ENERGY OPTIMIZATION LAB ===");
        Serial.println("Time(s)\tCurrent(mA)\tPower(mW)\tEnergy(J)");
        lastTime = millis();
    }
    
    void measureAndLog() {
        unsigned long currentTime = millis();
        float deltaTime = (currentTime - lastTime) / 1000.0; // секунды
        
        float voltage = ina219.getBusVoltage_V();
        float current = ina219.getCurrent_mA();
        float power = voltage * current; // мВт
        
        // Накапливаем энергию
        totalEnergy += (power / 1000.0) * deltaTime; // Джоули
        
        // Логирование данных
        Serial.print((currentTime - startTime) / 1000.0, 2);
        Serial.print("\t");
        Serial.print(current, 1);
        Serial.print("\t");
        Serial.print(power, 1);
        Serial.print("\t");
        Serial.println(totalEnergy, 3);
        
        lastTime = currentTime;
    }
    
    float getCurrentConsumption() {
        return ina219.getCurrent_mA();
    }
    
    float getTotalEnergy() {
        return totalEnergy;
    }
    
    void resetMeasurement() {
        totalEnergy = 0;
        lastTime = millis();
    }
};

EnergyMeter energyMeter;

class AdvancedEnergyAnalyzer {
private:
    vector<float> currentHistory;
    vector<float> powerHistory;
    int windowSize = 50; // Окно для анализа
    
public:
    void addMeasurement(float current, float power) {
        currentHistory.push_back(current);
        powerHistory.push_back(power);
        
        // Ограничиваем размер истории
        if (currentHistory.size() > windowSize) {
            currentHistory.erase(currentHistory.begin());
            powerHistory.erase(powerHistory.begin());
        }
    }
    
    float calculateEfficiency() {
        if (powerHistory.size() < 2) return 0;
        
        float avgPower = 0;
        float minPower = powerHistory[0];
        float maxPower = powerHistory[0];
        
        for (float power : powerHistory) {
            avgPower += power;
            minPower = min(minPower, power);
            maxPower = max(maxPower, power);
        }
        avgPower /= powerHistory.size();
        
        // Эффективность как обратная величина от разброса
        float efficiency = avgPower / (maxPower - minPower + 1);
        return efficiency;
    }
    
    bool detectPowerSpike() {
        if (currentHistory.size() < 5) return false;
        
        float recent = currentHistory.back();
        float average = 0;
        
        for (int i = currentHistory.size() - 5; i < currentHistory.size() - 1; i++) {
            average += currentHistory[i];
        }
        average /= 4;
        
        // Скачок тока более чем в 2 раза
        return recent > average * 2.0;
    }
    
    void generateReport() {
        Serial.println("=== ADVANCED ENERGY ANALYSIS ===");
        Serial.print("Efficiency score: ");
        Serial.println(calculateEfficiency(), 2);
        
        if (detectPowerSpike()) {
            Serial.println("WARNING: Power spike detected!");
        }
        
        Serial.print("Average power consumption: ");
        float avgPower = 0;
        for (float power : powerHistory) avgPower += power;
        Serial.print(avgPower / powerHistory.size(), 2);
        Serial.println(" mW");
    }
};

class AutomatedTestSuite {
private:
    EnergyMeter meter;
    int testDuration = 30000; // 30 секунд на тест
    
public:
    void runSpeedDependencyTest() {
        Serial.println("=== SPEED DEPENDENCY TEST ===");
        
        int speeds[] = {0, 50, 100, 150, 200, 255};
        
        for (int speed : speeds) {
            Serial.print("Testing speed: ");
            Serial.println(speed);
            
            meter.resetMeasurement();
            robot.setSpeed(speed);
            
            unsigned long startTime = millis();
            float totalCurrent = 0;
            int measurements = 0;
            
            while (millis() - startTime < testDuration) {
                float current = meter.getCurrentConsumption();
                totalCurrent += current;
                measurements++;
                
                meter.measureAndLog();
                delay(100);
            }
            
            robot.stop();
            
            float avgCurrent = totalCurrent / measurements;
            float estimatedRuntime = (BATTERY_CAPACITY * 1000) / avgCurrent; // минуты
            
            Serial.print("Speed: ");
            Serial.print(speed);
            Serial.print(" | Avg Current: ");
            Serial.print(avgCurrent, 1);
            Serial.print(" mA | Runtime: ");
            Serial.print(estimatedRuntime, 1);
            Serial.println(" min");
            
            delay(5000); // Пауза между тестами
        }
    }
    
    void runAccelerationTest() {
        Serial.println("=== ACCELERATION TEST ===");
        
        float accelerations[] = {0.5, 1.0, 2.0, 5.0, 10.0}; // м/с²
        
        for (float accel : accelerations) {
            Serial.print("Testing acceleration: ");
            Serial.print(accel);
            Serial.println(" m/s²");
            
            meter.resetMeasurement();
            robot.smoothAccelerate(150, accel);
            
            delay(3000); // Наблюдаем процесс разгона
            
            robot.stop();
            
            Serial.print("Acceleration energy: ");
            Serial.print(meter.getTotalEnergy(), 3);
            Serial.println(" J");
            
            delay(2000);
        }
    }
};

Ток потребления (мА)
    ↑
2000│                    ●
    │                ●
1500│            ●
    │        ●
1000│    ●
    │●
 500│
    │
   0└─────────────────────► Скорость PWM
    0   50  100 150 200 255

Время работы (мин)
    ↑
 120│●
    │ ●
  90│  ●
    │   ●
  60│    ●
    │     ●
  30│      ●
    │       ●
   0└─────────────────────► Скорость PWM
    0   50  100 150 200 255

class OptimizationAnalyzer {
private:
    vector<pair<int, float>> speedCurrentData;
    
public:
    void addDataPoint(int speed, float current) {
        speedCurrentData.push_back(make_pair(speed, current));
    }
    
    int findOptimalSpeed() {
        if (speedCurrentData.size() < 3) return 100; // Значение по умолчанию
        
        float bestEfficiency = 0;
        int optimalSpeed = 100;
        
        for (auto& point : speedCurrentData) {
            int speed = point.first;
            float current = point.second;
            
            if (speed == 0) continue; // Пропускаем состояние покоя
            
            // Эффективность = скорость / ток
            float efficiency = (float)speed / current;
            
            if (efficiency > bestEfficiency) {
                bestEfficiency = efficiency;
                optimalSpeed = speed;
            }
        }
        
        Serial.print("Optimal speed found: ");
        Serial.print(optimalSpeed);
        Serial.print(" (efficiency: ");
        Serial.print(bestEfficiency, 3);
        Serial.println(")");
        
        return optimalSpeed;
    }
    
    float calculatePowerSavings(int originalSpeed, int optimizedSpeed) {
        float originalCurrent = getCurrentForSpeed(originalSpeed);
        float optimizedCurrent = getCurrentForSpeed(optimizedSpeed);
        
        float savings = (originalCurrent - optimizedCurrent) / originalCurrent * 100;
        return max(0.0f, savings);
    }
    
private:
    float getCurrentForSpeed(int speed) {
        // Интерполяция для нахождения тока при заданной скорости
        for (int i = 0; i < speedCurrentData.size() - 1; i++) {
            if (speed >= speedCurrentData[i].first && speed <= speedCurrentData[i+1].first) {
                float ratio = (float)(speed - speedCurrentData[i].first) / 
                             (speedCurrentData[i+1].first - speedCurrentData[i].first);
                return speedCurrentData[i].second + 
                       ratio * (speedCurrentData[i+1].second - speedCurrentData[i].second);
            }
        }
        return speedCurrentData.back().second; // Последнее значение
    }
};

class MLOptimizer {
private:
    struct TrainingData {
        float speed;
        float acceleration;
        float load;
        float current;
    };
    
    vector<TrainingData> trainingSet;
    
public:
    void addTrainingExample(float speed, float accel, float load, float current) {
        trainingSet.push_back({speed, accel, load, current});
    }
    
    float predictCurrent(float speed, float accel, float load) {
        if (trainingSet.empty()) return 1000; // Значение по умолчанию
        
        // Простая k-nearest neighbors
        vector<pair<float, float>> distances;
        
        for (auto& data : trainingSet) {
            float distance = sqrt(
                pow(data.speed - speed, 2) + 
                pow(data.acceleration - accel, 2) + 
                pow(data.load - load, 2)
            );
            distances.push_back(make_pair(distance, data.current));
        }
        
        // Сортируем по расстоянию
        sort(distances.begin(), distances.end());
        
        // Берем среднее от 3 ближайших соседей
        float avgCurrent = 0;
        int k = min(3, (int)distances.size());
        
        for (int i = 0; i < k; i++) {
            avgCurrent += distances[i].second;
        }
        
        return avgCurrent / k;
    }
    
    vector<float> optimizeParameters(float targetRuntime) {
        float bestSpeed = 100;
        float bestAccel = 1.0;
        float bestRuntime = 0;
        
        // Перебираем возможные комбинации
        for (int speed = 50; speed <= 200; speed += 10) {
            for (float accel = 0.5; accel <= 3.0; accel += 0.5) {
                float predictedCurrent = predictCurrent(speed, accel, 1.0);
                float predictedRuntime = (BATTERY_CAPACITY * 1000) / predictedCurrent;
                
                if (predictedRuntime > bestRuntime && predictedRuntime >= targetRuntime) {
                    bestRuntime = predictedRuntime;
                    bestSpeed = speed;
                    bestAccel = accel;
                }
            }
        }
        
        return {bestSpeed, bestAccel, bestRuntime};
    }
};

class SmoothMotionController {
private:
    float currentSpeed = 0;
    float targetSpeed = 0;
    float acceleration = 1.0; // м/с² (настраиваемый параметр)
    
public:
    void setTargetSpeed(float target, float accel = 1.0) {
        targetSpeed = target;
        acceleration = accel;
    }
    
    void update(float deltaTime) {
        if (abs(targetSpeed - currentSpeed) < 0.1) {
            currentSpeed = targetSpeed; // Достигли цели
            return;
        }
        
        float speedChange = acceleration * deltaTime;
        
        if (targetSpeed > currentSpeed) {
            currentSpeed = min(targetSpeed, currentSpeed + speedChange);
        } else {
            currentSpeed = max(targetSpeed, currentSpeed - speedChange);
        }
        
        // Применяем скорость к моторам
        robot.setSpeed((int)currentSpeed);
    }
    
    bool isAtTargetSpeed() {
        return abs(targetSpeed - currentSpeed) < 0.1;
    }
    
    float getCurrentSpeed() {
        return currentSpeed;
    }
};

class AdaptivePowerManager {
private:
    bool lowPowerMode = false;
    float batteryLevel = 1.0; // 0-1
    unsigned long lastActivity = 0;
    
public:
    void updateBatteryLevel(float voltage) {
        // Оценка уровня заряда по напряжению
        batteryLevel = map(voltage, 6.0, 8.4, 0.0, 1.0);
        batteryLevel = constrain(batteryLevel, 0.0, 1.0);
    }
    
    void checkPowerMode() {
        if (batteryLevel < 0.3) {
            enableLowPowerMode();
        } else if (batteryLevel > 0.7) {
            disableLowPowerMode();
        }
        
        // Автоматический переход в режим сна
        if (millis() - lastActivity > 30000) { // 30 секунд неактивности
            enterSleepMode();
        }
    }
    
    float getSpeedMultiplier() {
        if (lowPowerMode) {
            return 0.7; // Снижаем максимальную скорость на 30%
        }
        return 1.0;
    }
    
    int getOptimalSpeed(int requestedSpeed) {
        float multiplier = getSpeedMultiplier();
        return (int)(requestedSpeed * multiplier);
    }
    
private:
    void enableLowPowerMode() {
        if (!lowPowerMode) {
            lowPowerMode = true;
            Serial.println("LOW POWER MODE ACTIVATED");
        }
    }
    
    void disableLowPowerMode() {
        if (lowPowerMode) {
            lowPowerMode = false;
            Serial.println("NORMAL POWER MODE RESTORED");
        }
    }
    
    void enterSleepMode() {
        Serial.println("Entering sleep mode...");
        // Отключаем все несущественные системы
        robot.stop();
        // В реальности здесь был бы переход в режим сна микроконтроллера
    }
};

class EnergyEfficientPathPlanner {
private:
    struct Waypoint {
        float x, y;
        float preferredSpeed;
        float energyCost;
    };
    
    vector<Waypoint> waypoints;
    
public:
    void addWaypoint(float x, float y, float speed = 100) {
        Waypoint wp = {x, y, speed, 0};
        wp.energyCost = calculateEnergyCost(wp);
        waypoints.push_back(wp);
    }
    
    vector<Waypoint> optimizePath() {
        if (waypoints.size() < 2) return waypoints;
        
        // Оптимизируем скорости для минимизации общего энергопотребления
        for (int i = 0; i < waypoints.size(); i++) {
            if (i == 0 || i == waypoints.size() - 1) {
                // Первая и последняя точки - плавный старт/стоп
                waypoints[i].preferredSpeed = 80;
            } else {
                // Промежуточные точки - оптимальная скорость
                waypoints[i].preferredSpeed = findOptimalSpeedForSegment(i);
            }
        }
        
        return waypoints;
    }
    
    void executeOptimizedPath() {
        SmoothMotionController motion;
        
        for (auto& wp : waypoints) {
            Serial.print("Moving to waypoint (");
            Serial.print(wp.x); Serial.print(", ");
            Serial.print(wp.y); Serial.print(") at speed ");
            Serial.println(wp.preferredSpeed);
            
            // Плавный переход к новой скорости
            motion.setTargetSpeed(wp.preferredSpeed, 0.5); // Мягкое ускорение
            
            // Движение к точке
            moveToPosition(wp.x, wp.y, motion);
            
            Serial.print("Waypoint reached. Energy cost: ");
            Serial.print(wp.energyCost, 2);
            Serial.println(" J");
        }
    }
    
private:
    float calculateEnergyCost(Waypoint& wp) {
        // Упрощенная модель энергозатрат
        float distance = sqrt(wp.x * wp.x + wp.y * wp.y);
        float energyPerMeter = 0.1 + (wp.preferredSpeed / 1000.0); // Дж/м
        return distance * energyPerMeter;
    }
    
    float findOptimalSpeedForSegment(int index) {
        // Анализируем условия сегмента
        float distance = calculateSegmentDistance(index);
        
        if (distance < 50) {
            return 60;  // Короткий сегмент - низкая скорость
        } else if (distance > 200) {
            return 120; // Длинный сегмент - умеренная скорость
        } else {
            return 90;  // Средний сегмент - средняя скорость
        }
    }
    
    void moveToPosition(float x, float y, SmoothMotionController& motion) {
        // Упрощенная навигация
        float targetAngle = atan2(y, x) * 180 / PI;
        float distance = sqrt(x*x + y*y);
        
        // Поворот к цели
        robot.turnToAngle(targetAngle);
        delay(1000);
        
        // Движение к цели с контролем скорости
        unsigned long startTime = millis();
        while (millis() - startTime < distance * 50) { // 50 мс на см
            motion.update(0.1); // 100 мс интервал
            delay(100);
        }
        
        motion.setTargetSpeed(0, 2.0); // Плавная остановка
        while (!motion.isAtTargetSpeed()) {
            motion.update(0.1);
            delay(100);
        }
    }
};

class PredictiveEnergyOptimizer {
private:
    struct SystemState {
        float batteryVoltage;
        float ambientTemp;
        float robotLoad;
        float terrainDifficulty;
        unsigned long timestamp;
    };
    
    vector<SystemState> stateHistory;
    
public:
    void recordSystemState() {
        SystemState state;
        state.batteryVoltage = energyMeter.getVoltage();
        state.ambientTemp = getAmbientTemperature();
        state.robotLoad = getCurrentLoad();
        state.terrainDifficulty = assessTerrain();
        state.timestamp = millis();
        
        stateHistory.push_back(state);
        
        // Ограничиваем историю последними 100 записями
        if (stateHistory.size() > 100) {
            stateHistory.erase(stateHistory.begin());
        }
    }
    
    float predictRemainingRuntime() {
        if (stateHistory.size() < 10) return 60; // Значение по умолчанию
        
        // Анализируем тренд разряда батареи
        float avgVoltageDecline = calculateVoltageDeclineRate();
        float currentVoltage = stateHistory.back().batteryVoltage;
        float cutoffVoltage = 6.0; // Минимальное напряжение
        
        float remainingTime = (currentVoltage - cutoffVoltage) / avgVoltageDecline;
        return max(0.0f, remainingTime / 60.0f); // Конвертируем в минуты
    }
    
    void adaptiveSpeedOptimization(float targetRuntime) {
        float predictedRuntime = predictRemainingRuntime();
        
        if (predictedRuntime < targetRuntime * 0.8) {
            // Критически мало времени - переходим в экономный режим
            Serial.println("CRITICAL BATTERY: Entering ultra-eco mode");
            maxAllowedSpeed = 50;
            accelerationLimit = 0.3;
        } else if (predictedRuntime < targetRuntime) {
            // Мало времени - умеренная экономия
            Serial.println("LOW BATTERY: Entering eco mode");
            maxAllowedSpeed = 80;
            accelerationLimit = 0.5;
        } else {
            // Достаточно времени - нормальный режим
            maxAllowedSpeed = 150;
            accelerationLimit = 1.0;
        }
    }
    
private:
    float maxAllowedSpeed = 150;
    float accelerationLimit = 1.0;
    
    float calculateVoltageDeclineRate() {
        if (stateHistory.size() < 2) return 0.01; // По умолчанию
        
        float totalDecline = 0;
        int samples = 0;
        
        for (int i = 1; i < stateHistory.size(); i++) {
            float timeDiff = (stateHistory[i].timestamp - stateHistory[i-1].timestamp) / 1000.0;
            float voltageDiff = stateHistory[i-1].batteryVoltage - stateHistory[i].batteryVoltage;
            
            if (timeDiff > 0) {
                totalDecline += voltageDiff / timeDiff; // В/с
                samples++;
            }
        }
        
        return samples > 0 ? totalDecline / samples : 0.01;
    }
    
    float getCurrentLoad() {
        // Оценка нагрузки на основе потребления тока
        float current = energyMeter.getCurrentConsumption();
        return map(current, 100, 2000, 0.0, 1.0); // Нормализация 0-1
    }
    
    float assessTerrain() {
        // Упрощенная оценка сложности местности
        float currentEffort = energyMeter.getCurrentConsumption();
        float baselineEffort = 500; // мА на ровной поверхности
        
        return currentEffort / baselineEffort;
    }
};