🔧 От чертежа к роботу

class PlatformBuilder {
private:
    struct Component {
        string name;
        float mass;          // кг
        float x, y, z;      // координаты центра масс
        vector<string> mountingPoints;
    };
    
    vector<Component> components;
    float platformWidth, platformLength;
    
public:
    void addComponent(Component comp) {
        components.push_back(comp);
    }
    
    Point3D calculateCenterOfGravity() {
        float totalMass = 0;
        float totalX = 0, totalY = 0, totalZ = 0;
        
        for (auto& comp : components) {
            totalMass += comp.mass;
            totalX += comp.mass * comp.x;
            totalY += comp.mass * comp.y;
            totalZ += comp.mass * comp.z;
        }
        
        return {totalX/totalMass, totalY/totalMass, totalZ/totalMass};
    }
    
    bool checkStability() {
        Point3D cg = calculateCenterOfGravity();
        
        // Проверка, что центр тяжести внутри опорного контура
        return (abs(cg.x) < platformLength/2) && 
               (abs(cg.y) < platformWidth/2) &&
               (cg.z < platformLength); // Высота не больше длины
    }
    
    vector<string> getStabilityRecommendations() {
        vector<string> recommendations;
        Point3D cg = calculateCenterOfGravity();
        
        if (cg.z > platformLength * 0.5) {
            recommendations.push_back("Снизить центр тяжести - переместить тяжелые компоненты вниз");
        }
        
        if (abs(cg.x) > platformLength * 0.3) {
            recommendations.push_back("Сбалансировать платформу по оси X");
        }
        
        if (abs(cg.y) > platformWidth * 0.3) {
            recommendations.push_back("Сбалансировать платформу по оси Y");
        }
        
        if (recommendations.empty()) {
            recommendations.push_back("Платформа стабильна!");
        }
        
        return recommendations;
    }
    
    void printStabilityAnalysis() {
        Point3D cg = calculateCenterOfGravity();
        bool stable = checkStability();
        
        cout << "=== АНАЛИЗ УСТОЙЧИВОСТИ ПЛАТФОРМЫ ===" << endl;
        cout << "Центр тяжести: (" << cg.x << ", " << cg.y << ", " << cg.z << ")" << endl;
        cout << "Платформа " << (stable ? "УСТОЙЧИВА" : "НЕУСТОЙЧИВА") << endl;
        
        cout << "\nРекомендации:" << endl;
        auto recommendations = getStabilityRecommendations();
        for (auto& rec : recommendations) {
            cout << "• " << rec << endl;
        }
    }
};

class DynamicStabilityAnalyzer {
private:
    struct MotionState {
        float velocity;      // м/с
        float acceleration; // м/с²
        float angularVelocity; // рад/с
        float lateralForce;  // Н
    };
    
public:
    bool analyzeDynamicStability(MotionState state, float mass, Point3D centerOfGravity) {
        // Центробежная сила при повороте
        float centrifugalForce = mass * state.velocity * state.angularVelocity;
        
        // Инерционная сила при ускорении
        float inertialForce = mass * state.acceleration;
        
        // Суммарная боковая сила
        float totalLateralForce = centrifugalForce + state.lateralForce;
        
        // Момент опрокидывания
        float tippingMoment = totalLateralForce * centerOfGravity.z;
        
        // Стабилизирующий момент
        float stabilizingMoment = mass * 9.81 * sqrt(
            centerOfGravity.x * centerOfGravity.x + 
            centerOfGravity.y * centerOfGravity.y
        );
        
        // Коэффициент запаса устойчивости
        float stabilityMargin = stabilizingMoment / tippingMoment;
        
        cout << "Анализ динамической устойчивости:" << endl;
        cout << "Центробежная сила: " << centrifugalForce << " Н" << endl;
        cout << "Инерционная сила: " << inertialForce << " Н" << endl;
        cout << "Момент опрокидывания: " << tippingMoment << " Н⋅м" << endl;
        cout << "Стабилизирующий момент: " << stabilizingMoment << " Н⋅м" << endl;
        cout << "Коэффициент запаса: " << stabilityMargin << endl;
        
        // Устойчивость обеспечена при коэффициенте > 1.5
        return stabilityMargin > 1.5;
    }
    
