retobots · nguyenhuuddinh · Apr 19, 2026
diff --git a/RETOCAR_Junior_Main_ESP/.gitignore b/RETOCAR_Junior_Main_ESP/.gitignore
@@ -3,3 +3,7 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
+AI/
+../testMaxDongCo/
+../test_encoder/
+../test_radio/
diff --git a/RETOCAR_Junior_Main_ESP/lib/common/DataStructs.h b/RETOCAR_Junior_Main_ESP/lib/common/DataStructs.h
@@ -0,0 +1,36 @@
+#ifndef DATA_STRUCTS_H
+#define DATA_STRUCTS_H
+
+#include <Arduino.h>
+#include <HardwareSerial.h>
+
+struct RobotCommand {
+    int8_t vx;
+    int8_t vy;  
+    int8_t omega;
+    uint8_t mode;
+    uint8_t buzzer;
+} __attribute__((packed));
+
+struct RobotState {
+    float posX;
+    float posY;
+    float heading;
+
+    uint8_t lineL2;
+    uint8_t lineL1;
+    uint8_t lineC;
+    uint8_t lineR1;
+    uint8_t lineR2;
+
+    float distance;
+    uint16_t battery;
+
+    int32_t tickFL;
+    int32_t tickFR;
+    int32_t tickRL;
+    int32_t tickRR;
+
+} __attribute__((packed));
+
+#endif
diff --git a/RETOCAR_Junior_Main_ESP/lib/driver/SlaveComm.cpp b/RETOCAR_Junior_Main_ESP/lib/driver/SlaveComm.cpp
@@ -0,0 +1,122 @@
+#include "SlaveComm.h"
+
+SlaveComm slaveComm;
+
+SlaveComm::SlaveComm() {
+    m_state = ParserState::WAIT_HEADER;
+    m_payloadIndex = 0;
+    m_expectedLength = 0;
+}
+
+void SlaveComm::begin(uint32_t baudrate) {
+    // Tăng kích thước bộ đệm Hardware Buffer nhằm giảm tỷ lệ packet loss 
+    // trong trường hợp Core 0 phải bận xử lý gửi dữ liệu ESP-NOW.
+    Serial2.setRxBufferSize(256);
+    Serial2.begin(baudrate);
+}
+
+bool SlaveComm::readData(RobotState &state) {
+    bool isPacketReady = false;
+
+    // Đọc liên tục cho tới khi xử lý sạch bộ đệm UART để đạt độ trễ thấp nhất có thể.
+    while (Serial2.available() > 0) {
+        uint8_t incomingByte = Serial2.read();
+
+        switch (m_state) {
+            case ParserState::WAIT_HEADER:
+                // Theo kiến trúc, 0xAA là chữ ký chiều Slave (STM32) -> Master (ESP32)
+                if (incomingByte == HEADER_RX) {
+                    m_state = ParserState::WAIT_LENGTH;
+                }
+                break;
+
+            case ParserState::WAIT_LENGTH:
+                m_expectedLength = incomingByte;
+
+                // Kiểm tra giới hạn bộ đệm để loại bỏ tình trạng kẹt trạng thái của State Machine
+                if (m_expectedLength == 0 || m_expectedLength > UART_MAX_LENGTH) {
+                    Serial.printf("[UART] Cảnh báo lỗi độ dài gói tin sai chuẩn: %d bytes\n", m_expectedLength);  // debug
+                    m_state = ParserState::WAIT_HEADER;
+                } else {
+                    m_payloadIndex = 0;
+                    m_state = ParserState::RECEIVE_PAYLOAD;
+                }
+                break;
+
+            case ParserState::RECEIVE_PAYLOAD:
+                m_payloadBuffer[m_payloadIndex++] = incomingByte;
+
+                if (m_payloadIndex >= m_expectedLength) {
+                    m_state = ParserState::WAIT_CHECKSUM;
+                }
+                break;
+
+            case ParserState::WAIT_CHECKSUM:
+                uint8_t calculatedCrc = calculateCrc8(m_payloadBuffer, m_expectedLength);
+
+                // Mọi gói dữ liệu hợp lệ bắt buộc đồng bộ mã CRC8 vòng
+                if (incomingByte == calculatedCrc) {
+                    if (m_expectedLength == RAW_TELEMETRY_SIZE) { 
+                        // Dữ liệu nội mạng nên ưu tiên ép kiểu thô memcpy, 
+                        // ESP32 và STM32 đều dùng Little-Endian
+                        memcpy(&state.tickFL, &m_payloadBuffer[0], 4);
+                        memcpy(&state.tickFR, &m_payloadBuffer[4], 4);
+                        memcpy(&state.tickRL, &m_payloadBuffer[8], 4);
+                        memcpy(&state.tickRR, &m_payloadBuffer[12], 4);
+
+                        uint8_t rawLine = m_payloadBuffer[16];
+                        state.lineL2 = bitRead(rawLine, 4);
+                        state.lineL1 = bitRead(rawLine, 3);
+                        state.lineC  = bitRead(rawLine, 2);
+                        state.lineR1 = bitRead(rawLine, 1);
+                        state.lineR2 = bitRead(rawLine, 0);
+
+                        memcpy(&state.battery, &m_payloadBuffer[17], 2);
+
+                        isPacketReady = true;
+                    }
+                } else {
+                    // Cảnh báo ở cấp độ code lỗi bộ đệm do nhiễu UART.
+                    // Serial.println("[UART] Lỗi lệch CRC Mismatch! Packet Dropped.");
+                }
+
+                // Trả về đợi bản tin mới không cần biết fail hay pass. 
+                // Cấu trúc FSM Push từ slave có lợi ích tự cứu này !
+                m_state = ParserState::WAIT_HEADER;
+                break;
+        }
+    }
+    return isPacketReady;
+}
+
+// Thuật toán kiểm tra vòng CRC8 tiêu chuẩn đa thức chuẩn 0x07 (x^8 + x^2 + x + 1)
+uint8_t SlaveComm::calculateCrc8(uint8_t* data, uint8_t length) {
+    uint8_t crc = 0x00;
+    for (uint8_t i = 0; i < length; i++) {
+        crc ^= data[i];
+        for (uint8_t j = 0; j < 8; j++) {
+            if (crc & 0x80) {
+                crc = (crc << 1) ^ 0x07;
+            } else {
+                crc <<= 1;
+            }
+        }
+    }
+    return crc;
+}
+
+void SlaveComm::sendCommand(RobotCommand cmd) {
+    // Độ lớn lệnh cơ sở của dự án là tĩnh và gói gọn trong 2 byte (Led+Còi)
+    uint8_t payload[2];
+    payload[0] = cmd.mode;
+    payload[1] = cmd.buzzer;
+    uint8_t length = sizeof(payload);
+
+    uint8_t crc = calculateCrc8(payload, length);
+
+    // Mã nhận diện gửi luồng UART xuống là 0xBB
+    Serial2.write(HEADER_TX);
+    Serial2.write(length);
+    Serial2.write(payload, length);
+    Serial2.write(crc);
+}
diff --git a/RETOCAR_Junior_Main_ESP/lib/driver/SlaveComm.h b/RETOCAR_Junior_Main_ESP/lib/driver/SlaveComm.h
@@ -0,0 +1,47 @@
+#ifndef SLAVE_COMM_HPP
+#define SLAVE_COMM_HPP
+
+#include <Arduino.h>
+#include "../common/DataStructs.h"
+
+// Trạng thái của bộ giải mã gói tin UART từ Slave
+enum class ParserState {
+    WAIT_HEADER,
+    WAIT_LENGTH,
+    RECEIVE_PAYLOAD,
+    WAIT_CHECKSUM
+};
+
+class SlaveComm {
+private:
+    ParserState m_state;
+    uint8_t m_payloadBuffer[64];
+    uint8_t m_payloadIndex;
+    uint8_t m_expectedLength;
+
+    // Giao thức Hãng: Định dạng Header và Giới hạn Payload để triệt nhiễu
+    static const uint8_t HEADER_RX = 0xAA;
+    static const uint8_t HEADER_TX = 0xBB;
+    static const uint8_t UART_MAX_LENGTH = 64;
+    static const uint8_t RAW_TELEMETRY_SIZE = 19; // 16 bytes(Encoder) + 1 byte(Line) + 2 bytes(Battery)
+
+    // Tính mã CRC vòng tránh rách dữ liệu
+    uint8_t calculateCrc8(uint8_t* data, uint8_t length);
+
+public:
+    SlaveComm();
+
+    // Khởi tạo UART2, được khuyên dùng setRxBufferSize để tăng độ ổn định bộ nhớ
+    void begin(uint32_t baudrate = 115200);
+
+    // Giải mã gói tin đẩy từ STM32, Non-blocking an toàn
+    bool readData(RobotState &state);
+
+    // Đóng gói RobotCommand với mã Header 0xBB hướng gửi xuống thiết bị phụ
+    void sendCommand(RobotCommand cmd);
+};
+
+// Truy cập theo cấu trúc Object duy nhất Singleton
+extern SlaveComm slaveComm;
+
+#endif
diff --git a/RETOCAR_Junior_Main_ESP/platformio.ini b/RETOCAR_Junior_Main_ESP/platformio.ini
@@ -8,7 +8,9 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env:esp32-c3-devkitm-1]
+[env:esp32doit-devkit-v1]
 platform = espressif32
