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

 1class PlatformBuilder {
 2private:
 3    struct Component {
 4        string name;
 5        float mass;          // кг
 6        float x, y, z;      // координаты центра масс
 7        vector<string> mountingPoints;
 8    };
 9    
10    vector<Component> components;
11    float platformWidth, platformLength;
12    
13public:
14    void addComponent(Component comp) {
15        components.push_back(comp);
16    }
17    
18    Point3D calculateCenterOfGravity() {
19        float totalMass = 0;
20        float totalX = 0, totalY = 0, totalZ = 0;
21        
22        for (auto& comp : components) {
23            totalMass += comp.mass;
24            totalX += comp.mass * comp.x;
25            totalY += comp.mass * comp.y;
26            totalZ += comp.mass * comp.z;
27        }
28        
29        return {totalX/totalMass, totalY/totalMass, totalZ/totalMass};
30    }
31    
32    bool checkStability() {
33        Point3D cg = calculateCenterOfGravity();
34        
35        // Проверка, что центр тяжести внутри опорного контура
36        return (abs(cg.x) < platformLength/2) && 
37               (abs(cg.y) < platformWidth/2) &&
38               (cg.z < platformLength); // Высота не больше длины
39    }
40    
41    vector<string> getStabilityRecommendations() {
42        vector<string> recommendations;
43        Point3D cg = calculateCenterOfGravity();
44        
45        if (cg.z > platformLength * 0.5) {
46            recommendations.push_back("Снизить центр тяжести - переместить тяжелые компоненты вниз");
47        }
48        
49        if (abs(cg.x) > platformLength * 0.3) {
50            recommendations.push_back("Сбалансировать платформу по оси X");
51        }
52        
53        if (abs(cg.y) > platformWidth * 0.3) {
54            recommendations.push_back("Сбалансировать платформу по оси Y");
55        }
56        
57        if (recommendations.empty()) {
58            recommendations.push_back("Платформа стабильна!");
59        }
60        
61        return recommendations;
62    }
63    
64    void printStabilityAnalysis() {
65        Point3D cg = calculateCenterOfGravity();
66        bool stable = checkStability();
67        
68        cout << "=== АНАЛИЗ УСТОЙЧИВОСТИ ПЛАТФОРМЫ ===" << endl;
69        cout << "Центр тяжести: (" << cg.x << ", " << cg.y << ", " << cg.z << ")" << endl;
70        cout << "Платформа " << (stable ? "УСТОЙЧИВА" : "НЕУСТОЙЧИВА") << endl;
71        
72        cout << "\nРекомендации:" << endl;
73        auto recommendations = getStabilityRecommendations();
74        for (auto& rec : recommendations) {
75            cout << "• " << rec << endl;
76        }
77    }
78};

 1class DynamicStabilityAnalyzer {
 2private:
 3    struct MotionState {
 4        float velocity;      // м/с
 5        float acceleration; // м/с²
 6        float angularVelocity; // рад/с
 7        float lateralForce;  // Н
 8    };
 9    
10public:
11    bool analyzeDynamicStability(MotionState state, float mass, Point3D centerOfGravity) {
12        // Центробежная сила при повороте
13        float centrifugalForce = mass * state.velocity * state.angularVelocity;
14        
15        // Инерционная сила при ускорении
16        float inertialForce = mass * state.acceleration;
17        
18        // Суммарная боковая сила
19        float totalLateralForce = centrifugalForce + state.lateralForce;
20        
21        // Момент опрокидывания
22        float tippingMoment = totalLateralForce * centerOfGravity.z;
23        
24        // Стабилизирующий момент
25        float stabilizingMoment = mass * 9.81 * sqrt(
26            centerOfGravity.x * centerOfGravity.x + 
27            centerOfGravity.y * centerOfGravity.y
28        );
29        
30        // Коэффициент запаса устойчивости
31        float stabilityMargin = stabilizingMoment / tippingMoment;
32        
33        cout << "Анализ динамической устойчивости:" << endl;
34        cout << "Центробежная сила: " << centrifugalForce << " Н" << endl;
35        cout << "Инерционная сила: " << inertialForce << " Н" << endl;
36        cout << "Момент опрокидывания: " << tippingMoment << " Н⋅м" << endl;
37        cout << "Стабилизирующий момент: " << stabilizingMoment << " Н⋅м" << endl;
38        cout << "Коэффициент запаса: " << stabilityMargin << endl;
39        
40        // Устойчивость обеспечена при коэффициенте > 1.5
41        return stabilityMargin > 1.5;
42    }
43    
44    float calculateMaxSafeSpeed(float mass, Point3D cg, float turnRadius) {
45        // Максимальная безопасная скорость поворота
46        float maxCentrifugalForce = mass * 9.81 * sqrt(cg.x*cg.x + cg.y*cg.y) / (1.5 * cg.z);
47        float maxSpeed = sqrt(maxCentrifugalForce * turnRadius / mass);
48        
49        return maxSpeed;
50    }
51};

 1// Главный класс робота
 2class TransportRobot {
 3private:
 4    // Подсистемы робота
 5    MotionController motion;
 6    SensorManager sensors;
 7    NavigationSystem navigation;
 8    SafetySystem safety;
 9    CommunicationModule comm;
10    
11    // Состояние робота
12    RobotState currentState;
13    Mission currentMission;
14    
15public:
16    void initialize() {
17        Serial.begin(115200);
18        Serial.println("=== TRANSPORT ROBOT INITIALIZATION ===");
19        
20        // Инициализация подсистем
21        motion.initialize();
22        sensors.initialize();
23        navigation.initialize();
24        safety.initialize();
25        comm.initialize();
26        
27        currentState = IDLE;
28        Serial.println("Robot ready for operation");
29    }
30    
31    void update() {
32        // Основной цикл работы робота
33        sensors.updateAllSensors();
34        safety.checkSafetyConditions();
35        
36        if (safety.isSafe()) {
37            executeCurrentMission();
38        } else {
39            emergencyStop();
40        }
41        
42        comm.sendStatusUpdate();
43    }
44    
45private:
46    void executeCurrentMission() {
47        switch (currentState) {
48            case IDLE:
49                waitForMission();
50                break;
51            case MOVING:
52                executeMovement();
53                break;
54            case LOADING:
55                executeLoading();
56                break;
57            case UNLOADING:
58                executeUnloading();
59                break;
60            case CHARGING:
61                executeCharging();
62                break;
63        }
64    }
65};

  1class MotionController {
  2private:
  3    Motor leftMotor, rightMotor;
  4    PIDController leftPID, rightPID;
  5    Encoder leftEncoder, rightEncoder;
  6    
  7    struct MotionParameters {
  8        float maxSpeed = 1.0;        // м/с
  9        float maxAcceleration = 0.5; // м/с²
 10        float wheelBase = 0.25;      // м
 11        float wheelDiameter = 0.06;  // м
 12    } params;
 13    
 14    struct RobotPose {
 15        float x, y;        // позиция в метрах
 16        float theta;       // ориентация в радианах
 17        float velocity;    // скорость в м/с
 18        float angularVel;  // угловая скорость в рад/с
 19    } pose;
 20    
 21public:
 22    void initialize() {
 23        leftMotor.attach(MOTOR_LEFT_PIN);
 24        rightMotor.attach(MOTOR_RIGHT_PIN);
 25        
 26        leftPID.setParameters(2.0, 0.1, 0.05);  // kP, kI, kD
 27        rightPID.setParameters(2.0, 0.1, 0.05);
 28        
 29        leftEncoder.initialize(ENCODER_LEFT_A, ENCODER_LEFT_B);
 30        rightEncoder.initialize(ENCODER_RIGHT_A, ENCODER_RIGHT_B);
 31    }
 32    
 33    void moveToPoint(float targetX, float targetY) {
 34        float deltaX = targetX - pose.x;
 35        float deltaY = targetY - pose.y;
 36        float distance = sqrt(deltaX*deltaX + deltaY*deltaY);
 37        float targetAngle = atan2(deltaY, deltaX);
 38        
 39        // Сначала поворачиваемся к цели
 40        rotateToAngle(targetAngle);
 41        
 42        // Затем движемся к цели
 43        moveForward(distance);
 44    }
 45    
 46    void rotateToAngle(float targetAngle) {
 47        float angleDiff = normalizeAngle(targetAngle - pose.theta);
 48        
 49        if (abs(angleDiff) < 0.1) return; // Уже на нужном угле
 50        
 51        float angularSpeed = copysign(0.5, angleDiff); // 0.5 рад/с
 52        
 53        unsigned long startTime = millis();
 54        while (abs(angleDiff) > 0.1 && millis() - startTime < 5000) {
 55            updateOdometry();
 56            
 57            float leftSpeed = -angularSpeed * params.wheelBase / 2;
 58            float rightSpeed = angularSpeed * params.wheelBase / 2;
 59            
 60            setWheelSpeeds(leftSpeed, rightSpeed);
 61            
 62            angleDiff = normalizeAngle(targetAngle - pose.theta);
 63            delay(20);
 64        }
 65        
 66        stop();
 67    }
 68    
 69    void moveForward(float distance) {
 70        float startX = pose.x;
 71        float startY = pose.y;
 72        float targetSpeed = min(params.maxSpeed, distance * 0.5); // Адаптивная скорость
 73        
 74        while (getDistanceTraveled(startX, startY) < distance) {
 75            updateOdometry();
 76            
 77            float remainingDistance = distance - getDistanceTraveled(startX, startY);
 78            float currentSpeed = min(targetSpeed, remainingDistance * 0.5);
 79            
 80            setWheelSpeeds(currentSpeed, currentSpeed);
 81            delay(20);
 82        }
 83        
 84        stop();
 85    }
 86    
 87private:
 88    void updateOdometry() {
 89        static unsigned long lastTime = 0;
 90        unsigned long currentTime = millis();
 91        float deltaTime = (currentTime - lastTime) / 1000.0;
 92        
 93        if (deltaTime < 0.01) return; // Слишком частые вызовы
 94        
 95        long leftTicks = leftEncoder.read();
 96        long rightTicks = rightEncoder.read();
 97        
 98        // Расчет пройденного расстояния
 99        float leftDistance = leftTicks * (PI * params.wheelDiameter) / ENCODER_COUNTS_PER_REV;
100        float rightDistance = rightTicks * (PI * params.wheelDiameter) / ENCODER_COUNTS_PER_REV;
101        
102        float distance = (leftDistance + rightDistance) / 2.0;
103        float deltaTheta = (rightDistance - leftDistance) / params.wheelBase;
104        
105        // Обновление позиции
106        pose.x += distance * cos(pose.theta + deltaTheta/2);
107        pose.y += distance * sin(pose.theta + deltaTheta/2);
108        pose.theta = normalizeAngle(pose.theta + deltaTheta);
109        
110        // Обновление скоростей
111        pose.velocity = distance / deltaTime;
112        pose.angularVel = deltaTheta / deltaTime;
113        
114        leftEncoder.reset();
115        rightEncoder.reset();
116        lastTime = currentTime;
117    }
118    
119    void setWheelSpeeds(float leftSpeed, float rightSpeed) {
120        // Преобразование м/с в PWM
121        int leftPWM = (leftSpeed / params.maxSpeed) * 255;
122        int rightPWM = (rightSpeed / params.maxSpeed) * 255;
123        
124        leftPWM = constrain(leftPWM, -255, 255);
125        rightPWM = constrain(rightPWM, -255, 255);
126        
127        leftMotor.setSpeed(leftPWM);
128        rightMotor.setSpeed(rightPWM);
129    }
130};

 1class DecisionMaker {
 2private:
 3    enum Priority {
 4        CRITICAL = 1,
 5        HIGH = 2,
 6        MEDIUM = 3,
 7        LOW = 4
 8    };
 9    
10    struct Decision {
11        string action;
12        Priority priority;
13        vector<string> conditions;
14        function<void()> executor;
15    };
16    
17    vector<Decision> decisionTree;
18    
19public:
20    void initialize() {
21        // Критические решения (безопасность)
22        addDecision("EMERGENCY_STOP", CRITICAL, 
23                   {"obstacle_too_close", "battery_critical", "system_error"},
24                   [this]() { executeEmergencyStop(); });
25        
26        // Высокоприоритетные решения (основные задачи)
27        addDecision("AVOID_OBSTACLE", HIGH,
28                   {"obstacle_detected", "path_blocked"},
29                   [this]() { executeObstacleAvoidance(); });
30        
31        addDecision("CONTINUE_MISSION", HIGH,
32                   {"path_clear", "battery_ok", "systems_normal"},
33                   [this]() { executeMissionContinuation(); });
34        
35        // Среднеприоритетные решения (оптимизация)
36        addDecision("OPTIMIZE_PATH", MEDIUM,
37                   {"new_path_found", "current_path_inefficient"},
38                   [this]() { executePathOptimization(); });
39        
40        // Низкоприоритетные решения (комфорт)
41        addDecision("UPDATE_STATUS", LOW,
42                   {"status_update_due"},
43                   [this]() { executeStatusUpdate(); });
44    }
45    
46    void processDecisions(map<string, bool> currentConditions) {
47        // Сортируем решения по приоритету
48        sort(decisionTree.begin(), decisionTree.end(),
49             [](const Decision& a, const Decision& b) {
50                 return a.priority < b.priority;
51             });
52        
53        // Проверяем условия и выполняем первое подходящее решение
54        for (auto& decision : decisionTree) {
55            if (checkConditions(decision.conditions, currentConditions)) {
56                Serial.print("Executing decision: ");
57                Serial.println(decision.action.c_str());
58                decision.executor();
59                break; // Выполняем только одно решение за цикл
60            }
61        }
62    }
63    
64private:
65    bool checkConditions(vector<string> required, map<string, bool> current) {
66        for (auto& condition : required) {
67            if (current.count(condition) && current[condition]) {
68                return true; // Любое условие достаточно
69            }
70        }
71        return false;
72    }
73    
74    void executeEmergencyStop() {
75        motion.stop();
76        safety.activateEmergencyProtocol();
77        Serial.println("EMERGENCY STOP ACTIVATED");
78    }
79    
80    void executeObstacleAvoidance() {
81        auto obstacle = sensors.getNearestObstacle();
82        auto newPath = navigation.planAvoidancePath(obstacle);
83        motion.followPath(newPath);
84    }
85};

 1class LearningSystem {
 2private:
 3    struct Experience {
 4        vector<float> sensorInputs;
 5        string action;
 6        float reward;
 7        vector<float> nextSensorInputs;
 8    };
 9    
10    vector<Experience> experienceReplay;
11    NeuralNetwork policyNetwork;
12    
13public:
14    void initialize() {
15        // Инициализация нейронной сети
16        policyNetwork.addLayer(SENSOR_COUNT, ACTIVATION_RELU);
17        policyNetwork.addLayer(64, ACTIVATION_RELU);
18        policyNetwork.addLayer(32, ACTIVATION_RELU);
19        policyNetwork.addLayer(ACTION_COUNT, ACTIVATION_SOFTMAX);
20        
21        policyNetwork.compile(OPTIMIZER_ADAM, LOSS_CATEGORICAL_CROSSENTROPY);
22    }
23    
24    string selectAction(vector<float> sensorInputs) {
25        vector<float> actionProbabilities = policyNetwork.predict(sensorInputs);
26        
27        // Epsilon-greedy стратегия
28        float epsilon = 0.1;
29        if (random(0, 100) < epsilon * 100) {
30            // Случайное действие для исследования
31            return getRandomAction();
32        } else {
33            // Лучшее действие согласно сети
34            int bestActionIndex = max_element(actionProbabilities.begin(), 
35                                            actionProbabilities.end()) - actionProbabilities.begin();
36            return getActionByIndex(bestActionIndex);
37        }
38    }
39    
40    void recordExperience(vector<float> state, string action, float reward, vector<float> nextState) {
41        Experience exp = {state, action, reward, nextState};
42        experienceReplay.push_back(exp);
43        
44        // Ограничиваем размер буфера опыта
45        if (experienceReplay.size() > 10000) {
46            experienceReplay.erase(experienceReplay.begin());
47        }
48    }
49    
50    void trainNetwork() {
51        if (experienceReplay.size() < 100) return; // Недостаточно опыта
52        
53        // Выбираем случайную мини-партию для обучения
54        vector<Experience> batch = sampleRandomBatch(32);
55        
56        vector<vector<float>> inputs, targets;
57        
58        for (auto& exp : batch) {
59            inputs.push_back(exp.sensorInputs);
60            
61            // Расчет целевого Q-значения
62            vector<float> qValues = policyNetwork.predict(exp.sensorInputs);
63            vector<float> nextQValues = policyNetwork.predict(exp.nextSensorInputs);
64            
65            float maxNextQ = *max_element(nextQValues.begin(), nextQValues.end());
66            float targetQ = exp.reward + 0.95 * maxNextQ; // gamma = 0.95
67            
68            int actionIndex = getActionIndex(exp.action);
69            qValues[actionIndex] = targetQ;
70            
71            targets.push_back(qValues);
72        }
73        
74        // Обучаем сеть
75        policyNetwork.fit(inputs, targets, 1);
76    }
77    
78    void saveModel(string filename) {
79        policyNetwork.saveToFile(filename);
80        Serial.print("Model saved to: ");
81        Serial.println(filename.c_str());
82    }
83    
84    void loadModel(string filename) {
85        policyNetwork.loadFromFile(filename);
86        Serial.print("Model loaded from: ");
87        Serial.println(filename.c_str());
88    }
89};