    float calculateMaxSafeSpeed(float mass, Point3D cg, float turnRadius) {
        // Максимальная безопасная скорость поворота
        float maxCentrifugalForce = mass * 9.81 * sqrt(cg.x*cg.x + cg.y*cg.y) / (1.5 * cg.z);
        float maxSpeed = sqrt(maxCentrifugalForce * turnRadius / mass);
        
        return maxSpeed;
    }
};

// Главный класс робота
class TransportRobot {
private:
    // Подсистемы робота
    MotionController motion;
    SensorManager sensors;
    NavigationSystem navigation;
    SafetySystem safety;
    CommunicationModule comm;
    
    // Состояние робота
    RobotState currentState;
    Mission currentMission;
    
public:
    void initialize() {
        Serial.begin(115200);
        Serial.println("=== TRANSPORT ROBOT INITIALIZATION ===");
        
        // Инициализация подсистем
        motion.initialize();
        sensors.initialize();
        navigation.initialize();
        safety.initialize();
        comm.initialize();
        
        currentState = IDLE;
        Serial.println("Robot ready for operation");
    }
    
    void update() {
        // Основной цикл работы робота
        sensors.updateAllSensors();
        safety.checkSafetyConditions();
        
        if (safety.isSafe()) {
            executeCurrentMission();
        } else {
            emergencyStop();
        }
        
        comm.sendStatusUpdate();
    }
    
private:
    void executeCurrentMission() {
        switch (currentState) {
            case IDLE:
                waitForMission();
                break;
            case MOVING:
                executeMovement();
                break;
            case LOADING:
                executeLoading();
                break;
            case UNLOADING:
                executeUnloading();
                break;
            case CHARGING:
                executeCharging();
                break;
        }
    }
};

class MotionController {
private:
    Motor leftMotor, rightMotor;
    PIDController leftPID, rightPID;
    Encoder leftEncoder, rightEncoder;
    
    struct MotionParameters {
        float maxSpeed = 1.0;        // м/с
        float maxAcceleration = 0.5; // м/с²
        float wheelBase = 0.25;      // м
        float wheelDiameter = 0.06;  // м
    } params;
    
    struct RobotPose {
        float x, y;        // позиция в метрах
        float theta;       // ориентация в радианах
        float velocity;    // скорость в м/с
        float angularVel;  // угловая скорость в рад/с
    } pose;
    
public:
    void initialize() {
        leftMotor.attach(MOTOR_LEFT_PIN);
        rightMotor.attach(MOTOR_RIGHT_PIN);
        
        leftPID.setParameters(2.0, 0.1, 0.05);  // kP, kI, kD
        rightPID.setParameters(2.0, 0.1, 0.05);
        
        leftEncoder.initialize(ENCODER_LEFT_A, ENCODER_LEFT_B);
        rightEncoder.initialize(ENCODER_RIGHT_A, ENCODER_RIGHT_B);
    }
    
    void moveToPoint(float targetX, float targetY) {
        float deltaX = targetX - pose.x;
        float deltaY = targetY - pose.y;
        float distance = sqrt(deltaX*deltaX + deltaY*deltaY);
        float targetAngle = atan2(deltaY, deltaX);
        
        // Сначала поворачиваемся к цели
        rotateToAngle(targetAngle);
        
        // Затем движемся к цели
        moveForward(distance);
    }
    
    void rotateToAngle(float targetAngle) {
        float angleDiff = normalizeAngle(targetAngle - pose.theta);
        
        if (abs(angleDiff) < 0.1) return; // Уже на нужном угле
        
        float angularSpeed = copysign(0.5, angleDiff); // 0.5 рад/с
        
        unsigned long startTime = millis();
        while (abs(angleDiff) > 0.1 && millis() - startTime < 5000) {
            updateOdometry();
            
            float leftSpeed = -angularSpeed * params.wheelBase / 2;
            float rightSpeed = angularSpeed * params.wheelBase / 2;
            
            setWheelSpeeds(leftSpeed, rightSpeed);
            
            angleDiff = normalizeAngle(targetAngle - pose.theta);
            delay(20);
        }
        
        stop();
    }
    