-board = esp32-c3-devkitc-02
+board = esp32doit-devkit-v1
 framework = arduino
+
+monitor_speed = 115200
diff --git a/RETOCAR_Junior_Main_ESP/src/main.cpp b/RETOCAR_Junior_Main_ESP/src/main.cpp
@@ -10,6 +10,9 @@
 TaskHandle_t hControlTask, hCommSlaveTask, hRadioTask, hSystemTask;
 SemaphoreHandle_t xMutexData;
 
+RobotState g_robotState;
+RobotCommand g_robotCmd;
+
 // --- Task Prototypes ---
 void Task_Control(void *pv);    // Core 1 - 10ms: PID & Hardware Output
 void Task_Comm_Slave(void *pv); // Core 0 - 10ms: UART DMA from STM32
@@ -23,7 +26,7 @@ void setup()
 
   // Khởi tạo Drivers & Middleware
   // motorDriver.begin();
-  // slaveComm.begin();
+  slaveComm.begin();
   // radioService.begin();
   // navigator.init();
 
@@ -69,19 +72,59 @@ void Task_Control(void *pv)
 // TASK 2: Giải mã UART từ STM32 (10ms)
 void Task_Comm_Slave(void *pv)
 {
+  TickType_t xLastWakeTime = xTaskGetTickCount();
+  const TickType_t xInterval = pdMS_TO_TICKS(10); // Check UART mỗi 10ms
+
   while (1)
   {
-    // // SlaveComm xử lý UART DMA ở background, Task này chỉ bóc tách struct
-    // if (slaveComm.available())
-    // {
-    //   if (xSemaphoreTake(xMutexData, pdMS_TO_TICKS(2)))
-    //   {
-    //     g_robotState.encoders = slaveComm.getEncoders();
-    //     g_robotState.lineSensors = slaveComm.getLine();
-    //     xSemaphoreGive(xMutexData);
-    //   }
-    // }
-    vTaskDelay(pdMS_TO_TICKS(10));
+    // Cầm riêng 1 Object để nhào nặn Data UART, không làm tắc nghẽn Semaphone của người khác
+    RobotState tempState;
+
+    if (slaveComm.readData(tempState))
+    {
+      // Chỉ khi tín hiệu báo Data Payload cực dạch bóc thành công, ta mới xin chìa khoá Mutex gán Memory
+      if (xSemaphoreTake(xMutexData, pdMS_TO_TICKS(3)))
+      {
+        g_robotState.tickFL = tempState.tickFL;
+        g_robotState.tickFR = tempState.tickFR;
+        g_robotState.tickRL = tempState.tickRL;
+        g_robotState.tickRR = tempState.tickRR;
+
+        g_robotState.lineL2 = tempState.lineL2;
+        g_robotState.lineL1 = tempState.lineL1;
+        g_robotState.lineC  = tempState.lineC;
+        g_robotState.lineR1 = tempState.lineR1;
+        g_robotState.lineR2 = tempState.lineR2;
+
+        g_robotState.battery = tempState.battery;
+
+        // // ---Đặt dữ liệu tĩnh để STM32 chớp tín hiệu led nhịp ---
+        // g_robotCmd.mode = 1;   
+        // g_robotCmd.buzzer = 0; 
+
+        // --- DEBUG SERIAL: Kểm tra xem có nhận được dữ liệu không ---
+        // static uint8_t debugCounter = 0;
+        // if (++debugCounter >= 50) {
+        //     Serial.printf("[DEBUG UART] Encoders: %d | %d | %d | %d -- Line: %d %d %d %d %d -- Pin: %d mV\n",
+        //         g_robotState.tickFL, g_robotState.tickFR, g_robotState.tickRL, g_robotState.tickRR,
+        //         g_robotState.lineL2, g_robotState.lineL1, g_robotState.lineC, g_robotState.lineR1, g_robotState.lineR2,
+        //         g_robotState.battery);
+        //     debugCounter = 0;
+        // }
+
+        xSemaphoreGive(xMutexData);
+      }
+    }
+
+    // Đẩy tiếp cấu hình mới nhất hiện tại xuống mạch STM32 bằng lệnh sendCommand
+    RobotCommand currentCmd;
+    if (xSemaphoreTake(xMutexData, pdMS_TO_TICKS(2))) {
+        currentCmd = g_robotCmd;
+        xSemaphoreGive(xMutexData);
+    }
+    slaveComm.sendCommand(currentCmd); // Bắn ngược lại UART
+
+    vTaskDelayUntil(&xLastWakeTime, xInterval);
   }
 }