💻 Взаимодействие роботов: от алгоритмов к коду

1🤖 Робот = Набор взаимодействующих модулей

 1class LocomotionModule {
 2private:
 3    Motor leftMotor;
 4    Motor rightMotor;
 5    Encoder leftEncoder;
 6    Encoder rightEncoder;
 7    IMU orientationSensor;
 8    
 9    float currentSpeed = 0;
10    float currentDirection = 0;
11    Position currentPosition;
12    
13public:
14    // Базовые функции движения
15    void moveForward(float speed, float distance = 0);
16    void moveBackward(float speed, float distance = 0);
17    void turnLeft(float angle);
18    void turnRight(float angle);
19    void stop();
20    
21    // Продвинутые функции
22    void moveToPosition(Position target);
23    void followPath(Path trajectory);
24    void maintainFormation(Robot* neighbors[], int count);
25    
26    // Сенсорные функции
27    Position getCurrentPosition();
28    float getCurrentSpeed();
29    float getCurrentDirection();
30};

 1void moveToPosition(float targetX, float targetY) {
 2    // Вычисляем расстояние и угол к цели
 3    float dx = targetX - currentPosition.x;
 4    float dy = targetY - currentPosition.y;
 5    float distance = sqrt(dx*dx + dy*dy);
 6    float targetAngle = atan2(dy, dx) * 180.0 / PI;
 7    
 8    // Поворачиваемся к цели
 9    float angleError = targetAngle - currentPosition.angle;
10    while (abs(angleError) > 180) {
11        angleError += (angleError > 0) ? -360 : 360;
12    }
13    
14    if (abs(angleError) > 5) {  // Допуск 5 градусов
15        if (angleError > 0) {
16            turnLeft(abs(angleError));
17        } else {
18            turnRight(abs(angleError));
19        }
20    }
21    
22    // Движемся к цели
23    moveForward(CRUISE_SPEED, distance);
24}

 1struct Position {
 2    float x, y;      // Координаты в метрах
 3    float angle;     // Ориентация в градусах
 4    
 5    float distanceTo(Position other) {
 6        float dx = x - other.x;
 7        float dy = y - other.y;
 8        return sqrt(dx*dx + dy*dy);
 9    }
10    
11    float angleTo(Position other) {
12        float dx = other.x - x;
13        float dy = other.y - y;
14        return atan2(dy, dx) * 180.0 / PI;
15    }
16};
17
18class NavigationSystem {
19private:
20    Position home = {0, 0, 0};
21    Position current = {0, 0, 0};
22    
23public:
24    void updatePosition() {
25        // Одометрия по энкодерам
26        float leftDistance = leftEncoder.getDistance();
27        float rightDistance = rightEncoder.getDistance();
28        
29        float deltaDistance = (leftDistance + rightDistance) / 2.0;
30        float deltaAngle = (rightDistance - leftDistance) / WHEEL_BASE;
31        
32        current.x += deltaDistance * cos(current.angle * PI / 180.0);
33        current.y += deltaDistance * sin(current.angle * PI / 180.0);
34        current.angle += deltaAngle * 180.0 / PI;
35        
36        // Нормализация угла
37        while (current.angle > 180) current.angle -= 360;
38        while (current.angle < -180) current.angle += 360;
39    }
40};

 1class PerceptionModule {
 2private:
 3    UltrasonicSensor frontSensor;
 4    UltrasonicSensor leftSensor;
 5    UltrasonicSensor rightSensor;
 6    Camera visionSystem;
 7    IMU inertialSensor;
 8    
 9    struct SensorData {
10        float distances[3];  // front, left, right
11        bool obstacleDetected;
12        float brightness;
13        int objectsDetected;
14        float acceleration[3];
15        unsigned long timestamp;
16    };
17    
18    SensorData currentData;
19    
20public:
21    void updateSensors();
22    bool isPathClear(float direction, float distance);
23    Position findNearestObstacle();
24    Robot* detectNearbyRobots();
25    bool recognizeObject(ObjectType type);
26};

 1class SensorFilter {
 2private:
 3    static const int FILTER_SIZE = 5;
 4    float buffer[FILTER_SIZE];
 5    int index = 0;
 6    bool filled = false;
 7    
 8public:
 9    float filter(float newValue) {
10        buffer[index] = newValue;
11        index = (index + 1) % FILTER_SIZE;
12        if (index == 0) filled = true;
13        
14        if (!filled) return newValue;
15        
16        // Медианная фильтрация
17        float sorted[FILTER_SIZE];
18        memcpy(sorted, buffer, sizeof(buffer));
19        
20        // Простая сортировка
21        for (int i = 0; i < FILTER_SIZE-1; i++) {
22            for (int j = i+1; j < FILTER_SIZE; j++) {
23                if (sorted[i] > sorted[j]) {
24                    float temp = sorted[i];
25                    sorted[i] = sorted[j];
26                    sorted[j] = temp;
27                }
28            }
29        }
30        
31        return sorted[FILTER_SIZE/2];  // Медиана
32    }
33};

 1Robot* PerceptionModule::detectNearbyRobots() {
 2    static Robot detectedRobots[MAX_ROBOTS];
 3    int robotCount = 0;
 4    
 5    // Сканируем 360 градусов
 6    for (int angle = 0; angle < 360; angle += 30) {
 7        // Поворачиваем сенсор (если есть сервопривод)
 8        sensorServo.write(angle);
 9        delay(100);  // Время стабилизации
10        
11        float distance = frontSensor.getDistance();
12        
13        // Проверяем, подходит ли расстояние для робота
14        if (distance > 10 && distance < 200) {  // 10см - 2м
15            // Отправляем ping другому роботу
16            communication.sendPing(angle);
17            
18            // Ждем ответ
19            if (communication.waitForResponse(1000)) {  // 1 сек таймаут
20                Robot newRobot;
21                newRobot.distance = distance;
22                newRobot.angle = angle;
23                newRobot.id = communication.getLastResponderId();
24                
25                detectedRobots[robotCount++] = newRobot;
26                
27                if (robotCount >= MAX_ROBOTS) break;
28            }
29        }
30    }
31    
32    // Возвращаем сенсор в исходное положение
33    sensorServo.write(90);
34    
35    return (robotCount > 0) ? detectedRobots : nullptr;
36}

 1// Определение типов сообщений
 2enum MessageType {
 3    PING = 0x01,           // Проверка связи
 4    PONG = 0x02,           // Ответ на ping
 5    COMMAND = 0x10,        // Команда выполнения
 6    DATA = 0x20,           // Передача данных
 7    STATUS = 0x30,         // Статус робота
 8    ERROR = 0x40,          // Сообщение об ошибке
 9    SYNC = 0x50,           // Синхронизация времени
10    ACK = 0x60             // Подтверждение получения
11};
12
13// Структура сообщения (32 байта)
14struct RobotMessage {
15    uint8_t startByte = 0xAA;     // Маркер начала
16    uint8_t senderID;             // ID отправителя
17    uint8_t receiverID;           // ID получателя  
18    uint8_t messageType;          // Тип сообщения
19    uint16_t sequenceNumber;      // Номер последовательности
20    uint16_t payloadLength;       // Длина полезной нагрузки
21    uint8_t payload[20];          // Полезная нагрузка
22    uint16_t checksum;            // Контрольная сумма
23    uint8_t endByte = 0x55;       // Маркер конца
24};

 1class ReliableCommunication {
 2private:
 3    static const int MAX_RETRIES = 3;
 4    static const int ACK_TIMEOUT = 1000; // миллисекунды
 5    uint16_t lastSequenceNumber = 0;
 6    
 7public:
 8    bool sendMessageReliable(uint8_t receiverID, MessageType type, 
 9                           uint8_t* data, uint8_t dataLength) {
10        
11        RobotMessage msg;
12        msg.senderID = MY_ROBOT_ID;
13        msg.receiverID = receiverID;
14        msg.messageType = type;
15        msg.sequenceNumber = ++lastSequenceNumber;
16        msg.payloadLength = dataLength;
17        
18        if (dataLength <= 20) {
19            memcpy(msg.payload, data, dataLength);
20        }
21        
22        msg.checksum = calculateChecksum(msg);
23        
24        // Попытки отправки с повтором
25        for (int attempt = 0; attempt < MAX_RETRIES; attempt++) {
26            // Отправляем сообщение
27            bluetooth.write((uint8_t*)&msg, sizeof(msg));
28            
29            // Ждем подтверждение
30            unsigned long startTime = millis();
31            while (millis() - startTime < ACK_TIMEOUT) {
32                if (checkForAcknowledgment(msg.sequenceNumber)) {
33                    return true;  // Успешно отправлено
34                }
35                delay(10);
36            }
37            
38            Serial.print("Retry attempt ");
39            Serial.println(attempt + 1);
40        }
41        
42        Serial.println("Message delivery failed");
43        return false;  // Не удалось отправить
44    }
45    
46private:
47    uint16_t calculateChecksum(const RobotMessage& msg) {
48        uint16_t sum = 0;
49        uint8_t* ptr = (uint8_t*)&msg;
50        
51        for (int i = 0; i < sizeof(msg) - 2; i++) {  // -2 для самой суммы
52            sum += ptr[i];
53        }
54        
55        return sum;
56    }
57};

 1class LinkQualityMonitor {
 2private:
 3    struct LinkStats {
 4        int messagesSent = 0;
 5        int messagesReceived = 0;
 6        int messagesFailed = 0;
 7        unsigned long totalLatency = 0;
 8        unsigned long lastActivity = 0;
 9    };
10    
11    LinkStats stats[MAX_ROBOTS];
12    
13public:
14    float getPacketLossRate(uint8_t robotID) {
15        LinkStats& s = stats[robotID];
16        if (s.messagesSent == 0) return 0.0;
17        
18        return (float)s.messagesFailed / s.messagesSent * 100.0;
19    }
20    
21    float getAverageLatency(uint8_t robotID) {
22        LinkStats& s = stats[robotID];
23        if (s.messagesReceived == 0) return 0.0;
24        
25        return (float)s.totalLatency / s.messagesReceived;
26    }
27    
28    bool isLinkAlive(uint8_t robotID) {
29        return (millis() - stats[robotID].lastActivity) < 5000; // 5 сек
30    }
31    
32    void printNetworkStatus() {
33        Serial.println("=== Network Status ===");
34        for (int i = 0; i < MAX_ROBOTS; i++) {
35            if (stats[i].messagesSent > 0) {
36                Serial.print("Robot ");
37                Serial.print(i);
38                Serial.print(": Loss=");
39                Serial.print(getPacketLossRate(i));
40                Serial.print("%, Latency=");
41                Serial.print(getAverageLatency(i));
42                Serial.print("ms, Alive=");
43                Serial.println(isLinkAlive(i) ? "Yes" : "No");
44            }
45        }
46    }
47};

 1class TaskAuctionSystem {
 2private:
 3    struct Task {
 4        int taskID;
 5        Position location;
 6        int priority;
 7        int estimatedTime;
 8        bool assigned = false;
 9        uint8_t assignedRobot = 255;
10    };
11    
12    std::vector<Task> taskQueue;
13    
14public:
15    void startAuction(Task newTask) {
16        // Добавляем задачу в очередь
17        taskQueue.push_back(newTask);
18        
19        // Запрашиваем ставки от всех роботов
20        AuctionMessage auctionMsg;
21        auctionMsg.taskID = newTask.taskID;
22        auctionMsg.taskX = newTask.location.x;
23        auctionMsg.taskY = newTask.location.y;
24        auctionMsg.priority = newTask.priority;
25        
26        // Отправляем всем роботам
27        for (int robotID = 1; robotID <= MAX_ROBOTS; robotID++) {
28            if (robotID != MY_ROBOT_ID && isRobotOnline(robotID)) {
29                communication.sendMessage(robotID, AUCTION_REQUEST, 
30                                        (uint8_t*)&auctionMsg, sizeof(auctionMsg));
31            }
32        }
33        
34        // Ждем ставки и выбираем лучшую
35        collectBidsAndAssign(newTask.taskID);
36    }
37    
38private:
39    void collectBidsAndAssign(int taskID) {
40        struct Bid {
41            uint8_t robotID;
42            float cost;  // Меньше = лучше
43            bool received = false;
44        };
45        
46        Bid bids[MAX_ROBOTS];
47        unsigned long auctionStart = millis();
48        const unsigned long AUCTION_TIMEOUT = 3000; // 3 секунды
49        
50        // Собираем ставки
51        while (millis() - auctionStart < AUCTION_TIMEOUT) {
52            RobotMessage msg;
53            if (communication.receiveMessage(msg) && 
54                msg.messageType == AUCTION_BID) {
55                
56                BidMessage* bid = (BidMessage*)msg.payload;
57                if (bid->taskID == taskID) {
58                    bids[msg.senderID].robotID = msg.senderID;
59                    bids[msg.senderID].cost = bid->bidCost;
60                    bids[msg.senderID].received = true;
61                    
62                    Serial.print("Bid from robot ");
63                    Serial.print(msg.senderID);
64                    Serial.print(": cost=");
65                    Serial.println(bid->bidCost);
66                }
67            }
68            delay(10);
69        }
70        
71        // Выбираем лучшую ставку (минимальная стоимость)
72        float bestCost = 99999;
73        uint8_t winnerID = 255;
74        
75        for (int i = 0; i < MAX_ROBOTS; i++) {
76            if (bids[i].received && bids[i].cost < bestCost) {
77                bestCost = bids[i].cost;
78                winnerID = bids[i].robotID;
79            }
80        }
81        
82        // Назначаем задачу победителю
83        if (winnerID != 255) {
84            assignTaskToRobot(taskID, winnerID);
85            notifyAuctionResult(taskID, winnerID);
86        }
87    }
88};

 1class SynchronizedAction {
 2private:
 3    unsigned long synchronizedTime = 0;
 4    bool timeSynchronized = false;
 5    
 6public:
 7    void scheduleSynchronizedAction(unsigned long delayMs, ActionType action) {
 8        // Время выполнения = текущее время + задержка
 9        unsigned long executionTime = getSynchronizedTime() + delayMs;
10        
11        // Отправляем команду синхронизации всем роботам
12        SyncActionMessage syncMsg;
13        syncMsg.actionType = action;
14        syncMsg.executionTime = executionTime;
15        syncMsg.coordinatorID = MY_ROBOT_ID;
16        
17        broadcast(SYNC_ACTION, (uint8_t*)&syncMsg, sizeof(syncMsg));
18        
19        // Сами тоже ждем этого времени
20        waitUntilTime(executionTime);
21        executeAction(action);
22    }
23    
24    void synchronizeTime() {
25        if (isCoordinator()) {
26            // Координатор рассылает свое время
27            SyncTimeMessage timeMsg;
28            timeMsg.currentTime = millis();
29            timeMsg.coordinatorID = MY_ROBOT_ID;
30            
31            broadcast(TIME_SYNC, (uint8_t*)&timeMsg, sizeof(timeMsg));
32            
33            synchronizedTime = timeMsg.currentTime;
34            timeSynchronized = true;
35        }
36    }
37    
38    void waitUntilTime(unsigned long targetTime) {
39        while (getSynchronizedTime() < targetTime) {
40            delay(1);
41            
42            // Обрабатываем входящие сообщения во время ожидания
43            processIncomingMessages();
44        }
45    }
46    
47    unsigned long getSynchronizedTime() {
48        if (!timeSynchronized) return millis();
49        
50        // Возвращаем локальное время относительно синхронизированной точки
51        return synchronizedTime + millis();
52    }
53};

 1class CollisionAvoidance {
 2private:
 3    struct Velocity {
 4        float x, y;
 5        
 6        Velocity operator+(const Velocity& other) const {
 7            return {x + other.x, y + other.y};
 8        }
 9        
10        Velocity operator*(float scalar) const {
11            return {x * scalar, y * scalar};
12        }
13        
14        float magnitude() const {
15            return sqrt(x*x + y*y);
16        }
17    };
18    
19    struct Robot {
20        Position position;
21        Velocity velocity;
22        float radius;
23        uint8_t id;
24    };
25    
26    std::vector<Robot> nearbyRobots;
27    
28public:
29    Velocity computeSafeVelocity(Velocity preferredVelocity, Position myPosition, 
30                                float myRadius, float maxSpeed) {
31        
32        // Если нет соседей, движемся с предпочтительной скоростью
33        if (nearbyRobots.empty()) {
34            return preferredVelocity;
35        }
36        
37        std::vector<Velocity> candidateVelocities;
38        
39        // Генерируем кандидатов скоростей
40        for (float angle = 0; angle < 2*PI; angle += PI/8) {
41            for (float speed = 0; speed <= maxSpeed; speed += maxSpeed/5) {
42                Velocity candidate = {
43                    speed * cos(angle),
44                    speed * sin(angle)
45                };
46                
47                if (isVelocitySafe(candidate, myPosition, myRadius)) {
48                    candidateVelocities.push_back(candidate);
49                }
50            }
51        }
52        
53        // Выбираем ближайшую к предпочтительной скорости
54        Velocity bestVelocity = {0, 0};
55        float minDifference = 99999;
56        
57        for (const auto& candidate : candidateVelocities) {
58            float diff = sqrt(pow(candidate.x - preferredVelocity.x, 2) + 
59                             pow(candidate.y - preferredVelocity.y, 2));
60            
61            if (diff < minDifference) {
62                minDifference = diff;
63                bestVelocity = candidate;
64            }
65        }
66        
67        return bestVelocity;
68    }
69    
70private:
71    bool isVelocitySafe(Velocity velocity, Position myPosition, float myRadius) {
72        const float TIME_HORIZON = 3.0; // секунды в будущее
73        
74        for (const auto& robot : nearbyRobots) {
75            // Предсказываем позиции через TIME_HORIZON секунд
76            Position myFuturePos = {
77                myPosition.x + velocity.x * TIME_HORIZON,
78                myPosition.y + velocity.y * TIME_HORIZON,
79                myPosition.angle
80            };
81            
82            Position robotFuturePos = {
83                robot.position.x + robot.velocity.x * TIME_HORIZON,
84                robot.position.y + robot.velocity.y * TIME_HORIZON,
85                robot.position.angle
86            };
87            
88            // Проверяем расстояние между предсказанными позициями
89            float futureDistance = myFuturePos.distanceTo(robotFuturePos);
90            float minSafeDistance = myRadius + robot.radius + 0.2; // +20см запас
91            
92            if (futureDistance < minSafeDistance) {
93                return false;  // Небезопасная скорость
94            }
95        }
96        
97        return true;  // Безопасная скорость
98    }
99};

  1class SystemTester {
  2private:
  3    enum TestLevel {
  4        UNIT_TEST,        // Тестирование отдельных функций
  5        MODULE_TEST,      // Тестирование модулей
  6        INTEGRATION_TEST, // Тестирование взаимодействия модулей
  7        SYSTEM_TEST       // Тестирование всей системы
  8    };
  9    
 10public:
 11    bool runAllTests() {
 12        Serial.println("=== Starting System Tests ===");
 13        
 14        // Уровень 1: Тестирование базовых функций
 15        if (!runUnitTests()) {
 16            Serial.println("FAIL: Unit tests failed");
 17            return false;
 18        }
 19        
 20        // Уровень 2: Тестирование модулей
 21        if (!runModuleTests()) {
 22            Serial.println("FAIL: Module tests failed");
 23            return false;
 24        }
 25        
 26        // Уровень 3: Тестирование интеграции
 27        if (!runIntegrationTests()) {
 28            Serial.println("FAIL: Integration tests failed");
 29            return false;
 30        }
 31        
 32        // Уровень 4: Системное тестирование
 33        if (!runSystemTests()) {
 34            Serial.println("FAIL: System tests failed");
 35            return false;
 36        }
 37        
 38        Serial.println("SUCCESS: All tests passed!");
 39        return true;
 40    }
 41    
 42private:
 43    bool runUnitTests() {
 44        Serial.println("Running unit tests...");
 45        
 46        // Тест движения
 47        TEST_ASSERT(testMovementFunctions(), "Movement functions");
 48        
 49        // Тест сенсоров
 50        TEST_ASSERT(testSensorFunctions(), "Sensor functions");
 51        
 52        // Тест коммуникации
 53        TEST_ASSERT(testCommunicationFunctions(), "Communication functions");
 54        
 55        return true;
 56    }
 57    
 58    bool testMovementFunctions() {
 59        // Тест базового движения
 60        locomotion.moveForward(50, 100); // 50% скорость, 100мм
 61        delay(2000);
 62        
 63        Position newPos = navigation.getCurrentPosition();
 64        float distance = abs(newPos.distanceTo(Position{0, 0, 0}));
 65        
 66        return (distance > 80 && distance < 120); // Допуск ±20мм
 67    }
 68    
 69    bool testSensorFunctions() {
 70        // Тест ультразвукового датчика
 71        float distance1 = perception.frontSensor.getDistance();
 72        delay(100);
 73        float distance2 = perception.frontSensor.getDistance();
 74        
 75        // Проверяем стабильность показаний
 76        return (abs(distance1 - distance2) < 5); // Допуск 5мм
 77    }
 78    
 79    bool testCommunicationFunctions() {
 80        // Тест отправки/получения сообщения
 81        uint8_t testData[] = {0x12, 0x34, 0x56, 0x78};
 82        
 83        bool sent = communication.sendMessage(BROADCAST_ID, DATA, 
 84                                            testData, sizeof(testData));
 85        
 86        // Для самотестирования отправляем себе
 87        return sent;
 88    }
 89};
 90
 91// Макрос для упрощения тестирования
 92#define TEST_ASSERT(condition, name) \
 93    if (!(condition)) { \
 94        Serial.print("FAIL: "); \
 95        Serial.println(name); \
 96        return false; \
 97    } else { \
 98        Serial.print("PASS: "); \
 99        Serial.println(name); \
100    }

 1class DebugSystem {
 2private:
 3    struct DebugMessage {
 4        unsigned long timestamp;
 5        uint8_t level;  // INFO=0, WARNING=1, ERROR=2, CRITICAL=3
 6        char message[100];
 7    };
 8    
 9    DebugMessage debugBuffer[50];
10    int bufferIndex = 0;
11    bool bufferFull = false;
12    
13public:
14    void log(uint8_t level, const char* format, ...) {
15        va_list args;
16        va_start(args, format);
17        
18        DebugMessage& msg = debugBuffer[bufferIndex];
19        msg.timestamp = millis();
20        msg.level = level;
21        vsnprintf(msg.message, sizeof(msg.message), format, args);
22        
23        // Вывод в Serial
24        const char* levelNames[] = {"INFO", "WARN", "ERR", "CRIT"};
25        Serial.print("[");
26        Serial.print(msg.timestamp);
27        Serial.print("] ");
28        Serial.print(levelNames[level]);
29        Serial.print(": ");
30        Serial.println(msg.message);
31        
32        // Сохранение в буфер
33        bufferIndex = (bufferIndex + 1) % 50;
34        if (bufferIndex == 0) bufferFull = true;
35        
36        va_end(args);
37    }
38    
39    void printDebugHistory() {
40        Serial.println("=== Debug History ===");
41        
42        int start = bufferFull ? bufferIndex : 0;
43        int count = bufferFull ? 50 : bufferIndex;
44        
45        for (int i = 0; i < count; i++) {
46            int idx = (start + i) % 50;
47            DebugMessage& msg = debugBuffer[idx];
48            
49            Serial.print("[");
50            Serial.print(msg.timestamp);
51            Serial.print("] ");
52            Serial.println(msg.message);
53        }
54    }
55    
56    void monitorPerformance() {
57        static unsigned long lastCheck = 0;
58        static int lastLoopCount = 0;
59        static int currentLoopCount = 0;
60        
61        currentLoopCount++;
62        
63        if (millis() - lastCheck >= 1000) { // Каждую секунду
64            int loopsPerSecond = currentLoopCount - lastLoopCount;
65            
66            log(INFO, "Performance: %d loops/sec, Free RAM: %d bytes",
67                loopsPerSecond, getFreeRAM());
68            
69            if (loopsPerSecond < 50) {
70                log(WARNING, "Low performance detected!");
71            }
72            
73            lastCheck = millis();
74            lastLoopCount = currentLoopCount;
75        }
76    }
77    
78private:
79    int getFreeRAM() {
80        extern int __heap_start, *__brkval;
81        int v;
82        return (int)&v - (__brkval == 0 ? (int)&__heap_start : (int)__brkval);
83    }
84};
85
86// Глобальная система отладки
87DebugSystem debug;
88
89// Удобные макросы
90#define DEBUG_INFO(fmt, ...) debug.log(0, fmt, ##__VA_ARGS__)
91#define DEBUG_WARNING(fmt, ...) debug.log(1, fmt, ##__VA_ARGS__)
92#define DEBUG_ERROR(fmt, ...) debug.log(2, fmt, ##__VA_ARGS__)
93#define DEBUG_CRITICAL(fmt, ...) debug.log(3, fmt, ##__VA_ARGS__)

 1class SystemMonitor {
 2private:
 3    struct SystemStats {
 4        unsigned long uptime;
 5        float cpuUsage;
 6        int freeMemory;
 7        float batteryVoltage;
 8        int messagesPerSecond;
 9        int errorsCount;
10        
11        struct ModuleStatus {
12            bool locomotion;
13            bool perception;
14            bool communication;
15            bool coordination;
16        } modules;
17    };
18    
19    SystemStats currentStats;
20    
21public:
22    void updateStats() {
23        currentStats.uptime = millis();
24        currentStats.freeMemory = getFreeRAM();
25        currentStats.batteryVoltage = readBatteryVoltage();
26        currentStats.cpuUsage = calculateCPUUsage();
27        
28        // Проверяем статус модулей
29        currentStats.modules.locomotion = testLocomotionModule();
30        currentStats.modules.perception = testPerceptionModule();
31        currentStats.modules.communication = testCommunicationModule();
32        currentStats.modules.coordination = testCoordinationModule();
33    }
34    
35    void printSystemStatus() {
36        Serial.println("=== System Status ===");
37        Serial.print("Uptime: ");
38        Serial.print(currentStats.uptime / 1000);
39        Serial.println(" seconds");
40        
41        Serial.print("Free RAM: ");
42        Serial.print(currentStats.freeMemory);
43        Serial.println(" bytes");
44        
45        Serial.print("Battery: ");
46        Serial.print(currentStats.batteryVoltage);
47        Serial.println(" V");
48        
49        Serial.print("CPU Usage: ");
50        Serial.print(currentStats.cpuUsage);
51        Serial.println("%");
52        
53        Serial.println("Module Status:");
54        Serial.print("  Locomotion: ");
55        Serial.println(currentStats.modules.locomotion ? "OK" : "FAIL");
56        Serial.print("  Perception: ");
57        Serial.println(currentStats.modules.perception ? "OK" : "FAIL");
58        Serial.print("  Communication: ");
59        Serial.println(currentStats.modules.communication ? "OK" : "FAIL");
60        Serial.print("  Coordination: ");
61        Serial.println(currentStats.modules.coordination ? "OK" : "FAIL");
62    }
63    
64    bool isSystemHealthy() {
65        return (currentStats.batteryVoltage > 6.5 &&  // Минимальное напряжение
66                currentStats.freeMemory > 500 &&       // Минимум свободной памяти
67                currentStats.modules.locomotion &&     // Все модули работают
68                currentStats.modules.perception &&
69                currentStats.modules.communication);
70    }
71    
72private:
73    float readBatteryVoltage() {
74        int rawValue = analogRead(BATTERY_PIN);
75        return rawValue * (5.0 / 1023.0) * VOLTAGE_DIVIDER_RATIO;
76    }
77    
78    float calculateCPUUsage() {
79        static unsigned long lastMeasure = 0;
80        static unsigned long lastIdle = 0;
81        
82        unsigned long currentTime = millis();
83        unsigned long idleTime = getIdleTime(); // Функция подсчета простоя
84        
85        if (lastMeasure != 0) {
86            unsigned long timeDiff = currentTime - lastMeasure;
87            unsigned long idleDiff = idleTime - lastIdle;
88            
89            return (1.0 - (float)idleDiff / timeDiff) * 100.0;
90        }
91        
92        lastMeasure = currentTime;
93        lastIdle = idleTime;
94        return 0.0;
95    }
96};

 1КОМАНДА: _________ РЕВЬЮЕР: _________
 2
 3СИЛЬНЫЕ СТОРОНЫ:
 4✓ ________________________________
 5✓ ________________________________
 6
 7ОБЛАСТИ ДЛЯ УЛУЧШЕНИЯ:
 8△ ________________________________
 9△ ________________________________
10
11ВОПРОСЫ К КОДУ:
12? ________________________________
13? ________________________________
14
15ОБЩАЯ ОЦЕНКА: ___/10