    void moveForward(float distance) {
        float startX = pose.x;
        float startY = pose.y;
        float targetSpeed = min(params.maxSpeed, distance * 0.5); // Адаптивная скорость
        
        while (getDistanceTraveled(startX, startY) < distance) {
            updateOdometry();
            
            float remainingDistance = distance - getDistanceTraveled(startX, startY);
            float currentSpeed = min(targetSpeed, remainingDistance * 0.5);
            
            setWheelSpeeds(currentSpeed, currentSpeed);
            delay(20);
        }
        
        stop();
    }
    
private:
    void updateOdometry() {
        static unsigned long lastTime = 0;
        unsigned long currentTime = millis();
        float deltaTime = (currentTime - lastTime) / 1000.0;
        
        if (deltaTime < 0.01) return; // Слишком частые вызовы
        
        long leftTicks = leftEncoder.read();
        long rightTicks = rightEncoder.read();
        
        // Расчет пройденного расстояния
        float leftDistance = leftTicks * (PI * params.wheelDiameter) / ENCODER_COUNTS_PER_REV;
        float rightDistance = rightTicks * (PI * params.wheelDiameter) / ENCODER_COUNTS_PER_REV;
        
        float distance = (leftDistance + rightDistance) / 2.0;
        float deltaTheta = (rightDistance - leftDistance) / params.wheelBase;
        
        // Обновление позиции
        pose.x += distance * cos(pose.theta + deltaTheta/2);
        pose.y += distance * sin(pose.theta + deltaTheta/2);
        pose.theta = normalizeAngle(pose.theta + deltaTheta);
        
        // Обновление скоростей
        pose.velocity = distance / deltaTime;
        pose.angularVel = deltaTheta / deltaTime;
        
        leftEncoder.reset();
        rightEncoder.reset();
        lastTime = currentTime;
    }
    
    void setWheelSpeeds(float leftSpeed, float rightSpeed) {
        // Преобразование м/с в PWM
        int leftPWM = (leftSpeed / params.maxSpeed) * 255;
        int rightPWM = (rightSpeed / params.maxSpeed) * 255;
        
        leftPWM = constrain(leftPWM, -255, 255);
        rightPWM = constrain(rightPWM, -255, 255);
        
        leftMotor.setSpeed(leftPWM);
        rightMotor.setSpeed(rightPWM);
    }
};

class DecisionMaker {
private:
    enum Priority {
        CRITICAL = 1,
        HIGH = 2,
        MEDIUM = 3,
        LOW = 4
    };
    
    struct Decision {
        string action;
        Priority priority;
        vector<string> conditions;
        function<void()> executor;
    };
    
    vector<Decision> decisionTree;
    
public:
    void initialize() {
        // Критические решения (безопасность)
        addDecision("EMERGENCY_STOP", CRITICAL, 
                   {"obstacle_too_close", "battery_critical", "system_error"},
                   [this]() { executeEmergencyStop(); });
        
        // Высокоприоритетные решения (основные задачи)
        addDecision("AVOID_OBSTACLE", HIGH,
                   {"obstacle_detected", "path_blocked"},
                   [this]() { executeObstacleAvoidance(); });
        
        addDecision("CONTINUE_MISSION", HIGH,
                   {"path_clear", "battery_ok", "systems_normal"},
                   [this]() { executeMissionContinuation(); });
        
        // Среднеприоритетные решения (оптимизация)
        addDecision("OPTIMIZE_PATH", MEDIUM,
                   {"new_path_found", "current_path_inefficient"},
                   [this]() { executePathOptimization(); });
        
        // Низкоприоритетные решения (комфорт)
        addDecision("UPDATE_STATUS", LOW,
                   {"status_update_due"},
                   [this]() { executeStatusUpdate(); });
    }
    
