diff --git a/RETOCAR_Junior_Main_ESP/.gitignore b/RETOCAR_Junior_Main_ESP/.gitignore index 89cc49c..e57883d 100644 --- 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/ \ No newline at end of file diff --git a/RETOCAR_Junior_Main_ESP/lib/common/DataStructs.h b/RETOCAR_Junior_Main_ESP/lib/common/DataStructs.h index e69de29..516e2df 100644 --- 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 +#include + +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 \ No newline at end of file diff --git a/RETOCAR_Junior_Main_ESP/lib/driver/SlaveComm.cpp b/RETOCAR_Junior_Main_ESP/lib/driver/SlaveComm.cpp index e69de29..0a45b48 100644 --- 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); +} \ No newline at end of file diff --git a/RETOCAR_Junior_Main_ESP/lib/driver/SlaveComm.h b/RETOCAR_Junior_Main_ESP/lib/driver/SlaveComm.h index e69de29..dadbd7a 100644 --- 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 +#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 \ No newline at end of file diff --git a/RETOCAR_Junior_Main_ESP/platformio.ini b/RETOCAR_Junior_Main_ESP/platformio.ini index 878618a..7997102 100644 --- 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 \ No newline at end of file diff --git a/RETOCAR_Junior_Main_ESP/src/main.cpp b/RETOCAR_Junior_Main_ESP/src/main.cpp index e5defda..512b2b6 100644 --- 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); } }