    void processDecisions(map<string, bool> currentConditions) {
        // Сортируем решения по приоритету
        sort(decisionTree.begin(), decisionTree.end(),
             [](const Decision& a, const Decision& b) {
                 return a.priority < b.priority;
             });
        
        // Проверяем условия и выполняем первое подходящее решение
        for (auto& decision : decisionTree) {
            if (checkConditions(decision.conditions, currentConditions)) {
                Serial.print("Executing decision: ");
                Serial.println(decision.action.c_str());
                decision.executor();
                break; // Выполняем только одно решение за цикл
            }
        }
    }
    
private:
    bool checkConditions(vector<string> required, map<string, bool> current) {
        for (auto& condition : required) {
            if (current.count(condition) && current[condition]) {
                return true; // Любое условие достаточно
            }
        }
        return false;
    }
    
    void executeEmergencyStop() {
        motion.stop();
        safety.activateEmergencyProtocol();
        Serial.println("EMERGENCY STOP ACTIVATED");
    }
    
    void executeObstacleAvoidance() {
        auto obstacle = sensors.getNearestObstacle();
        auto newPath = navigation.planAvoidancePath(obstacle);
        motion.followPath(newPath);
    }
};

class LearningSystem {
private:
    struct Experience {
        vector<float> sensorInputs;
        string action;
        float reward;
        vector<float> nextSensorInputs;
    };
    
    vector<Experience> experienceReplay;
    NeuralNetwork policyNetwork;
    
public:
    void initialize() {
        // Инициализация нейронной сети
        policyNetwork.addLayer(SENSOR_COUNT, ACTIVATION_RELU);
        policyNetwork.addLayer(64, ACTIVATION_RELU);
        policyNetwork.addLayer(32, ACTIVATION_RELU);
        policyNetwork.addLayer(ACTION_COUNT, ACTIVATION_SOFTMAX);
        
        policyNetwork.compile(OPTIMIZER_ADAM, LOSS_CATEGORICAL_CROSSENTROPY);
    }
    
    string selectAction(vector<float> sensorInputs) {
        vector<float> actionProbabilities = policyNetwork.predict(sensorInputs);
        
        // Epsilon-greedy стратегия
        float epsilon = 0.1;
        if (random(0, 100) < epsilon * 100) {
            // Случайное действие для исследования
            return getRandomAction();
        } else {
            // Лучшее действие согласно сети
            int bestActionIndex = max_element(actionProbabilities.begin(), 
                                            actionProbabilities.end()) - actionProbabilities.begin();
            return getActionByIndex(bestActionIndex);
        }
    }
    
    void recordExperience(vector<float> state, string action, float reward, vector<float> nextState) {
        Experience exp = {state, action, reward, nextState};
        experienceReplay.push_back(exp);
        
        // Ограничиваем размер буфера опыта
        if (experienceReplay.size() > 10000) {
            experienceReplay.erase(experienceReplay.begin());
        }
    }
    
    void trainNetwork() {
        if (experienceReplay.size() < 100) return; // Недостаточно опыта
        
        // Выбираем случайную мини-партию для обучения
        vector<Experience> batch = sampleRandomBatch(32);
        
        vector<vector<float>> inputs, targets;
        
        for (auto& exp : batch) {
            inputs.push_back(exp.sensorInputs);
            
            // Расчет целевого Q-значения
            vector<float> qValues = policyNetwork.predict(exp.sensorInputs);
            vector<float> nextQValues = policyNetwork.predict(exp.nextSensorInputs);
            
            float maxNextQ = *max_element(nextQValues.begin(), nextQValues.end());
            float targetQ = exp.reward + 0.95 * maxNextQ; // gamma = 0.95
            
            int actionIndex = getActionIndex(exp.action);
            qValues[actionIndex] = targetQ;
            
            targets.push_back(qValues);
        }
        
        // Обучаем сеть
        policyNetwork.fit(inputs, targets, 1);
    }
    
    void saveModel(string filename) {
        policyNetwork.saveToFile(filename);
        Serial.print("Model saved to: ");
        Serial.println(filename.c_str());
    }
    
    void loadModel(string filename) {
        policyNetwork.loadFromFile(filename);
        Serial.print("Model loaded from: ");
        Serial.println(filename.c_str());
    }
};