lingyunxsh/mqtt_power
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

单片机互通调试初步完成

Browse Source
This commit is contained in:
lingyunxsh
2025-05-27 16:44:45 +08:00
parent 22e1109d81
commit 6aaae06bac
22 changed files with 1678 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
LogBook.md
View File

@@ -4,3 +4,40 @@
- 初始化项目变更日志。
- 确认采用 Gitea Webhook + Shell 脚本的 CI/CD 方案。
- 部署脚本 (`deploy.sh`) 和相关配置已准备。
## YYYY-MM-DD (请替换为实际日期)
- 详细分析了项目整体结构，特别是单片机 (`mqtt_esp32_client.ino`) 与后端 (`springboot-init-main`) 之间基于 `README.md` 的 MQTT 通信流程和协议。
- **核心理解**:
- **主题**:
- 上行 (设备 -> 后端): `yupi_mqtt_power_project/robot/status/{spotUid}` (状态/心跳/ACK)
- 下行 (后端 -> 设备): `yupi_mqtt_power_project/robot/command/{spotUid}` (指令)
- **消息格式**: JSON。上行消息结构对应后端 `com.yupi.project.model.dto.mqtt.RobotStatusMessage`。
- **关键上行消息字段**: `robotUid`, `actualRobotStatus`, `taskId` (for ACK), `status` (for ACK), `activeTaskId` (通常为 `sessionId`)。
- **关键下行消息字段**: `commandType` ("START_CHARGE", "STOP_CHARGE", 等), `taskId`, `sessionId` (for "START_CHARGE")。
- **通信流程**: 设备连接后订阅指令主题并上报初始状态。周期性上报状态和心跳。后端下发指令，设备执行后通过ACK响应，并更新状态。
- **`activeTaskId` 的作用**: 该字段在单片机的状态上报和ACK中被用来传递当前有效的 `sessionId`，对于后端跟踪会话状态至关重要。
- 单片机代码中包含对 `MOVE_TO_SPOT` 指令的处理，`README.md` 未明确列出，后端可能需同步支持。
- 后续 Debug 将基于此理解进行。
## YYYY-MM-DD (请替换为今天的实际日期)
- **核心逻辑变更："到达即充电"**
- 移除了原先设想的由后端发送 `START_CHARGE` MQTT 指令来启动充电的流程。
- 新流程：机器人（单片机）在执行 `MOVE_TO_SPOT` 指令并成功到达车位后，立即将自身状态更新为 `CHARGING` 并上报。后端在收到 `MOVE_TO_SPOT` 任务的 ACK (其中应包含机器人已是 `CHARGING` 状态) 后，将机器人DB状态更新为 `CHARGING`，并将充电会话状态更新为 `CHARGING_STARTED`。
- **代码修改**:
- `mqtt_esp32_client.ino`:
- `MOVE_TO_SPOT` 指令处理逻辑修改：
- 收到指令后，状态变为 `MOVING` 并上报。
- 模拟移动到达后，状态变为 `CHARGING`。
- 发送的 ACK 消息中 `actualRobotStatus` 反映为 `CHARGING`。
- 再次上报 `CHARGING` 状态。
- `ChargingSessionServiceImpl.java`:
- `handleRobotArrival` 方法修改：
- 当 `MOVE_TO_SPOT` 任务完成后，将充电会话状态 (`ChargingSession.status`) 直接更新为 `CHARGING_STARTED`。
- 记录 `robotArrivalTime` 和 `chargeStartTime`。
- 将数据库中机器人状态 (`ChargingRobot.status`) 更新为 `CHARGING`。
- 确保关联的 `RobotTask` (移动任务) 被正确处理和完成。
- `README.md`:
- 更新了MQTT通信约定、消息示例、单片机开发关键逻辑以及示例充电流程，以反映"到达即充电"的新业务规则。
- **待考虑/后续**:
- `ChargingSessionServiceImpl.handleChargingStart` 方法目前因 `START_CHARGE` 指令不再由后端主动发送而可能不再被主要流程调用，其定位和用途需要进一步明确或在未来重构中调整。
- 前端界面和用户交互流程可能需要相应调整，以匹配后端"到达即充电"的逻辑（例如，可能不再需要用户在机器人到达后点击"开始充电"按钮）。

88
README.md
View File

@@ -142,34 +142,30 @@
* **消息格式 (Payload)**: JSON。
消息体结构应与后端 `com.yupi.project.model.dto.mqtt.RobotStatusMessage` 类对应。通过消息体内的字段来区分具体的消息含义。
**示例 - 常规状态更新或心跳包 (Heartbeat):**
**示例 - 常规状态更新 (如移动中、充电中、空闲等) 或心跳包 (Heartbeat):**
```json
{
"robotUid": "ESP32_SPOT_001",
"actualRobotStatus": "IDLE", // 当前设备状态，如 IDLE, CHARGING, COMPLETED, FAULTED
"actualRobotStatus": "CHARGING", // 设备当前状态, 如 IDLE, MOVING, CHARGING, COMPLETED, FAULTED
// 以下为可选的详细状态，根据实际需求和后端处理逻辑添加
"voltage": 220.5,
"current": 0.0,
"power": 0.0,
"energyConsumed": 1.23, // kWh, 本次或累计，根据业务定义
"errorCode": 0, // 设备故障码，0为正常
"message": "Device operational", // 可选的文本消息
"activeTaskId": null // 如果当前正在执行某个任务，可上报任务ID或会话ID
"voltage": 220.5, // 充电时相关
"current": 5.1, // 充电时相关
"power": 1.12, // kW, 充电时相关
"energyConsumed": 0.5, // kWh, 本次充电已消耗电量
"errorCode": 0, // 0表示无错误，其他值表示特定错误类型
"activeTaskId": "session_xyz123" // 如果设备当前正在执行某个任务或会话，上报其ID
}
```
* **心跳频率**: 建议每 30-60 秒发送一次心跳（可以是一个简化的状态更新消息）。
**示例 - 指令执行回执 (ACK):**
当单片机执行完后端下发的指令后，应向此统一上行主题发送一个回执消息。
**示例 - 对后端指令的ACK (成功):**
```json
{
"robotUid": "ESP32_SPOT_001",
"taskId": "backend_provided_task_id_123", // 后端下发指令时提供的taskId
"status": "SUCCESS", // 或 "FAILURE"
"message": "Charging started successfully", // 对指令执行结果的描述
"errorCode": "0", // 如果失败，提供错误码
"actualRobotStatus": "CHARGING" // 执行指令后设备的当前状态
// "activeTaskId": "session_xyz" // 可选，如果与特定会话相关
"taskId": "backend_task_789", // 对应后端指令中的taskId
"status": "SUCCESS", // 指令执行结果: SUCCESS 或 FAILURE
"message": "Command executed successfully",
"actualRobotStatus": "IDLE", // ACK发生时，设备的当前核心状态
"activeTaskId": "session_abc456" // 可选，如果ACK与特定会话相关
}
```
@@ -182,44 +178,58 @@
* **消息格式 (Payload)**: JSON。
JSON消息体内部包含了具体的指令类型和所需参数。
**示例 - 启动充电指令:**
**示例 - 移动到车位指令 (MOVE_TO_SPOT):**
此指令指示机器人移动到指定车位。机器人到达后将自动进入充电状态。
```json
{
"commandType": "START_CHARGE", // 指令类型
"commandType": "MOVE_TO_SPOT",
"taskId": "backend_task_id_for_ack_789", // 供单片机ACK时使用的任务ID
"sessionId": "session_abc_123" // 关联的充电会话ID
// ...其他可能的参数...
"target_spot_uid": "SPOT_UID_001" // 目标车位ID (单片机仅用于确认，主要由 spotUid 主题参数决定目标)
// "sessionId": "session_abc_123" // 可选，如果需要在payload中也传递会话ID
}
```
**示例 - 停止充电指令:**
**示例 - 停止充电指令 (STOP_CHARGE):**
```json
{
"commandType": "STOP_CHARGE",
"taskId": "backend_task_id_for_ack_000"
// ...其他可能的参数...
"taskId": "backend_task_id_for_ack_000",
"sessionId": "session_abc_123" // 关联的充电会话ID
}
```
* **单片机处理逻辑**: 单片机收到消息后，需解析JSON负载，识别 `commandType`，提取 `taskId` (用于后续ACK)，并获取其他指令参数来执行相应操作。
* **单片机处理逻辑**: 单片机收到消息后，需解析JSON负载，识别 `commandType`，提取 `taskId` (用于后续ACK)，并获取其他指令参数来执行相应操作。 对于 `MOVE_TO_SPOT`，到达后自动转换为 `CHARGING` 状态。
### 4.3. 单片机开发关键逻辑
1. **MQTT 初始化与重连机制**。
2. **订阅指令主题**。
3. **JSON 指令消息解析**。
4. **充电启停控制** (继电器等硬件操作)。
5. **状态监测与实时上报**。
6. **心跳包定时发送**。
7. **故障检测与上报**。
8. **安全性**: 确保 `spotUid` 唯一性；推荐 MQTT 通信使用 TLS/SSL 加密。
4. **移动控制** (如果机器人需要物理移动)。
5. **充电启停控制** (继电器等硬件操作，停止充电时需要)。
6. **状态监测与实时上报**:
* 收到 `MOVE_TO_SPOT` 后，状态变为 `MOVING` 并上报。
* 到达目标车位后，状态变为 `CHARGING` 并上报，同时对 `MOVE_TO_SPOT` 指令进行ACK。
* 收到 `STOP_CHARGE` 后，停止充电，状态变为 `COMPLETED` (或 `IDLE`) 并上报，同时对 `STOP_CHARGE` 指令进行ACK。
7. **心跳包定时发送**。
8. **故障检测与上报**。
9. **安全性**: 确保 `spotUid` 唯一性；推荐 MQTT 通信使用 TLS/SSL 加密。
### 4.4. 示例流程：用户启动充电
1. 用户在前端 App 请求启动充电桩 `SPOT001`。
2. 后端服务验证，创建充电会话 `sessionId=789`。
3. 后端向 MQTT 主题 `charging/spot/SPOT001/command/start` 发布启动指令。
4. 单片机 `SPOT001` 接收指令，启动充电。
5. 单片机向 `charging/spot/SPOT001/status` 上报状态 `CHARGING`，包含 `sessionId`。
6. (可选) 单片机向 `charging/spot/SPOT001/command/ack` 发布成功回执。
7. 后端更新会话状态，前端界面同步更新。
### 4.4. 示例流程：用户启动充电 (更新后)
1. 用户在前端 App 请求在车位 `SPOT001` 进行充电。
2. 后端服务验证，创建充电会话 `sessionId=789`，状态为 `REQUESTED`。
3. 后端分配机器人，创建 `MOVE_TO_SPOT` 任务 (例如 `taskId=task_move_123`)，并将机器人DB状态更新为 `MOVING`，会话状态更新为 `ROBOT_ASSIGNED`。
4. 后端向 MQTT 主题 `yupi_mqtt_power_project/robot/command/SPOT001` 发布 `MOVE_TO_SPOT` 指令，payload 包含 `taskId=task_move_123`。
5. 单片机 `SPOT001` 接收指令：
a. 将其内部状态更新为 `MOVING`。
b. 向上行主题 `yupi_mqtt_power_project/robot/status/SPOT001` 上报状态 `MOVING`。
c. (模拟)执行移动。
d. 移动到达后，将其内部状态更新为 `CHARGING`。
e. 向上行主题发送 ACK 消息，确认 `taskId=task_move_123` 完成，消息中 `actualRobotStatus` 为 `CHARGING`。
f. (可选，或由ACK消息覆盖) 再次向上行主题上报状态 `CHARGING`，包含 `sessionId`。
6. 后端接收到 `MOVE_TO_SPOT` 任务 ( `taskId=task_move_123`) 的 ACK (其中机器人状态为 `CHARGING`):
a. 将 `RobotTask` ( `taskId=task_move_123`) 标记为 `COMPLETED`。
b. 将机器人 `SPOT001` 在数据库中的状态更新为 `CHARGING`。
c. 将充电会话 `sessionId=789` 的状态更新为 `CHARGING_STARTED`，并记录充电开始时间。
7. 前端界面同步更新，显示充电已开始。
## 5. 项目结构 (简要)

126
esp32_robot_firmware/NEXT_STEPS.md Normal file
View File

@@ -0,0 +1,126 @@
# ESP32 机器人固件 - 后续步骤与待办事项
本文档旨在记录在当前固件版本基础上，您需要完成的配置、代码调整以及后续的测试和开发步骤。
## 1. 关键配置与代码调整 (必须)
在烧录和测试之前，请务必完成以下配置和代码检查/调整：
### 1.1. `config.h` - 硬件与网络配置
打开 `esp32_robot_firmware/config.h` 文件，根据您的实际情况修改以下宏定义：
* **WiFi 配置**:
* `WIFI_SSID`: 您的WiFi网络名称。
* `WIFI_PASSWORD`: 您的WiFi密码。
* **MQTT Broker 配置**:
* 检查 `MQTT_BROKER_HOST`, `MQTT_BROKER_PORT` 是否适用于您的MQTT代理。
* 如果您的代理需要认证，请更新 `MQTT_USERNAME` 和 `MQTT_PASSWORD`。
* **设备唯一标识符**:
* `DEVICE_UID`: **极其重要**，必须与您在后端系统中注册的机器人ID完全一致。
* **硬件引脚定义**:
* `MOTOR_LEFT_IN1_PIN`, `MOTOR_LEFT_IN2_PIN`, `MOTOR_LEFT_ENA_PIN`
* `MOTOR_RIGHT_IN3_PIN`, `MOTOR_RIGHT_IN4_PIN`, `MOTOR_RIGHT_ENB_PIN`
* `SERVO_PIN`
* `ULTRASONIC_FRONT_TRIG_PIN`, `ULTRASONIC_FRONT_ECHO_PIN`
* `ULTRASONIC_SIDE_TRIG_PIN`, `ULTRASONIC_SIDE_ECHO_PIN`
* `TCRT5000_LEFT_PIN`, `TCRT5000_RIGHT_PIN` (以及您可能添加的更多循迹传感器引脚)
* `RELAY_PIN`
* **请仔细核对这些引脚号是否与您ESP32板子上实际连接的硬件引脚一致。**
* **业务逻辑参数**:
* `SIDE_ULTRASONIC_SPOT_THRESHOLD_CM`: 侧向超声波检测到充电桩的距离阈值，根据实际情况调整。
* `NUM_CHARGING_SPOTS` 和 `SPOT_ID_1`, `SPOT_ID_2`, `SPOT_ID_3`: 根据您的充电桩数量和ID定义进行调整。
### 1.2. `navigation.cpp` - 循迹逻辑 (`navigation_follow_line_once`)
当前 `navigation_follow_line_once` 函数中的循迹逻辑非常基础，**很可能无法直接在您的机器人上良好工作**。您需要：
* **大幅调整**: 根据您的循迹传感器类型（模拟/数字）、数量、安装位置以及机器人的机械结构（轮子、电机特性）来修改逻辑。
* **参数校准**: `base_speed`, `turn_speed_diff`, `servo_turn_angle_small`, `servo_turn_angle_large` 等参数需要反复实验和校准。
* **考虑PID控制**: 为了实现更平滑、更准确的循迹，强烈建议研究并实现PID（比例-积分-微分）控制算法。代码中已预留了PID相关的变量和注释框架。
* **脱线处理**: 当前脱线处理很简单（停止），您可能需要实现更复杂的逻辑，如原地旋转搜索线路。
### 1.3. `sensor_handler.cpp` - 传感器校准
* **TCRT5000循迹传感器**:
* 打开 `sensor_handler.h`，校准 `TCRT5000_THRESHOLD` 的值。这个值取决于您的传感器、巡的线的颜色、地面的颜色。
* 在 `sensor_handler.cpp` 的 `is_on_line` 函数中，确认 `return sensor_value > threshold;` 这行（或您修改后的逻辑）是否正确反映了传感器在黑线上和不在黑线上的读数关系（是值变大还是变小）。
* **超声波传感器**:
* 测试 `get_ultrasonic_distance_cm` 函数的准确性。`pulseIn` 的超时时间可能需要根据实际最大探测距离调整。
### 1.4. `relay_control.cpp` - 继电器逻辑
* 确认 `relay_setup()` 中 `digitalWrite(RELAY_PIN, LOW);` 是否使继电器处于断开状态。有些继电器模块可能是高电平触发断开。相应地调整 `relay_start_charging()` 和 `relay_stop_charging()` 中的 `HIGH`/`LOW`。
## 2. 后续开发与测试步骤
完成上述配置和初步代码调整后，按以下步骤进行测试和进一步开发：
### 2.1. 模块化硬件测试与校准
逐个模块进行测试，确保硬件按预期工作：
* **电机 (`motor_control.cpp`)**:
* 测试 `motor_move_forward()`, `motor_move_backward()`, `motor_stop()`。
* 测试 `motor_turn_left()`, `motor_turn_right()`，观察差速和舵机辅助转向的效果。调整速度、转向强度和舵机角度参数。
* 确保 `set_motor_speed()` 中 `MOTOR_LEFT_IN1_PIN` 等引脚的 `HIGH`/`LOW` 组合能正确控制电机方向。
* **舵机 (`motor_control.cpp`)**:
* 测试 `servo_set_angle()`, `servo_center()`。确认舵机转动范围和中心位置是否准确。
* **传感器 (`sensor_handler.cpp`)**:
* **超声波**: 独立测试前方和侧方超声波传感器，验证距离读数的准确性。
* **TCRT5000循迹**: 重点测试 `get_line_follow_values()`，手动移动传感器或机器人，观察 `left_on_line` 和 `right_on_line` 是否能正确反映传感器相对于引导线的位置。
* **继电器 (`relay_control.cpp`)**:
* 测试 `relay_start_charging()` 和 `relay_stop_charging()` 是否能正确控制继电器（以及与之关联的任何指示灯或设备）。
### 2.2. 导航逻辑调试 (`navigation.cpp`)
这是最复杂且耗时的部分：
* **循迹 (`navigation_follow_line_once`)**:
* 将机器人放在引导线上，逐步调试循迹逻辑。
* 从低速开始，逐步增加速度。
* 细致调整转向逻辑和参数，目标是让机器人能够平稳、准确地跟随线路。
* 处理各种情况：直线、弯道、可能的交叉点（如果场景中有）。
* **车位检测 (`navigation_check_spot_arrival`)**:
* 测试侧向超声波传感器检测充电桩（或模拟物）的逻辑。
* 校准 `SIDE_ULTRASONIC_SPOT_THRESHOLD_CM`。
* 测试 `SPOT_DEBOUNCE_TIME_MS` 去抖逻辑，确保路过一个桩时只计数一次。
* 验证 `detected_spot_count` 是否能正确累计。
* **避障 (`navigation_check_obstacle`, `navigation_handle_obstacle`)**:
* 测试前方超声波传感器检测障碍物的逻辑。
* 当前的 `navigation_handle_obstacle` 只是停止，您可以根据需求实现更复杂的避障策略（如后退、转向绕行等）。
* **完整导航 (`navigation_move_to_spot`)**:
* 测试从起点到指定目标充电桩的完整导航过程。
* 观察状态转换、车位计数、最终到达判断是否都符合预期。
* 测试导航超时逻辑 (`MAX_NAV_TIME_MS`)。
### 2.3. 状态同步与MQTT测试 (`mqtt_handler.cpp`, `esp32_robot_firmware.ino`)
* **连接**: 确保ESP32能成功连接WiFi和MQTT Broker。
* **指令接收**: 从后端（或使用MQTT客户端工具）发送指令（MOVE, STOP_CHARGE），检查 `handle_mqtt_command` 是否能正确解析并触发相应动作。
* **ACK上报**: 验证机器人是否能正确上报ACK消息，包括成功和失败的情况。
* **状态上报**:
* 监控机器人定期上报的状态消息。
* 验证 `current_robot_status`, `robot_current_location`, `robot_battery_level` 等字段是否准确。
* 特别关注 `CHARGING` 状态下的 `durationSeconds` 和 `spotId`，以及 `FAULTED` 状态下的 `errorCode` 和 `message`。
* **心跳**: 检查心跳消息是否按预期发送。
### 2.4. 整体联调与鲁棒性测试
* 将所有模块集成起来，进行完整的业务流程测试：
* 接收移动指令 -> 移动到车位 -> 到达后自动开始充电 -> 状态更新 -> 接收停止充电指令 -> 停止充电 -> 状态更新。
* 测试各种边界条件和异常情况：
* WiFi/MQTT断开重连。
* 导航中途遇到障碍。
* 目标车位不存在或无法到达。
* 电池电量模拟变化是否合理。
* 长时间运行测试，观察系统稳定性和资源占用情况（虽然在ESP32上较难直接监控内存）。
## 3. 代码结构与进一步优化建议
* **错误处理**: 当前的错误处理还比较基础，可以考虑更细致的错误码和错误信息。
* **状态机完善**: `esp32_robot_firmware.ino` 中的主循环可以进一步优化为更清晰的状态机。
* **非阻塞操作**: `navigation_move_to_spot` 目前是阻塞函数。对于更复杂的应用，可以考虑将其改造为非阻塞的，通过在 `loop()` 中轮询其状态来执行。但这会显著增加复杂性。
* **代码注释和可读性**: 在您修改和添加代码时，保持良好的注释习惯。
祝您项目顺利！

67
esp32_robot_firmware/config.h Normal file
View File

@@ -0,0 +1,67 @@
#ifndef CONFIG_H
#define CONFIG_H
// ----------- WiFi 配置 -----------
#define WIFI_SSID "YOUR_WIFI_SSID"
#define WIFI_PASSWORD "YOUR_WIFI_PASSWORD"
// ----------- MQTT Broker 配置 -----------
#define MQTT_BROKER_HOST "broker.emqx.io"
#define MQTT_BROKER_PORT 1883
#define MQTT_USERNAME "emqx" // 如果MQTT Broker需要认证，请填写
#define MQTT_PASSWORD "public" // 如果MQTT Broker需要认证，请填写
// ----------- 设备唯一标识符 -----------
// 这个ID必须与后端系统中注册的设备ID一致
#define DEVICE_UID "ESP32_ROBOT_001" // 请根据实际情况修改
// ----------- MQTT 主题动态部分 -----------
// 完整主题将在 mqtt_handler.cpp 中基于 DEVICE_UID 构建
#define MQTT_TOPIC_BASE "yupi_mqtt_power_project/robot"
// ----------- 硬件引脚定义 -----------
// --- 电机驱动 (例如使用 L298N 或类似模块) ---
#define MOTOR_LEFT_IN1_PIN 0 // 左轮电机正转控制引脚
#define MOTOR_LEFT_IN2_PIN 0 // 左轮电机反转控制引脚
#define MOTOR_LEFT_ENA_PIN 0 // 左轮电机PWM调速引脚
#define MOTOR_RIGHT_IN3_PIN 0 // 右轮电机正转控制引脚
#define MOTOR_RIGHT_IN4_PIN 0 // 右轮电机反转控制引脚
#define MOTOR_RIGHT_ENB_PIN 0 // 右轮电机PWM调速引脚
// --- 舵机控制 ---
#define SERVO_PIN 0 // 舵机信号引脚
// --- 超声波传感器 ---
#define ULTRASONIC_FRONT_TRIG_PIN 0 // 前方超声波 Trig 引脚
#define ULTRASONIC_FRONT_ECHO_PIN 0 // 前方超声波 Echo 引脚
#define ULTRASONIC_SIDE_TRIG_PIN 0 // 侧方超声波 Trig 引脚
#define ULTRASONIC_SIDE_ECHO_PIN 0 // 侧方超声波 Echo 引脚
// --- TCRT5000 循迹传感器 ---
#define TCRT5000_LEFT_PIN 0 // 左循迹传感器模拟/数字输出引脚
#define TCRT5000_RIGHT_PIN 0 // 右循迹传感器模拟/数字输出引脚
// (如果使用更多循迹传感器，继续添加定义)
// --- 继电器控制 ---
#define RELAY_PIN 0 // 控制充电继电器的引脚
// ----------- 业务逻辑参数 -----------
#define SIDE_ULTRASONIC_SPOT_THRESHOLD_CM 60 // 侧向超声波检测到充电桩的距离阈值 (cm)
#define NUM_CHARGING_SPOTS 3 // 系统中定义的充电桩数量
// 充电位置ID定义 (与后端协商或在此固定)
// 这些将用于 spot counting 和与 target_spot_uid 匹配
// 示例：如果后端发送 "001", "002", "003"
#define SPOT_ID_1 "001"
#define SPOT_ID_2 "002"
#define SPOT_ID_3 "003"
// ----------- 定时器与延时 -----------
#define STATUS_UPDATE_INTERVAL_MS 30000 // 状态上报间隔 (毫秒)
#define HEARTBEAT_INTERVAL_MS 60000 // 心跳间隔 (毫秒)
#define BATTERY_UPDATE_INTERVAL_MS 5000 // 电池模拟更新间隔 (毫秒)
#endif // CONFIG_H

176
esp32_robot_firmware/esp32_robot_firmware.ino Normal file
View File

@@ -0,0 +1,176 @@
#include "config.h"
#include "mqtt_handler.h"
#include "motor_control.h"
#include "sensor_handler.h"
#include "navigation.h"
#include "relay_control.h"
// ----------- 全局状态变量 -----------
RobotStatus current_robot_status = IDLE;
String robot_current_location = "BASE_STATION"; // 初始位置
int robot_battery_level = 95; // 初始电量
unsigned long robot_charge_start_time_ms = 0; // 充电开始时间戳
String current_active_task_id = ""; // 当前执行的后端任务ID
String target_spot_for_move = ""; // MOVE指令的目标车位
// 定时器相关
unsigned long last_status_publish_time = 0;
unsigned long last_heartbeat_time = 0; // 心跳可以使用简化的状态更新
unsigned long last_battery_sim_time = 0;
// --- MQTT 指令回调函数 ---
void handle_mqtt_command(const char* commandType, const char* taskId, const char* targetSpotId) {
Serial.print("主程序收到指令回调: commandType="); Serial.print(commandType);
Serial.print(", taskId="); Serial.print(taskId);
if (targetSpotId) { Serial.print(", targetSpotId="); Serial.print(targetSpotId); }
Serial.println();
current_active_task_id = String(taskId);
if (strcmp(commandType, "MOVE") == 0 || strcmp(commandType, "MOVE_TO_SPOT") == 0) {
if (targetSpotId && strlen(targetSpotId) > 0) {
if (current_robot_status == IDLE || current_robot_status == COMPLETED || current_robot_status == FAULTED) {
Serial.print("指令有效: 准备移动到 "); Serial.println(targetSpotId);
target_spot_for_move = String(targetSpotId);
// 状态将在navigation_move_to_spot开始时变为MOVING
// navigation_move_to_spot会阻塞，直到完成、失败或超时
} else {
Serial.println("指令无效: 机器人当前不处于可接收MOVE指令的状态。");
mqtt_publish_ack(taskId, false, "Robot busy or not in correct state for MOVE", current_robot_status, nullptr);
current_active_task_id = "";
}
} else {
Serial.println("指令错误: MOVE 指令缺少 targetSpotId/target_spot_uid。");
mqtt_publish_ack(taskId, false, "MOVE command missing target spot ID", current_robot_status, nullptr);
current_active_task_id = "";
}
} else if (strcmp(commandType, "STOP_CHARGE") == 0) {
if (current_robot_status == CHARGING) {
Serial.println("指令有效: 停止充电。");
current_robot_status = COMPLETED; // 立即改变状态
relay_stop_charging();
unsigned long charge_duration = (robot_charge_start_time_ms > 0) ? (millis() - robot_charge_start_time_ms) / 1000 : 0;
robot_charge_start_time_ms = 0;
robot_current_location = navigation_get_current_spot_count() > 0 ? String(SPOT_IDS[navigation_get_current_spot_count()-1]) : robot_current_location; // 假设停在最后一个检测到的桩
mqtt_publish_ack(taskId, true, ("Charging stopped. Duration: " + String(charge_duration) + "s").c_str(), current_robot_status, robot_current_location.c_str());
// 立即发布一次状态更新
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, nullptr, nullptr, robot_current_location.c_str(), 0, 0, nullptr);
} else {
Serial.println("指令无效: 机器人未在充电状态。");
mqtt_publish_ack(taskId, false, "Robot not in CHARGING state for STOP_CHARGE", current_robot_status, nullptr);
}
current_active_task_id = ""; // STOP_CHARGE任务通常是瞬时完成
} else if (strcmp(commandType, "START_CHARGE") == 0) { // 保留旧指令处理的ACK，但业务上已忽略
Serial.println("收到 START_CHARGE 指令，但业务逻辑为到达即充，仅ACK。");
mqtt_publish_ack(taskId, true, "START_CHARGE acknowledged (auto-charge on arrival)", current_robot_status, robot_current_location.c_str());
current_active_task_id = "";
} else {
Serial.print("未知指令类型: "); Serial.println(commandType);
mqtt_publish_ack(taskId, false, "Unknown command type", current_robot_status, nullptr);
current_active_task_id = "";
}
}
void simulate_battery_update(){
if (current_robot_status == CHARGING) {
if (robot_battery_level < 100) robot_battery_level += 1;
else robot_battery_level = 100;
} else if (current_robot_status == MOVING) {
if (robot_battery_level > 5) robot_battery_level -=2;
else robot_battery_level = 5;
} else { // IDLE, COMPLETED, FAULTED - very slow drain or none
// if (robot_battery_level > 1) robot_battery_level -=1; // Optional very slow drain
}
}
void setup() {
Serial.begin(115200);
Serial.println("\nESP32 真实机器人固件启动中...");
motor_setup();
sensor_setup();
relay_setup();
navigation_setup(); // 初始化导航状态和计数器
mqtt_setup(handle_mqtt_command); // 设置MQTT并传入指令回调
Serial.println("所有模块初始化完成。");
// 初始上报一次状态
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, nullptr, nullptr, nullptr, 0, 0, nullptr);
last_status_publish_time = millis();
last_heartbeat_time = millis();
last_battery_sim_time = millis();
}
void loop() {
mqtt_loop(); // 处理MQTT连接和消息
unsigned long current_time = millis();
// --- 状态机和任务处理 ---
if (target_spot_for_move.length() > 0 && (current_robot_status == IDLE || current_robot_status == COMPLETED || current_robot_status == FAULTED) ) {
Serial.print("开始执行移动任务到: "); Serial.println(target_spot_for_move);
bool move_success = navigation_move_to_spot(target_spot_for_move.c_str(), &current_robot_status, &robot_current_location);
if (move_success) {
Serial.println("移动成功，准备充电。");
current_robot_status = CHARGING; // 到达即充电
robot_charge_start_time_ms = millis();
relay_start_charging();
mqtt_publish_ack(current_active_task_id.c_str(), true, "Robot arrived and started charging.", current_robot_status, robot_current_location.c_str());
} else {
Serial.println("移动失败或超时。");
// current_robot_status 已经在 navigation_move_to_spot 中被设为 FAULTED (如果超时)
// 如果不是超时而是其他导航错误，可能需要在这里显式设置为FAULTED
if(current_robot_status != FAULTED) current_robot_status = FAULTED;
mqtt_publish_ack(current_active_task_id.c_str(), false, "Failed to reach target spot.", current_robot_status, robot_current_location.c_str());
}
target_spot_for_move = ""; // 清除移动目标
current_active_task_id = ""; // 清除任务ID
// 立即上报一次状态
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, nullptr,
nullptr, // target_spot (已到达或失败)
(current_robot_status == CHARGING) ? robot_current_location.c_str() : nullptr, // current_spot
0, // duration (刚开始充电是0)
(current_robot_status == FAULTED) ? 1 : 0, // errorCode
(current_robot_status == FAULTED) ? "Navigation failed" : nullptr // errorMessage
);
}
// --- 定期任务 ---
// 状态上报
if (current_time - last_status_publish_time > STATUS_UPDATE_INTERVAL_MS) {
unsigned long charge_duration = 0;
if (current_robot_status == CHARGING && robot_charge_start_time_ms > 0) {
charge_duration = (millis() - robot_charge_start_time_ms) / 1000;
}
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level,
nullptr, // current_active_task_id (常规状态不上报任务ID)
nullptr, // target_spot (常规状态下无)
(current_robot_status == CHARGING || current_robot_status == COMPLETED) ? robot_current_location.c_str() : nullptr, // current_spot
charge_duration,
(current_robot_status == FAULTED) ? 1 : 0, // errorCode for FAULTED state
(current_robot_status == FAULTED) ? "General fault" : nullptr // errorMessage for FAULTED
);
last_status_publish_time = current_time;
}
// 心跳 (可以是一个简化的状态更新，或者特定心跳消息)
// 为简单起见，这里的心跳就是一次常规状态更新，如果上面刚发过，则跳过
if (current_time - last_heartbeat_time > HEARTBEAT_INTERVAL_MS && (current_time - last_status_publish_time > 1000) /*避免过于频繁*/) {
// 重用上面的状态上报逻辑，或发送更简洁的心跳包
// mqtt_publish_status_update(...) with minimal fields for heartbeat
Serial.println("发送心跳 (通过常规状态更新)...");
// (与上面状态上报逻辑相同，所以如果时间接近，可以省略以减少冗余)
// 这里我们确保它至少比状态上报稀疏一点，或者如果状态刚上报就不发
last_heartbeat_time = current_time;
}
// 电池模拟更新
if (current_time - last_battery_sim_time > BATTERY_UPDATE_INTERVAL_MS) {
simulate_battery_update();
last_battery_sim_time = current_time;
// 注意：电池电量变化后，下一次常规状态上报时会发送更新后的值
}
delay(10); // 主循环延时，给ESP32其他核心任务一些时间
}

142
esp32_robot_firmware/motor_control.cpp Normal file
View File

@@ -0,0 +1,142 @@
#include "motor_control.h"
#if defined(ESP32)
Servo esp_servo; // 使用 ESP32Servo 库的 Servo 对象
#else
Servo standard_servo; // 使用标准 Servo 库的 Servo 对象
#endif
void motor_setup() {
// --- 电机引脚设置 ---
pinMode(MOTOR_LEFT_IN1_PIN, OUTPUT);
pinMode(MOTOR_LEFT_IN2_PIN, OUTPUT);
pinMode(MOTOR_RIGHT_IN3_PIN, OUTPUT);
pinMode(MOTOR_RIGHT_IN4_PIN, OUTPUT);
#if defined(ESP32)
// 配置 ESP32 LEDC PWM 通道
ledcSetup(PWM_CHANNEL_LEFT, PWM_FREQUENCY, PWM_RESOLUTION);
ledcSetup(PWM_CHANNEL_RIGHT, PWM_FREQUENCY, PWM_RESOLUTION);
// 将 LEDC 通道附加到电机 ENA/ENB 引脚
ledcAttachPin(MOTOR_LEFT_ENA_PIN, PWM_CHANNEL_LEFT);
ledcAttachPin(MOTOR_RIGHT_ENB_PIN, PWM_CHANNEL_RIGHT);
#else
// 对于非ESP32板，ENA/ENB引脚通常直接用 analogWrite 控制
pinMode(MOTOR_LEFT_ENA_PIN, OUTPUT);
pinMode(MOTOR_RIGHT_ENB_PIN, OUTPUT);
#endif
motor_stop(); // 初始状态停止电机
Serial.println("电机驱动已初始化。");
// --- 舵机初始化 ---
servo_attach();
servo_center(); // 舵机初始居中
Serial.println("舵机已初始化并居中。");
}
// --- 内部辅助函数设置单个电机速度和方向 ---
void set_motor_speed(char motor_char, int speed, bool forward) {
// speed: 0-255
int actual_speed = constrain(speed, 0, 255);
if (motor_char == 'L') { // 左电机
digitalWrite(MOTOR_LEFT_IN1_PIN, forward ? HIGH : LOW);
digitalWrite(MOTOR_LEFT_IN2_PIN, forward ? LOW : HIGH);
#if defined(ESP32)
ledcWrite(PWM_CHANNEL_LEFT, actual_speed);
#else
analogWrite(MOTOR_LEFT_ENA_PIN, actual_speed);
#endif
} else if (motor_char == 'R') { // 右电机
digitalWrite(MOTOR_RIGHT_IN3_PIN, forward ? HIGH : LOW);
digitalWrite(MOTOR_RIGHT_IN4_PIN, forward ? LOW : HIGH);
#if defined(ESP32)
ledcWrite(PWM_CHANNEL_RIGHT, actual_speed);
#else
analogWrite(MOTOR_RIGHT_ENB_PIN, actual_speed);
#endif
}
}
// --- 公开的电机控制函数 ---
void motor_move_forward(int speed) {
set_motor_speed('L', speed, true);
set_motor_speed('R', speed, true);
Serial.print("电机前进，速度: "); Serial.println(speed);
}
void motor_move_backward(int speed) {
set_motor_speed('L', speed, false);
set_motor_speed('R', speed, false);
Serial.print("电机后退，速度: "); Serial.println(speed);
}
void motor_stop() {
set_motor_speed('L', 0, true); // 速度为0，方向无所谓
set_motor_speed('R', 0, true);
Serial.println("电机停止。");
}
void motor_set_raw_speeds(int left_speed, int right_speed) {
bool left_forward = left_speed >= 0;
bool right_forward = right_speed >= 0;
set_motor_speed('L', abs(left_speed), left_forward);
set_motor_speed('R', abs(right_speed), right_forward);
Serial.print("设置原始速度 左:"); Serial.print(left_speed);
Serial.print(" 右:"); Serial.println(right_speed);
}
void motor_turn_left(int speed, float turn_intensity, int steer_angle) {
servo_set_angle(steer_angle);
int outer_speed = speed;
int inner_speed = speed * (1.0 - constrain(turn_intensity, 0.0, 1.0));
motor_set_raw_speeds(inner_speed, outer_speed);
Serial.print("左转: 基础速度="); Serial.print(speed);
Serial.print(", 强度="); Serial.print(turn_intensity);
Serial.print(", 舵机角度="); Serial.println(steer_angle);
}
void motor_turn_right(int speed, float turn_intensity, int steer_angle) {
servo_set_angle(steer_angle);
int outer_speed = speed;
int inner_speed = speed * (1.0 - constrain(turn_intensity, 0.0, 1.0));
motor_set_raw_speeds(outer_speed, inner_speed);
Serial.print("右转: 基础速度="); Serial.print(speed);
Serial.print(", 强度="); Serial.print(turn_intensity);
Serial.print(", 舵机角度="); Serial.println(steer_angle);
}
// --- 舵机控制函数 ---
void servo_attach() {
#if defined(ESP32)
esp_servo.attach(SERVO_PIN);
#else
standard_servo.attach(SERVO_PIN);
#endif
Serial.println("舵机已连接。");
}
void servo_set_angle(int angle) {
int constrained_angle = constrain(angle, 0, 180); // 标准舵机角度范围
#if defined(ESP32)
esp_servo.write(constrained_angle);
#else
standard_servo.write(constrained_angle);
#endif
// Serial.print("舵机角度设置为: "); Serial.println(constrained_angle);
}
void servo_center() {
servo_set_angle(90); // 假设90度是舵机中心位置
Serial.println("舵机已居中 (90度)。");
}
void servo_detach() {
#if defined(ESP32)
esp_servo.detach();
#else
standard_servo.detach();
#endif
Serial.println("舵机已断开连接。");
}

37
esp32_robot_firmware/motor_control.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef MOTOR_CONTROL_H
#define MOTOR_CONTROL_H
#include "config.h"
#include <Arduino.h>
// 如果使用ESP32的ledcWrite功能进行PWM调速
#if defined(ESP32)
#include <ESP32Servo.h>
#define PWM_CHANNEL_LEFT 0 // ESP32 LEDC通道 0-15
#define PWM_CHANNEL_RIGHT 1 // ESP32 LEDC通道
#define PWM_FREQUENCY 5000 // PWM频率 (Hz)
#define PWM_RESOLUTION 8 // PWM分辨率 (8位: 0-255)
#else // 其他板子可能使用 analogWrite
#include <Servo.h>
#endif
void motor_setup();
// 基本电机控制
void motor_move_forward(int speed); // speed: 0-255 for PWM
void motor_move_backward(int speed);
void motor_stop();
// 转向控制 (TT电机差速 + 舵机辅助)
// speed: 基础速度, turn_intensity: 转向强度/差速程度 (0-1.0), steer_angle: 舵机角度
void motor_turn_left(int speed, float turn_intensity, int steer_angle);
void motor_turn_right(int speed, float turn_intensity, int steer_angle);
void motor_set_raw_speeds(int left_speed, int right_speed); // 直接设置左右轮速度（可以是负数表示反转）
// 舵机控制
void servo_attach();
void servo_set_angle(int angle); // angle: 通常0-180度
void servo_center();
void servo_detach();
#endif // MOTOR_CONTROL_H

233
esp32_robot_firmware/mqtt_handler.cpp Normal file
View File

@@ -0,0 +1,233 @@
#include "mqtt_handler.h"
#include "config.h" // 确保config.h被包含以获取定义
#include <WiFi.h>
// ----------- 全局MQTT相关变量 -----------
WiFiClient espClient;
PubSubClient mqttClient(espClient);
CommandCallback global_command_callback = nullptr;
// 主题字符串
String topic_uplink_to_backend;
String topic_downlink_from_backend;
// 将RobotStatus枚举转换为字符串 (与后端DTO的actualRobotStatus对应)
const char* robotStatusToString(RobotStatus status) {
switch (status) {
case IDLE: return "IDLE";
case MOVING: return "MOVING";
case CHARGING: return "CHARGING";
case COMPLETED: return "COMPLETED";
case FAULTED: return "FAULTED"; // 或 "ERROR"
default: return "UNKNOWN";
}
}
void callback(char* topic, byte* payload, unsigned int length) {
Serial.print("消息抵达, 主题: ");
Serial.println(topic);
char message[length + 1];
memcpy(message, payload, length);
message[length] = '\0';
Serial.print("消息内容: ");
Serial.println(message);
if (String(topic) != topic_downlink_from_backend) {
Serial.println("消息非来自预期的指令主题，忽略。");
return;
}
StaticJsonDocument<256> doc;
DeserializationError error = deserializeJson(doc, message);
if (error) {
Serial.print("JSON 解析失败: ");
Serial.println(error.f_str());
// 可以在这里发送一个格式错误的ACK，但通常不建议对无法解析的taskId发ACK
return;
}
const char* cmdType = nullptr;
if (doc.containsKey("commandType")) {
cmdType = doc["commandType"];
} else if (doc.containsKey("command")) {
cmdType = doc["command"];
}
const char* taskId = doc["taskId"];
if (cmdType == nullptr || taskId == nullptr) {
Serial.println("指令JSON缺少 commandType/command 或 taskId 字段。");
// 尝试发送ACK，即使taskId可能为null，但通常最好有taskId
mqtt_publish_ack(taskId, false, "Command JSON invalid (missing commandType/command or taskId)", IDLE /*随便给个状态*/, nullptr);
return;
}
const char* targetSpot = nullptr;
if (doc.containsKey("spotId")) {
targetSpot = doc["spotId"].as<const char*>();
} else if (doc.containsKey("target_spot_uid")) {
targetSpot = doc["target_spot_uid"].as<const char*>();
}
Serial.printf("解析到指令: Type=%s, TaskID=%s, TargetSpot=%s\n", cmdType, taskId, targetSpot ? targetSpot : "N/A");
if (global_command_callback) {
global_command_callback(cmdType, taskId, targetSpot);
} else {
Serial.println("错误: 指令回调函数未设置!");
}
}
void connect_wifi() {
Serial.println("正在连接 Wi-Fi...");
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
Serial.println("\nWi-Fi 连接成功!");
Serial.print("IP 地址: ");
Serial.println(WiFi.localIP());
}
void reconnect_mqtt() {
while (!mqttClient.connected()) {
Serial.print("尝试连接 MQTT Broker: ");
Serial.println(MQTT_BROKER_HOST);
String clientId = "esp32-robot-" + String(DEVICE_UID) + "-" + String(random(0xffff), HEX);
Serial.printf("客户端 ID: %s \n", clientId.c_str());
if (mqttClient.connect(clientId.c_str(), MQTT_USERNAME, MQTT_PASSWORD)) {
Serial.println("MQTT Broker 连接成功!");
mqttClient.subscribe(topic_downlink_from_backend.c_str());
Serial.println("已订阅指令主题: " + topic_downlink_from_backend);
// 连接成功后可以发送一次初始状态，但这通常由主逻辑决定
} else {
Serial.print("连接失败, rc=");
Serial.print(mqttClient.state());
Serial.println(" 2秒后重试...");
delay(2000);
}
}
}
// ----------- Public Functions -----------
void mqtt_setup(CommandCallback cmd_callback) {
global_command_callback = cmd_callback;
// 构建主题字符串
String base_topic_path = MQTT_TOPIC_BASE;
topic_uplink_to_backend = base_topic_path + "/status/" + String(DEVICE_UID);
topic_downlink_from_backend = base_topic_path + "/command/" + String(DEVICE_UID);
Serial.println("MQTT 主题初始化完成:");
Serial.println(" 上行主题: " + topic_uplink_to_backend);
Serial.println(" 下行主题: " + topic_downlink_from_backend);
connect_wifi();
mqttClient.setServer(MQTT_BROKER_HOST, MQTT_BROKER_PORT);
mqttClient.setCallback(callback);
}
void mqtt_loop() {
if (!mqttClient.connected()) {
reconnect_mqtt();
}
mqttClient.loop();
}
void mqtt_publish_status_update(
RobotStatus current_status,
const char* location,
int battery_level,
const char* current_task_id, // 这个字段在我们的状态消息中通常不直接用，但保留参数以备将来
const char* target_spot,
const char* current_spot,
unsigned long charge_duration_seconds,
int error_code,
const char* error_message
) {
StaticJsonDocument<512> doc;
doc["robotUid"] = DEVICE_UID;
doc["actualRobotStatus"] = robotStatusToString(current_status);
if (location) doc["location"] = location;
doc["battery"] = battery_level;
// 根据状态添加特定字段
switch (current_status) {
case MOVING:
if (target_spot) doc["targetSpot"] = target_spot;
break;
case CHARGING:
if (current_spot) doc["spotId"] = current_spot;
doc["durationSeconds"] = charge_duration_seconds;
break;
case COMPLETED:
if (current_spot) doc["spotId"] = current_spot;
// totalDurationSeconds 通常在特定完成事件的ACK中上报更准确
break;
case FAULTED:
doc["errorCode"] = error_code; // 仅在FAULTED状态下发送错误码
if (error_message) doc["message"] = error_message;
break;
default:
break;
}
// 注意：移除了常规状态更新中的errorCode，除非状态是FAULTED
String jsonBuffer;
serializeJson(doc, jsonBuffer);
Serial.print("发送 StatusUpdate 到主题 [");
Serial.print(topic_uplink_to_backend);
Serial.print("]: ");
Serial.println(jsonBuffer);
if (mqttClient.publish(topic_uplink_to_backend.c_str(), jsonBuffer.c_str())) {
Serial.println("StatusUpdate 发送成功");
} else {
Serial.println("StatusUpdate 发送失败");
}
}
void mqtt_publish_ack(
const char* task_id,
bool success,
const char* message,
RobotStatus current_robot_status_at_ack,
const char* ack_context_spot_id
) {
if (!task_id) {
Serial.println("无法发送ACK: taskId 为空");
return;
}
StaticJsonDocument<256> doc;
doc["robotUid"] = DEVICE_UID;
doc["taskId"] = task_id;
doc["status"] = success ? "SUCCESS" : "FAILURE";
if (message) doc["message"] = message;
doc["actualRobotStatus"] = robotStatusToString(current_robot_status_at_ack);
if (ack_context_spot_id) {
doc["spotId"] = ack_context_spot_id; // 在ACK上下文中关联spotId
}
// 根据之前的讨论，ACK中不发送errorCode字段
String jsonBuffer;
serializeJson(doc, jsonBuffer);
Serial.print("发送 ACK/TaskUpdate 到主题 [");
Serial.print(topic_uplink_to_backend);
Serial.print("]: ");
Serial.println(jsonBuffer);
if (mqttClient.publish(topic_uplink_to_backend.c_str(), jsonBuffer.c_str())) {
Serial.println("ACK/TaskUpdate 发送成功");
} else {
Serial.println("ACK/TaskUpdate 发送失败");
}
}
// 这个函数可以移除或重新设计，因为状态现在由主逻辑管理并通过参数传递给 publish 函数
// RobotStatus mqtt_get_current_robot_status_commanded() {
// return IDLE; // Placeholder
// }

46
esp32_robot_firmware/mqtt_handler.h Normal file
View File

@@ -0,0 +1,46 @@
#ifndef MQTT_HANDLER_H
#define MQTT_HANDLER_H
#include <PubSubClient.h>
#include <WiFiClient.h>
#include <ArduinoJson.h>
#include "config.h" // 引入配置文件以获取 MQTT 设置和设备ID
// 定义机器人可能的状态枚举，与后端和需求文档对齐
enum RobotStatus {
IDLE,
MOVING,
CHARGING,
COMPLETED,
FAULTED // 或 ERROR，根据后端实际使用的枚举值
};
// 回调函数指针类型，用于通知主程序收到了特定指令
// 参数: commandType, taskId, targetSpotId (如果指令是移动)
typedef void (*CommandCallback)(const char* commandType, const char* taskId, const char* targetSpotId);
void mqtt_setup(CommandCallback cmd_callback); // MQTT 初始化，传入指令回调函数
void mqtt_loop(); // 在主 loop 中调用，保持MQTT连接和处理消息
void mqtt_publish_status_update(
RobotStatus current_status,
const char* location,
int battery_level,
const char* current_task_id, // 后端任务的ID (如果正在执行)
const char* target_spot, // 仅在MOVING状态下
const char* current_spot, // 仅在CHARGING, COMPLETED状态下
unsigned long charge_duration_seconds, // 仅在CHARGING状态下
int error_code, // 仅在FAULTED状态下
const char* error_message // 仅在FAULTED状态下
); // 发布状态更新
void mqtt_publish_ack(
const char* task_id,
bool success,
const char* message,
RobotStatus current_robot_status_at_ack,
const char* ack_context_spot_id // 例如，MOVE_TO_SPOT完成后的spotId
); // 发布ACK消息
// 内部状态，供其他模块查询 (可选，或者通过回调传递)
RobotStatus mqtt_get_current_robot_status_commanded(); // 获取后端指令可能影响的最新状态
#endif // MQTT_HANDLER_H

242
esp32_robot_firmware/navigation.cpp Normal file
View File

@@ -0,0 +1,242 @@
#include "navigation.h"
#include <Arduino.h>
// ----------- 内部状态变量 -----------
static NavigationState current_nav_state = NAV_IDLE;
static int detected_spot_count = 0;
static bool side_spot_debounce = false; // 用于侧向超声波检测去抖
static unsigned long last_side_spot_detect_time = 0;
const unsigned long SPOT_DEBOUNCE_TIME_MS = 2000; // 2秒去抖时间，避免重复计数同一个桩
// 循迹PID相关 (如果使用)
static float pid_error = 0;
static float pid_last_error = 0;
static float pid_integral = 0;
static float pid_derivative = 0;
// 目标车位ID的映射 (从config.h中获取)
const char* SPOT_IDS[NUM_CHARGING_SPOTS] = {SPOT_ID_1, SPOT_ID_2, SPOT_ID_3};
// 将字符串的 spot_id (如 "001") 转换为索引 (0, 1, 2)
int get_spot_index(const char* target_spot_uid) {
if (!target_spot_uid) return -1;
for (int i = 0; i < NUM_CHARGING_SPOTS; ++i) {
if (strcmp(target_spot_uid, SPOT_IDS[i]) == 0) {
return i;
}
}
return -1; // 未找到
}
void navigation_setup() {
current_nav_state = NAV_IDLE;
detected_spot_count = 0;
side_spot_debounce = false;
// 初始化PID变量 (如果使用)
pid_error = 0; pid_last_error = 0; pid_integral = 0; pid_derivative = 0;
Serial.println("导航系统已初始化。");
}
void navigation_reset_spot_counter() {
detected_spot_count = 0;
side_spot_debounce = false;
Serial.println("车位计数器已重置。");
}
int navigation_get_current_spot_count() {
return detected_spot_count;
}
NavigationState navigation_get_state() {
return current_nav_state;
}
void navigation_follow_line_once() {
LineFollowValues line_values = get_line_follow_values();
int base_speed = 150; // 基础循迹速度，需要调整
int turn_speed_diff = 80; // 转向时的速度差或舵机调整量，需要调整
int servo_turn_angle_small = 15; // 小幅调整舵机角度 (例如10-20度)
int servo_turn_angle_large = 30; // 大幅调整舵机角度
// 这是一个非常基础的循迹逻辑，需要根据实际传感器和机器人进行大幅调整
// 您可能需要更复杂的PID控制或其他算法
if (line_values.left_on_line && line_values.right_on_line) {
// 1. 都在线上 (或者在两条线的中间)，直行
servo_center();
motor_move_forward(base_speed);
// Serial.println("Nav: Straight");
} else if (line_values.left_on_line && !line_values.right_on_line) {
// 2. 左边在线，右边不在：车体偏右，需要向左调整
// motor_set_raw_speeds(base_speed - turn_speed_diff, base_speed + turn_speed_diff); // 左轮慢，右轮快
servo_set_angle(90 - servo_turn_angle_small); // 舵机向左
motor_move_forward(base_speed * 0.8); // 调整时可以稍微减速
// Serial.println("Nav: Turn Left (correcting rightward drift)");
} else if (!line_values.left_on_line && line_values.right_on_line) {
// 3. 右边在线，左边不在：车体偏左，需要向右调整
// motor_set_raw_speeds(base_speed + turn_speed_diff, base_speed - turn_speed_diff); // 左轮快，右轮慢
servo_set_angle(90 + servo_turn_angle_small); // 舵机向右
motor_move_forward(base_speed * 0.8);
// Serial.println("Nav: Turn Right (correcting leftward drift)");
} else {
// 4. 两边都不在线：可能偏离太远，或者到达线路末端/交叉口
// 简单处理：停止或尝试最后的方向转动
motor_stop(); // 或者执行更复杂的搜索线路逻辑
Serial.println("Nav: Both sensors off line! Stopping.");
// current_nav_state = NAV_ERROR; // 可以设置错误状态
}
// PID循迹逻辑 (示例，未完全集成)
/*
if (line_values.left_on_line && !line_values.right_on_line) pid_error = -1; // 偏右
else if (!line_values.left_on_line && line_values.right_on_line) pid_error = 1; // 偏左
else if (line_values.left_on_line && line_values.right_on_line) pid_error = 0; // 居中
else { // 都脱离，根据上次误差方向大幅调整或停车
pid_error = pid_last_error * 2; // 放大上次误差
}
pid_integral += pid_error;
pid_derivative = pid_error - pid_last_error;
float motor_adjustment = KP * pid_error + KI * pid_integral + KD * pid_derivative;
pid_last_error = pid_error;
int left_motor_speed = base_speed - motor_adjustment;
int right_motor_speed = base_speed + motor_adjustment;
servo_set_angle(90 - (int)(motor_adjustment * 10)); // 假设舵机调整与motor_adjustment相关
motor_set_raw_speeds(left_motor_speed, right_motor_speed);
*/
}
bool navigation_check_obstacle() {
long dist = get_front_obstacle_distance();
if (dist > 0 && dist < 20) { // 假设前方小于20cm为障碍物
Serial.print("Nav: Obstacle detected at "); Serial.print(dist); Serial.println(" cm");
return true;
}
return false;
}
void navigation_handle_obstacle() {
Serial.println("Nav: Handling obstacle...");
motor_stop();
// 简单处理：可以尝试后退一点，然后再次尝试，或者等待一段时间
// 更复杂的避障逻辑（如转向绕过）需要更多传感器和算法
// For now, just stop and set state to allow intervention or timeout
current_nav_state = NAV_AVOIDING_OBSTACLE; // 更新导航内部状态
// 在实际应用中，这里可能需要通知主循环机器人状态为FAULTED或需要帮助
}
// 返回true表示到达目标，false表示未到达或已过目标
bool navigation_check_spot_arrival(const char* target_spot_uid) {
long side_dist = get_side_spot_distance();
int target_spot_index = get_spot_index(target_spot_uid);
if (target_spot_index < 0) {
Serial.print("Nav Error: Invalid target_spot_uid for arrival check: "); Serial.println(target_spot_uid);
return false; // 无效目标
}
if (side_dist > 0 && side_dist < SIDE_ULTRASONIC_SPOT_THRESHOLD_CM) {
if (!side_spot_debounce) {
detected_spot_count++;
side_spot_debounce = true; // 进入去抖状态
last_side_spot_detect_time = millis();
Serial.print("Nav: Detected spot candidate. Count: "); Serial.println(detected_spot_count);
// 检查是否到达了目标充电桩 (目标是第 target_spot_index + 1 个桩)
if (detected_spot_count == (target_spot_index + 1)) {
Serial.print("Nav: TARGET SPOT ["
Serial.println("Nav: TARGET SPOT [" + String(target_spot_uid) + "] REACHED (Count matched)!");
return true; // 到达目标
}
}
} else {
// 如果距离大于阈值，且距离上次检测到满足条件已经超过去抖时间，则解除去抖
if (side_spot_debounce && (millis() - last_side_spot_detect_time > SPOT_DEBOUNCE_TIME_MS)) {
side_spot_debounce = false;
Serial.println("Nav: Side spot debounce period ended.");
}
}
// 如果已经计数的桩超过了目标桩的索引+1，说明可能错过了，或者目标逻辑有问题
if (detected_spot_count > (target_spot_index + 1)) {
Serial.println("Nav Warning: Detected spot count exceeds target. Possible overshoot or error.");
// current_nav_state = NAV_ERROR; // 可以设置为错误状态
// return false; // 或者根据业务逻辑，这里也视为一种失败
}
return false; // 未到达目标
}
bool navigation_move_to_spot(const char* target_spot_uid, RobotStatus* current_robot_status_ptr, String* current_location_ptr) {
if (!target_spot_uid || !current_robot_status_ptr || !current_location_ptr) {
Serial.println("Nav Error: Invalid arguments to navigation_move_to_spot.");
return false;
}
Serial.print("Nav: Starting move_to_spot for target: "); Serial.println(target_spot_uid);
*current_robot_status_ptr = MOVING;
*current_location_ptr = "EN_ROUTE_TO_" + String(target_spot_uid);
current_nav_state = NAV_FOLLOWING_LINE;
// navigation_reset_spot_counter(); // 在开始新的导航任务时重置计数器
// 注意: 重置计数器的时机很重要。如果机器人从任意位置启动，则应重置。
// 如果是连续任务的一部分，则可能不需要。
// 假设从基站出发或新任务开始，这里重置。
// 但如果是在多个桩之间移动，这个逻辑需要调整。
// 为了简单，我们假设每次MOVE指令都是一个新的完整寻路。
if (detected_spot_count != 0) { // 如果不是从0开始，说明上次任务可能未清理或逻辑错误
Serial.println("Nav Warning: Spot counter not zero at start of new navigation. Resetting.");
navigation_reset_spot_counter();
}
unsigned long nav_start_time = millis();
unsigned long MAX_NAV_TIME_MS = 120000; // 例如，最大导航时间2分钟，防止卡死
while (current_nav_state != NAV_TARGET_REACHED && current_nav_state != NAV_ERROR) {
if (millis() - nav_start_time > MAX_NAV_TIME_MS) {
Serial.println("Nav Error: Navigation timeout!");
motor_stop();
current_nav_state = NAV_ERROR;
*current_robot_status_ptr = FAULTED; // 更新主状态
*current_location_ptr = "LOST_OR_TIMEOUT";
break;
}
if (navigation_check_obstacle()) {
navigation_handle_obstacle();
*current_robot_status_ptr = MOVING; // 仍然是移动中，但可能需要人工介入或等待
*current_location_ptr = "OBSTACLE_AVOIDANCE";
// 在简单避障后，可能需要重新进入 NAV_FOLLOWING_LINE 状态
// motor_stop(); // 确保在避障时机器人停止，或执行避障动作
// delay(1000); // 等待
// current_nav_state = NAV_FOLLOWING_LINE; // 尝试继续 (这部分逻辑需要完善)
// 目前简单处理是停止，并依赖外部超时或指令取消
Serial.println("Nav: Obstacle detected, stopping and awaiting further logic/timeout.");
motor_stop();
// break; // 或者跳出循环，让外部处理
// 暂不break，允许其他状态检查或MQTT消息进来中断
}
if (current_nav_state == NAV_FOLLOWING_LINE || current_nav_state == NAV_AT_SPOT_CANDIDATE) {
navigation_follow_line_once();
if (navigation_check_spot_arrival(target_spot_uid)) {
current_nav_state = NAV_TARGET_REACHED;
*current_location_ptr = String(target_spot_uid); // 更新位置为目标车位
}
}
// (这里可以加入MQTT消息的检查，如果收到STOP指令，可以中断导航)
// mqtt_loop(); // 不建议在阻塞函数中频繁调用mqtt_loop，应由主循环处理
delay(50); // 短暂延时，控制循环频率
}
motor_stop(); // 确保在函数结束时电机停止
if (current_nav_state == NAV_TARGET_REACHED) {
Serial.print("Nav: Successfully reached target "); Serial.println(target_spot_uid);
return true;
} else {
Serial.print("Nav: Failed to reach target "); Serial.println(target_spot_uid);
return false;
}
}

45
esp32_robot_firmware/navigation.h Normal file
View File

@@ -0,0 +1,45 @@
#ifndef NAVIGATION_H
#define NAVIGATION_H
#include "config.h"
#include "motor_control.h"
#include "sensor_handler.h"
#include "mqtt_handler.h" // 可能需要访问机器人状态定义
// 循迹PID控制参数 (如果使用PID)
#define KP 0.5 // 比例常数
#define KI 0.1 // 积分常数
#define KD 0.2 // 微分常数
// 导航状态，用于 moveToSpot 函数的内部状态管理
enum NavigationState {
NAV_IDLE,
NAV_FOLLOWING_LINE,
NAV_AVOIDING_OBSTACLE,
NAV_AT_SPOT_CANDIDATE, // 检测到侧方可能是充电桩
NAV_TARGET_REACHED,
NAV_ERROR
};
void navigation_setup();
// 主要的移动到指定车位函数
// target_spot_uid: 目标车位的ID (如 "001", "002")
// current_robot_status_ptr: 指向主程序中当前机器人状态的指针，用于更新
// returns: true 如果成功到达, false 如果失败或被中断
bool navigation_move_to_spot(const char* target_spot_uid, RobotStatus* current_robot_status_ptr, String* current_location_ptr);
// 内部辅助函数，也可以根据需要公开
void navigation_follow_line_once(); // 执行一次循迹逻辑并调整电机/舵机
bool navigation_check_obstacle(); // 检查前方障碍物
void navigation_handle_obstacle(); // 处理障碍物 (例如停止或简单避障)
bool navigation_check_spot_arrival(const char* target_spot_uid); // 检查是否到达目标车位
// 简单的自计数车位识别逻辑
void navigation_reset_spot_counter();
int navigation_get_current_spot_count();
// 获取当前导航模块内部状态（用于调试或复杂逻辑）
NavigationState navigation_get_state();
#endif // NAVIGATION_H

18
esp32_robot_firmware/relay_control.cpp Normal file
View File

@@ -0,0 +1,18 @@
#include "relay_control.h"
void relay_setup() {
pinMode(RELAY_PIN, OUTPUT);
digitalWrite(RELAY_PIN, LOW); // 初始状态确保继电器是断开的 (假设LOW是断开)
// 如果您的高低电平定义相反，请修改这里
Serial.println("继电器已初始化，初始状态：断开。");
}
void relay_start_charging() {
digitalWrite(RELAY_PIN, HIGH); // 假设HIGH是闭合继电器
Serial.println("继电器闭合 - 开始充电 (模拟)。");
}
void relay_stop_charging() {
digitalWrite(RELAY_PIN, LOW); // 假设LOW是断开继电器
Serial.println("继电器断开 - 停止充电 (模拟)。");
}

11
esp32_robot_firmware/relay_control.h Normal file
View File

@@ -0,0 +1,11 @@
#ifndef RELAY_CONTROL_H
#define RELAY_CONTROL_H
#include "config.h"
#include <Arduino.h>
void relay_setup();
void relay_start_charging(); // 闭合继电器，模拟开始充电
void relay_stop_charging(); //断开继电器，模拟停止充电
#endif // RELAY_CONTROL_H

74
esp32_robot_firmware/sensor_handler.cpp Normal file
View File

@@ -0,0 +1,74 @@
#include "sensor_handler.h"
void sensor_setup() {
// 超声波传感器引脚设置
pinMode(ULTRASONIC_FRONT_TRIG_PIN, OUTPUT);
pinMode(ULTRASONIC_FRONT_ECHO_PIN, INPUT);
pinMode(ULTRASONIC_SIDE_TRIG_PIN, OUTPUT);
pinMode(ULTRASONIC_SIDE_ECHO_PIN, INPUT);
Serial.println("超声波传感器引脚已初始化。");
// TCRT5000 循迹传感器引脚设置
// 如果TCRT5000模块的DOUT引脚直接给出高低电平，则设为INPUT
// 如果是AOUT引脚，需要用analogRead()，引脚模式通常不需要显式设置，但INPUT也可以
pinMode(TCRT5000_LEFT_PIN, INPUT);
pinMode(TCRT5000_RIGHT_PIN, INPUT);
Serial.println("TCRT5000循迹传感器引脚已初始化。");
}
// --- 超声波传感器函数 ---
long get_ultrasonic_distance_cm(int trig_pin, int echo_pin) {
digitalWrite(trig_pin, LOW);
delayMicroseconds(2);
digitalWrite(trig_pin, HIGH);
delayMicroseconds(10);
digitalWrite(trig_pin, LOW);
long duration = pulseIn(echo_pin, HIGH, 30000); // 等待脉冲返回，超时时间30ms (约5米)
// 声速为 340m/s 或 29.412 µs/cm (1 / (34000 / 1000000 / 2))
// (duration / 2) * 0.034 = duration * 0.017
// More precise: duration / 58.8 (for cm)
if (duration == 0) { // 超时或检测失败
return -1; // 返回-1表示无效读数或超出范围
}
return duration / 58; // Convert duration to cm
}
long get_front_obstacle_distance() {
return get_ultrasonic_distance_cm(ULTRASONIC_FRONT_TRIG_PIN, ULTRASONIC_FRONT_ECHO_PIN);
}
long get_side_spot_distance() {
return get_ultrasonic_distance_cm(ULTRASONIC_SIDE_TRIG_PIN, ULTRASONIC_SIDE_ECHO_PIN);
}
// --- TCRT5000 循迹传感器函数 ---
// 辅助函数：根据模拟值判断是否在线上
// 注意: TCRT5000输出特性可能不同，白色反射强->低电压/低模拟值，黑色吸收->高电压/高模拟值
// 或者相反，取决于模块设计。这里的THRESHOLD假设的是：检测到黑线时读数高于阈值。
// 您需要根据实际情况校准和调整逻辑。
bool is_on_line(int sensor_pin, int threshold) {
int sensor_value = analogRead(sensor_pin);
// Serial.print("Sensor pin "); Serial.print(sensor_pin);
// Serial.print(" value: "); Serial.println(sensor_value);
return sensor_value > threshold; // 示例：高于阈值是在黑线上
// 如果您的传感器是检测到黑线时值变小，则应为: return sensor_value < threshold;
}
LineFollowValues get_line_follow_values() {
LineFollowValues values;
// 如果TCRT5000的DOUT引脚直接连接到数字引脚，则使用digitalRead()
// values.left_on_line = digitalRead(TCRT5000_LEFT_PIN) == HIGH; // 假设HIGH表示在黑线上
// values.right_on_line = digitalRead(TCRT5000_RIGHT_PIN) == HIGH;
// 如果使用AOUT引脚并进行模拟比较：
values.left_on_line = is_on_line(TCRT5000_LEFT_PIN, TCRT5000_THRESHOLD);
values.right_on_line = is_on_line(TCRT5000_RIGHT_PIN, TCRT5000_THRESHOLD);
// 调试输出
// Serial.print("Line Follow: Left OnLine="); Serial.print(values.left_on_line);
// Serial.print(", Right OnLine="); Serial.println(values.right_on_line);
return values;
}

38
esp32_robot_firmware/sensor_handler.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef SENSOR_HANDLER_H
#define SENSOR_HANDLER_H
#include "config.h"
#include <Arduino.h>
// TCRT5000 循迹传感器阈值 (如果使用数字模式或需要软件比较)
// 这个值需要根据实际传感器和线路进行校准
#define TCRT5000_THRESHOLD 500 // 假设模拟读取值，高于此值为检测到线，低于则为背景
void sensor_setup();
// 超声波传感器
long get_ultrasonic_distance_cm(int trig_pin, int echo_pin);
long get_front_obstacle_distance(); // 获取前方障碍物距离 (cm)
long get_side_spot_distance(); // 获取侧方充电桩距离 (cm)
// TCRT5000 循迹传感器
// 返回值可以根据具体循迹算法设计，例如:
// 0: 均在线上或中间
// -1: 左边传感器在线上，车体偏右
// 1: 右边传感器在线上，车体偏左
// -2: 左边传感器出界
// 2: 右边传感器出界
// 或者直接返回左右传感器的原始值/数字值
struct LineFollowValues {
bool left_on_line;
bool right_on_line;
// 可以添加原始模拟值如果需要
// int raw_left;
// int raw_right;
};
LineFollowValues get_line_follow_values();
// 辅助函数，判断是否在线上 (基于模拟值和阈值)
bool is_on_line(int sensor_pin, int threshold = TCRT5000_THRESHOLD);
#endif // SENSOR_HANDLER_H

221
mqtt_esp32_client/mqtt_esp32_client.ino
View File

@@ -26,19 +26,26 @@ WiFiClient espClient;
PubSubClient client(espClient);
// 模拟硬件状态 (实际项目中需要从传感器或硬件逻辑获取)
const char* currentDeviceStatus = "IDLE"; // 设备当前状态: IDLE, CHARGING, FAULTED 等
const char* currentDeviceStatus = "IDLE"; // 设备当前状态: IDLE, CHARGING, COMPLETED, FAULTED 等
float currentVoltage = 220.0;
float currentCurrent = 0.0;
float currentPower = 0.0;
float currentEnergyConsumed = 0.0;
int currentErrorCode = 0;
String currentSessionId = ""; // 当前充电会话ID
String currentSimulatedLocation = "BASE_STATION"; // 新增：模拟当前位置
int currentSimulatedBattery = 95; // 新增：模拟当前电量
String currentTargetSpot = ""; // 新增：用于移动状态的目标车位
String currentSpotId = ""; // 新增：当前所在车位 (用于充电等状态)
unsigned long chargeStartTimeMillis = 0; // 新增：用于计算充电时长
// 定时发送相关
unsigned long lastStatusUpdateTime = 0;
unsigned long lastHeartbeatTime = 0;
const long statusUpdateInterval = 30000; // 状态上报间隔 (例如: 30秒)
const long heartbeatInterval = 60000; // 心跳间隔 (例如: 60秒)
unsigned long lastBatteryUpdateTime = 0; // 新增：用于模拟电池电量变化的时间戳
const long batteryUpdateInterval = 5000; // 新增：电池状态更新间隔（例如5秒）
void setup_mqtt_topics() {
String backend_status_base = "yupi_mqtt_power_project/robot/status/";
@@ -113,50 +120,102 @@ void callback(char *topic, byte *payload, unsigned int length) {
return;
}
const char* cmdType = doc["commandType"]; // 例如: "START_CHARGE", "STOP_CHARGE"
const char* taskId = doc["taskId"]; // 用于ACK
const char* cmdType = nullptr;
if (doc.containsKey("commandType")) {
cmdType = doc["commandType"];
} else if (doc.containsKey("command")) {
cmdType = doc["command"];
}
const char* taskId = doc["taskId"];
if (cmdType == nullptr || taskId == nullptr) {
Serial.println("指令JSON缺少 commandType 或 taskId 字段。");
publish_ack_message(taskId, false, "Command JSON invalid", nullptr); // 尝试ACK错误
Serial.println("指令JSON缺少 commandType/command 或 taskId 字段。");
publish_ack_message(taskId, false, "Command JSON invalid (missing commandType/command or taskId)", nullptr);
return;
}
if (strcmp(cmdType, "MOVE_TO_SPOT") == 0) {
Serial.println("接收到 [移动到车位] 指令");
// const char* targetSpotUid = doc["target_spot_uid"]; // 可选: 从payload中获取目标车位ID (如果存在且需要进一步处理)
// if (targetSpotUid) {
// Serial.println("目标车位UID: " + String(targetSpotUid));
// }
// 模拟机器人移动到指定位置的动作
Serial.println("模拟: 机器人正在移动到目标车位...");
delay(1000); // 模拟移动耗时 (缩短演示时间)
Serial.println("模拟: 机器人已到达目标车位。");
publish_ack_message(taskId, true, "Robot arrived at spot (simulated)", nullptr);
// 注意：此时设备状态 currentDeviceStatus 可以保持不变，或根据业务逻辑更新
// 例如: currentDeviceStatus = "IDLE_AT_SPOT";
// 如果需要，可以立即上报一次状态: publish_regular_status_update();
const char* spotIdFromCommand = nullptr;
if (doc.containsKey("spotId")) {
spotIdFromCommand = doc["spotId"].as<const char*>();
} else if (doc.containsKey("target_spot_uid")) {
spotIdFromCommand = doc["target_spot_uid"].as<const char*>();
}
if (strcmp(cmdType, "MOVE_TO_SPOT") == 0 || strcmp(cmdType, "MOVE") == 0) {
Serial.println("接收到 [移动] 指令 (MOVE_TO_SPOT 或 MOVE)");
if (spotIdFromCommand) {
currentTargetSpot = String(spotIdFromCommand);
currentSpotId = "";
} else {
currentTargetSpot = "UNKNOWN_SPOT";
}
currentDeviceStatus = "MOVING";
currentSimulatedLocation = "EN_ROUTE_TO_" + currentTargetSpot;
Serial.println("状态更新: MOVING (前往目标车位: " + currentTargetSpot + ")");
publish_status_update(false, nullptr, nullptr, nullptr, nullptr, nullptr);
Serial.println("模拟: 机器人正在移动到目标车位 " + currentTargetSpot + "...");
// 模拟移动过程中的电量消耗，更细致的可以在 loop 中做
if (currentSimulatedBattery > 10) currentSimulatedBattery -= 5; // 假设移动固定消耗一些电
else currentSimulatedBattery = 5; // 最低电量
delay(3000);
Serial.println("模拟: 机器人已到达目标车位 " + currentTargetSpot + "。");
currentDeviceStatus = "CHARGING";
currentSpotId = currentTargetSpot;
currentTargetSpot = "";
currentSimulatedLocation = currentSpotId;
chargeStartTimeMillis = millis();
Serial.println("状态更新: CHARGING (已到达目标车位 " + currentSpotId + "，视为开始充电)");
publish_ack_message(taskId, true, "Robot arrived at spot and started charging (simulated)", currentSpotId.c_str());
publish_status_update(false, nullptr, nullptr, nullptr, nullptr, nullptr);
} else if (strcmp(cmdType, "START_CHARGE") == 0) {
Serial.println("接收到 [启动充电] 指令");
currentDeviceStatus = "CHARGING";
if (strcmp(currentDeviceStatus, "CHARGING") != 0) { // 仅当未在充电时才响应
currentDeviceStatus = "CHARGING";
if (spotIdFromCommand) {
currentSpotId = String(spotIdFromCommand);
} else if (currentSpotId.length() == 0) {
currentSpotId = "DEFAULT_SPOT"; // 如果没有从指令获取且之前也没有，给个默认值
}
currentSimulatedLocation = currentSpotId;
chargeStartTimeMillis = millis();
Serial.println("状态更新: CHARGING (指令启动于 " + currentSpotId + ")");
} else {
Serial.println("设备已在充电中，忽略 START_CHARGE 指令。");
}
if (doc.containsKey("sessionId")) {
currentSessionId = String(doc["sessionId"].as<const char*>());
} else {
currentSessionId = ""; //确保没有sessionId时清空
currentSessionId = "";
}
Serial.println("模拟: 充电已启动。会话ID: " + currentSessionId);
Serial.println("模拟: 充电已启动。会话ID: " + currentSessionId + ", 车位: " + currentSpotId);
publish_ack_message(taskId, true, "Charging started successfully", currentSessionId.c_str());
publish_status_update(false, nullptr, nullptr, nullptr, nullptr, nullptr); // 立即更新状态
publish_status_update(false, nullptr, nullptr, nullptr, nullptr, nullptr);
} else if (strcmp(cmdType, "STOP_CHARGE") == 0) {
Serial.println("接收到 [停止充电] 指令");
bool wasCharging = strcmp(currentDeviceStatus, "CHARGING") == 0;
currentDeviceStatus = "COMPLETED";
Serial.println("模拟: 充电已停止。");
String previousSessionId = currentSessionId; // 保存一下，以防ACK需要
currentSimulatedLocation = currentSpotId;
unsigned long chargeDuration = 0;
if (chargeStartTimeMillis > 0 && wasCharging) {
chargeDuration = (millis() - chargeStartTimeMillis) / 1000;
}
chargeStartTimeMillis = 0;
Serial.println("模拟: 充电已停止。车位: " + currentSpotId + ", 本次充电时长约: " + String(chargeDuration) + "s");
String previousSessionId = currentSessionId;
currentSessionId = "";
publish_ack_message(taskId, true, "Charging stopped successfully", previousSessionId.c_str());
publish_status_update(false, nullptr, nullptr, nullptr, nullptr, nullptr); // 立即更新状态
// 在ACK中上报准确的充电时长，如果需要的话，可以通过修改 publish_ack_message 或在 message 字段中添加
// For now, the generic ACK is sent.
publish_ack_message(taskId, true, ("Charging stopped. Duration: " + String(chargeDuration) + "s").c_str(), previousSessionId.c_str());
publish_status_update(false, nullptr, nullptr, nullptr, nullptr, nullptr);
}
// Add other commandType handling here if needed, e.g., "QUERY_STATUS"
// else if (strcmp(cmdType, "QUERY_STATUS") == 0) {
@@ -196,26 +255,47 @@ void publish_status_update(bool isAckOrTaskUpdate, const char* ackTaskId, const
if (isAckOrTaskUpdate) {
if (ackTaskId) doc["taskId"] = ackTaskId;
if (ackStatus) doc["status"] = ackStatus; // e.g., "SUCCESS", "FAILURE" or task-specific status
if (ackStatus) doc["status"] = ackStatus;
if (ackMessage) doc["message"] = ackMessage;
if (ackErrorCode) doc["errorCode"] = ackErrorCode;
// actualRobotStatus should still be sent to reflect current state after ACK
// 根据用户要求，ACK中不发送errorCode
// if (ackErrorCode) doc["errorCode"] = ackErrorCode;
doc["actualRobotStatus"] = currentDeviceStatus;
if (ackSessionId && strlen(ackSessionId) > 0) doc["activeTaskId"] = ackSessionId; // Assuming activeTaskId can hold sessionId for context in ACKs
// Or, if RobotStatusMessage is extended for sessionId in future.
// For now, activeTaskId might be a way to correlate, or it might be ignored by backend for ACKs.
if (ackSessionId && strlen(ackSessionId) > 0) {
// For ACKs, if we have a sessionId or a spotId that's relevant for context, we can add it.
// Let's use "spotId" as suggested by requirements for charging related ACKs
if (strcmp(currentDeviceStatus, "CHARGING") == 0 || strcmp(currentDeviceStatus, "COMPLETED") == 0 || (ackMessage && strstr(ackMessage, "arrived"))) {
if(currentSpotId.length() > 0) doc["spotId"] = currentSpotId;
}
}
} else { // General status update / heartbeat
doc["actualRobotStatus"] = currentDeviceStatus;
doc["voltage"] = currentVoltage; // Example: Add these if backend expects them with general status
doc["current"] = currentCurrent;
doc["power"] = currentPower;
doc["energyConsumed"] = currentEnergyConsumed;
doc["errorCode"] = currentErrorCode; // General device error code
if (currentSessionId.length() > 0) {
// For general status, if a session is active, it might be relevant as activeTaskId
// This depends on how backend interprets activeTaskId outside of specific task ACKs.
doc["activeTaskId"] = currentSessionId; // Or a more generic field if RobotStatusMessage evolves
// Add fields as per requirements.md
doc["location"] = currentSimulatedLocation;
doc["battery"] = currentSimulatedBattery; // Example value, should be updated by a battery sim function
// doc["errorCode"] = currentErrorCode; // 根据用户要求，常规状态下不发送errorCode
// Fields specific to certain statuses
if (strcmp(currentDeviceStatus, "MOVING") == 0) {
if (currentTargetSpot.length() > 0) doc["targetSpot"] = currentTargetSpot;
} else if (strcmp(currentDeviceStatus, "CHARGING") == 0) {
if (currentSpotId.length() > 0) doc["spotId"] = currentSpotId;
if (chargeStartTimeMillis > 0) {
doc["durationSeconds"] = (millis() - chargeStartTimeMillis) / 1000;
} else {
doc["durationSeconds"] = 0;
}
} else if (strcmp(currentDeviceStatus, "COMPLETED") == 0) {
if (currentSpotId.length() > 0) doc["spotId"] = currentSpotId;
// totalDurationSeconds would typically be set upon actual completion event,
// for now, a regular status update might not have final total, or we can omit.
// Let's assume for now that if status is COMPLETED, we send a placeholder or last known duration.
// A more robust solution is needed for totalDurationSeconds.
// For now, publish_ack_message for STOP_CHARGE should probably carry the final duration.
} else if (strcmp(currentDeviceStatus, "ERROR") == 0) {
// errorCode is already included. message for error could be added if available.
// doc["message"] = "Simulated error description"; // if we have one
}
}
// Common fields (timestamp can be added by backend or here if NTP is used)
@@ -244,14 +324,17 @@ void publish_heartbeat() {
}
// Simplified ACK message function
void publish_ack_message(const char* taskId, bool success, const char* message, const char* sessionIdForAckContext) {
void publish_ack_message(const char* taskId, bool success, const char* message, const char* contextInfo) {
if (!taskId || strlen(taskId) == 0) {
Serial.println("无法发送ACK: taskId 为空");
// Potentially send a general error status if appropriate, but usually ACK needs a taskId
return;
}
// Use the main publish_status_update function formatted as an ACK
publish_status_update(true, taskId, success ? "SUCCESS" : "FAILURE", message, success ? "0" : "GENERAL_ERROR_ON_ACK", sessionIdForAckContext);
// For contextInfo, we can pass spotId if relevant, or sessionId if that's what backend expects for ACKs.
// The 'true' indicates it's an ACK.
// The ackErrorCode field in publish_status_update will be set to "SUCCESS_ACK" or "FAILURE_ACK"
// 根据用户要求，ACK中的errorCode也暂时简化或移除。如果保留，确保含义清晰。
publish_status_update(true, taskId, success ? "SUCCESS" : "FAILURE", message, nullptr, contextInfo);
}
void setup() {
@@ -293,4 +376,52 @@ void loop() {
currentPower = 0.0;
}
delay(100); // 短暂延时，避免loop过于频繁，给其他任务一点时间 (可选)
// 模拟电量消耗和位置变化 (更符合需求文档)
if (strcmp(currentDeviceStatus, "CHARGING") == 0) {
if (currentSimulatedBattery > 0) { // 充电时电量可以缓慢增加，或保持不变，取决于模拟逻辑
// currentSimulatedBattery = min(100, currentSimulatedBattery + 1); // 简单模拟充电增加
}
// durationSeconds is calculated in publish_status_update
} else if (strcmp(currentDeviceStatus, "MOVING") == 0) {
if (currentSimulatedBattery > 5) { // 移动时消耗电量
// currentSimulatedBattery--; // 简单模拟电量消耗
}
} else { // IDLE, COMPLETED, ERROR
//电量可能不变或缓慢消耗
}
// 简单模拟位置更新 (可以更复杂)
// currentSimulatedLocation = ... ; // 可以在特定事件中更新
// --- 动态模拟数据更新 ---
if (currentTime - lastBatteryUpdateTime > batteryUpdateInterval) {
if (strcmp(currentDeviceStatus, "CHARGING") == 0) {
if (currentSimulatedBattery < 100) {
currentSimulatedBattery += 1; // 每 batteryUpdateInterval 增加 1% 电量
Serial.println("模拟: 电量增加至 " + String(currentSimulatedBattery) + "%");
} else {
currentSimulatedBattery = 100; // 防止超过100
}
} else if (strcmp(currentDeviceStatus, "MOVING") == 0) {
if (currentSimulatedBattery > 2) {
currentSimulatedBattery -= 2; // 每 batteryUpdateInterval 消耗 2% 电量
Serial.println("模拟: 电量消耗至 " + String(currentSimulatedBattery) + "%");
} else {
currentSimulatedBattery = 2; // 防止低于2，极端情况
}
} else { // IDLE, COMPLETED, FAULTED
if (currentSimulatedBattery > 1) {
// 非常缓慢的自然消耗，或者不消耗
// currentSimulatedBattery -= 1;
}
}
lastBatteryUpdateTime = currentTime;
}
// 位置模拟: 通常在状态转换时（callback中）已经更新了主要位置。
// loop中可以添加更细致的移动中的位置更新，但目前保持简单，依赖callback中的设定。
// 例如: if (strcmp(currentDeviceStatus, "MOVING") == 0) { /* update location based on time/progress */ }
delay(100);
}

9
springboot-init-main/src/main/java/com/yupi/project/aop/MqttPublishLogAspect.java
View File

@@ -48,6 +48,7 @@ public class MqttPublishLogAspect {
String commandTypeArg = null;
String payloadJsonArg = null;
Long sessionIdArg = null;
Long associatedTaskIdArg = null;
Long taskIdForLog = null;
if (args.length >= 3) {
@@ -57,6 +58,9 @@ public class MqttPublishLogAspect {
if (args.length >= 4 && args[3] instanceof Long) {
sessionIdArg = (Long) args[3];
}
if (args.length >= 5 && args[4] instanceof Long) {
associatedTaskIdArg = (Long) args[4];
}
}
String topic = null;
@@ -82,7 +86,10 @@ public class MqttPublishLogAspect {
throw throwable;
}
if (robotIdArg != null && sessionIdArg !=null) {
if (associatedTaskIdArg != null) {
taskIdForLog = associatedTaskIdArg;
} else if (robotIdArg != null && sessionIdArg !=null) {
log.warn("associatedTaskIdArg was null in MqttPublishLogAspect for robot {}, session {}. Falling back to DB query for taskId.", robotIdArg, sessionIdArg);
RobotTask latestTask = robotTaskService.findLatestTaskByRobotIdAndSessionId(robotIdArg,sessionIdArg);
if(latestTask != null){
taskIdForLog = latestTask.getId();

4
springboot-init-main/src/main/java/com/yupi/project/service/MqttService.java
View File

@@ -14,10 +14,12 @@ public interface MqttService {
* @param commandType 指令类型
* @param payloadJson 指令的参数内容 (JSON格式字符串)。对于无参数指令，可以为 null 或空字符串。
* @param sessionId 关联的充电会话ID (可选, 主要用于充电相关指令)
* @param associatedTaskId (可选) 如果此指令关联到一个已由调用方创建的RobotTask，则传入其ID。
* 如果为null，MqttService内部可能会尝试创建新任务（取决于实现策略，但当前策略是外部创建）。
* @return 如果指令成功进入发送流程（任务已创建并尝试发布），返回 true；
* 如果机器人正忙或创建任务失败等原因导致无法发送，返回 false。
* @throws Exception 如果MQTT发布过程中发生异常 (例如 MqttException)
*/
boolean sendCommand(String robotId, CommandTypeEnum commandType, String payloadJson, Long sessionId) throws Exception;
boolean sendCommand(String robotId, CommandTypeEnum commandType, String payloadJson, Long sessionId, Long associatedTaskId) throws Exception;
}

9
springboot-init-main/src/main/java/com/yupi/project/service/RobotTaskService.java
View File

@@ -22,6 +22,15 @@ public interface RobotTaskService extends IService<RobotTask> {
*/
boolean hasPendingOrSentTask(String robotId);
/**
* 检查指定机器人是否有正在处理（PENDING 或 SENT）的任务, 排除指定的任务ID。
*
* @param robotId 机器人ID
* @param taskIdToExclude 要排除的任务ID (可以为 null)
* @return 如果有其他待处理或已发送任务，则返回 true，否则 false
*/
boolean hasOtherPendingOrSentTask(String robotId, Long taskIdToExclude);
/**
* 创建一个新的机器人指令任务。
*

40
springboot-init-main/src/main/java/com/yupi/project/service/impl/ChargingSessionServiceImpl.java
View File

@@ -153,9 +153,15 @@ public class ChargingSessionServiceImpl extends ServiceImpl<ChargingSessionMappe
// 5. 发送MQTT指令给机器人
try {
mqttService.sendCommand(assignedRobot.getRobotUid(), CommandTypeEnum.MOVE_TO_SPOT, moveToSpotPayload, moveTask.getId());
robotTaskService.markTaskAsSent(moveTask.getId(), new Date());
log.info("已向机器人 {} 发送移动指令 for session {}, 任务ID: {}", assignedRobot.getRobotUid(), sessionId, moveTask.getId());
boolean commandPublished = mqttService.sendCommand(assignedRobot.getRobotUid(), CommandTypeEnum.MOVE_TO_SPOT, moveToSpotPayload, sessionId, moveTask.getId());
if (!commandPublished) {
log.error("MQTT移动指令发送失败 (sendCommand返回false) for session {}, 任务ID: {}. 将任务标记为失败。", sessionId, moveTask.getId());
robotTaskService.markTaskAsFailed(moveTask.getId(), "MQTT 指令未成功发布 (sendCommand返回false)", new Date());
throw new BusinessException(ErrorCode.OPERATION_ERROR, "发送MQTT指令失败 (未发布)");
}
log.info("已成功请求向机器人 {} 发送移动指令 for session {}, 任务ID: {}", assignedRobot.getRobotUid(), sessionId, moveTask.getId());
} catch (Exception e) {
log.error("发送MQTT移动指令失败 for session {}, 任务ID: {}: ", sessionId, moveTask.getId(), e);
// 此处可能需要更复杂的错误处理，如重试、标记任务失败等
@@ -182,25 +188,29 @@ public class ChargingSessionServiceImpl extends ServiceImpl<ChargingSessionMappe
return false;
}
// 校验会话状态和关联的任务ID
// 原始状态应该是 ROBOT_ASSIGNED，表示移动任务已发出
if (!ChargingSessionStatusEnum.ROBOT_ASSIGNED.getValue().equals(session.getStatus()) ||
!robotTaskId.equals(session.getRelatedRobotTaskId())) {
log.warn("处理机器人到达事件失败，会话状态 ({}) 或任务ID ({}) 不匹配 (期望任务ID: {}) for session {}",
(session.getRelatedRobotTaskId() != null && !robotTaskId.equals(session.getRelatedRobotTaskId())) ) { // 确保关联的任务ID也匹配
log.warn("处理机器人到达事件失败 (或视为开始充电事件)，会话状态 ({}) 或任务ID ({}) 不匹配 (期望任务ID: {}) for session {}",
session.getStatus(), robotTaskId, session.getRelatedRobotTaskId(), sessionId);
return false;
}
session.setStatus(ChargingSessionStatusEnum.ROBOT_ARRIVED.getValue());
session.setRobotArrivalTime(new Date());
// 更新会话状态为 CHARGING_STARTED，并记录相关信息
session.setStatus(ChargingSessionStatusEnum.CHARGING_STARTED.getValue());
session.setRobotArrivalTime(new Date()); // 机器人到达时间 (即充电开始时间点)
session.setChargeStartTime(new Date()); // 明确记录充电开始时间
session.setUpdateTime(new Date());
// 接下来可以创建"开始充电"的机器人任务，或等待用户指令/自动开始
// 此处简化，认为到达后即可准备开始充电，相关任务由MQTT回调触发创建
boolean updated = this.updateById(session);
if(updated) {
log.info("机器人已到达，会话 {} 状态更新为 ROBOT_ARRIVED. 关联任务ID: {}", sessionId, robotTaskId);
// 可选：更新机器人状态为IDLE（在车位旁等待）或 CHARGING_READY
chargingRobotService.updateRobotStatus(session.getRobotUidSnapshot(), RobotStatusEnum.IDLE, null, null, null, new Date());
// 可选：更新车位状态为 CHARGING (如果机器人到达即代表车位被用于充电)
parkingSpotService.updateSpotStatus(session.getSpotUidSnapshot(), ParkingSpotStatusEnum.CHARGING, session.getId());
log.info("机器人已到达，会话 {} 状态更新为 CHARGING_STARTED (视为充电开始). 关联任务ID: {}", sessionId, robotTaskId);
// 更新机器人DB状态为 CHARGING
chargingRobotService.updateRobotStatus(session.getRobotUidSnapshot(), RobotStatusEnum.CHARGING, null, null, robotTaskId, new Date());
// 确保车位状态为 CHARGING (这一步之前已经有了，保持即可)
parkingSpotService.updateSpotStatus(session.getSpotUidSnapshot(), ParkingSpotStatusEnum.CHARGING, session.getId());
}
return updated;
}
@@ -543,7 +553,7 @@ public class ChargingSessionServiceImpl extends ServiceImpl<ChargingSessionMappe
boolean mqttSent = false;
try {
mqttSent = mqttService.sendCommand(session.getRobotUidSnapshot(), CommandTypeEnum.STOP_CHARGE, stopChargePayload, stopTask.getId());
mqttSent = mqttService.sendCommand(session.getRobotUidSnapshot(), CommandTypeEnum.STOP_CHARGE, stopChargePayload, sessionId, stopTask.getId());
} catch (Exception e) {
log.error("发送MQTT停止充电指令失败 for session {}, 任务ID: {}: {}", sessionId, stopTask.getId(), e.getMessage(), e);
robotTaskService.markTaskAsFailed(stopTask.getId(), "MQTT发送失败: " + e.getMessage(), new Date());

120
springboot-init-main/src/main/java/com/yupi/project/service/impl/MqttServiceImpl.java
View File

@@ -3,6 +3,7 @@ package com.yupi.project.service.impl;
import com.yupi.project.config.properties.MqttProperties;
import com.yupi.project.model.entity.RobotTask;
import com.yupi.project.model.enums.CommandTypeEnum;
import com.yupi.project.model.enums.RobotTaskStatusEnum;
import com.yupi.project.service.MqttService;
import com.yupi.project.service.RobotTaskService;
import lombok.RequiredArgsConstructor;
@@ -16,6 +17,9 @@ import org.springframework.transaction.annotation.Transactional;
import java.nio.charset.StandardCharsets;
import java.util.Date;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.node.ObjectNode;
import java.util.Map;
/**
* MQTT 服务实现类
@@ -28,14 +32,17 @@ public class MqttServiceImpl implements MqttService {
private final MqttClient mqttClient; // 由Spring从MqttConfig注入的MQTT客户端实例
private final MqttProperties mqttProperties; // MQTT相关的配置属性
private final RobotTaskService robotTaskService; // 机器人任务服务，用于创建和更新任务状态
private final ObjectMapper objectMapper; // Jackson ObjectMapper for JSON manipulation
// 构造函数注入依赖
public MqttServiceImpl(@Qualifier("mqttClientBean") MqttClient mqttClient,
MqttProperties mqttProperties,
RobotTaskService robotTaskService) {
RobotTaskService robotTaskService,
ObjectMapper objectMapper) {
this.mqttClient = mqttClient;
this.mqttProperties = mqttProperties;
this.robotTaskService = robotTaskService;
this.objectMapper = objectMapper;
}
/**
@@ -45,32 +52,93 @@ public class MqttServiceImpl implements MqttService {
* @param commandType 指令类型 (枚举)
* @param payloadJson 指令的JSON格式负载
* @param sessionId (可选) 与此指令关联的充电会话ID
* @param associatedTaskId (可选) 此指令关联的、已由调用方创建的RobotTask ID。
* @return 指令是否发送成功 (注意：这仅表示MQTT消息已发布，不代表设备已执行)
* @throws Exception 如果发送过程中发生MQTT异常或其他意外异常
*/
@Override
@Transactional(rollbackFor = Exception.class) // 如果MQTT发布失败或后续操作失败，确保事务回滚（例如任务创建）
public boolean sendCommand(String robotId, CommandTypeEnum commandType, String payloadJson, Long sessionId) throws Exception {
log.info("尝试向机器人 {} 发送指令 {}，负载: {}", commandType, robotId, payloadJson);
@Transactional(rollbackFor = Exception.class)
public boolean sendCommand(String robotId, CommandTypeEnum commandType, String payloadJson, Long sessionId, Long associatedTaskId) throws Exception {
log.info("尝试向机器人 {} 发送指令 {} (关联任务ID: {})，负载: {}", commandType, robotId, associatedTaskId == null ? "N/A" : associatedTaskId, payloadJson);
// 步骤1：首先检查机器人是否已有正在处理(PENDING)或已发送(SENT)的任务
if (robotTaskService.hasPendingOrSentTask(robotId)) {
// 特殊处理：STOP_CHARGE (停止充电) 指令具有高优先级，可以覆盖其他任务
if (associatedTaskId == null) {
log.error("MqttServiceImpl.sendCommand 错误: associatedTaskId 不能为 null。任务应由调用者预先创建。机器人 {}，指令 {}", robotId, commandType);
// 根据当前设计，associatedTaskId 是必需的，因为 MqttServiceImpl 不再负责创建任务。
throw new IllegalArgumentException("associatedTaskId cannot be null. Task must be pre-created by caller.");
}
// 步骤1：检查机器人是否已有 *其他* 正在处理(PENDING)或已发送(SENT)的任务
if (robotTaskService.hasOtherPendingOrSentTask(robotId, associatedTaskId)) {
if (CommandTypeEnum.STOP_CHARGE.equals(commandType)) {
log.info("机器人 {} 有正在处理的任务，但 STOP_CHARGE 是优先指令，继续执行。", robotId);
log.info("机器人 {} 有其他正在处理的任务，但 STOP_CHARGE 是优先指令，继续执行任务 {}。", robotId, associatedTaskId);
} else {
log.warn("机器人 {} 正忙 (存在PENDING或SENT状态的任务)，指令 {} 已中止。", robotId, commandType);
return false; // 机器人忙，不发送新指令 (除非是停止指令)
log.warn("机器人 {} 正忙 (存在其他PENDING或SENT状态的任务)，指令 {} (任务 {}) 已中止。", robotId, commandType, associatedTaskId);
// 注意：即使中止，预先创建的 associatedTaskId 任务仍然是 PENDING。调用者需要处理此情况。
return false;
}
}
// 步骤2：如果机器人不忙（或指令是STOP_CHARGE），再创建新任务
RobotTask task = robotTaskService.createTask(robotId, commandType, payloadJson, sessionId);
if (task == null || task.getId() == null) {
log.error("为机器人 {} 的指令 {} 创建RobotTask失败。发送操作中止。", commandType, robotId);
return false; // 任务创建失败，不继续发送
// 步骤2：获取预先创建的任务 (associatedTaskId)
RobotTask taskToProcess = robotTaskService.getById(associatedTaskId);
if (taskToProcess == null) {
log.error("为机器人 {} 的指令 {} 查找预创建的 RobotTask (ID: {}) 失败。发送操作中止。", commandType, robotId, associatedTaskId);
return false; // 任务未找到
}
log.info("已为机器人 {} 的指令 {} 创建 PENDING 状态的 RobotTask，任务ID: {}。", commandType, robotId, task.getId());
// 校验任务状态是否为 PENDING，因为后续会将其标记为 SENT
if (taskToProcess.getStatus() != RobotTaskStatusEnum.PENDING) {
log.warn("预创建的任务 {} (机器人 {}, 指令 {}) 状态为 {}，而非预期的 PENDING。可能已被处理或状态错误。发送操作中止。",
associatedTaskId, robotId, commandType, taskToProcess.getStatus());
return false;
}
log.info("处理预创建的 PENDING 状态 RobotTask ID: {}，机器人 {}，指令 {}。", associatedTaskId, robotId, commandType);
// <<<< START MODIFICATION: Add/Ensure taskId in payloadJson using associatedTaskId >>>>
String finalPayloadJson;
try {
// Parse the original payloadJson
// If payloadJson is null or empty, create a new JSON object
// Otherwise, parse it and add/overwrite the taskId
ObjectNode payloadNode;
if (payloadJson == null || payloadJson.trim().isEmpty()) {
payloadNode = objectMapper.createObjectNode();
} else {
// Check if it's a valid JSON, otherwise treat as non-JSON or handle error
try {
payloadNode = (ObjectNode) objectMapper.readTree(payloadJson);
} catch (Exception e) {
// If original payload is not a valid JSON, we might log a warning
// and create a new JSON object just for the taskId, or wrap the original string.
// For now, let's assume if it's not empty, it should be valid JSON.
// Or, simpler: create a Map, add taskId, then add all from original parsed map.
log.warn("Original payloadJson for robot {} command {} is not a valid JSON object: {}. Proceeding by creating a new JSON with taskId.", robotId, commandType, payloadJson, e);
// Fallback: create a new node. This might lose original non-JSON payload if that was intended.
// A safer approach for non-JSON or mixed payloads might be needed if that's a use case.
payloadNode = objectMapper.createObjectNode();
// If you expect non-JSON payloads and want to preserve them, this part needs more thought.
// For now, assuming command payloads are always JSON.
}
}
payloadNode.put("taskId", String.valueOf(associatedTaskId)); // NEW: use associatedTaskId
// For START_CHARGE, ensure sessionId from method argument is also in the payload if not already
// This is based on ESP32 code expecting sessionId in payload for START_CHARGE
if (CommandTypeEnum.START_CHARGE.equals(commandType) && sessionId != null) {
if (!payloadNode.has("sessionId") || payloadNode.get("sessionId") == null || payloadNode.get("sessionId").isNull()) {
payloadNode.put("sessionId", sessionId); // Add sessionId if not present
log.info("Added sessionId {} to START_CHARGE command payload for task {}.", sessionId, associatedTaskId);
}
}
finalPayloadJson = objectMapper.writeValueAsString(payloadNode);
} catch (Exception e) {
log.error("为机器人 {} 的指令 {} (任务ID: {}) 添加 taskId 到 payload 时发生JSON处理异常: {}", commandType, robotId, associatedTaskId, e.getMessage(), e);
// Mark task as failed because we cannot construct a valid payload
robotTaskService.markTaskAsFailed(associatedTaskId, "Payload JSON processing error: " + e.getMessage(), new Date());
return false; // Payload construction failed
}
// <<<< END MODIFICATION >>>>
// 3. 构建目标MQTT主题
// 主题格式: [mqttProperties.commandTopicBase]/[robotId]
@@ -78,38 +146,38 @@ public class MqttServiceImpl implements MqttService {
String topic = mqttProperties.getCommandTopicBase() + "/" + robotId;
// 4. 准备MQTT消息
String effectivePayload = (payloadJson == null) ? "" : payloadJson; // 确保payload不为null
MqttMessage mqttMessage = new MqttMessage(effectivePayload.getBytes(StandardCharsets.UTF_8));
MqttMessage mqttMessage = new MqttMessage(finalPayloadJson.getBytes(StandardCharsets.UTF_8));
mqttMessage.setQos(mqttProperties.getDefaultQos()); // 设置QoS级别，从配置中读取
// mqttMessage.setRetained(false); // 保留消息标志，默认为false，通常指令不需要保留
try {
// 5. 发布MQTT消息
if (!mqttClient.isConnected()) {
log.error("MQTT客户端未连接。无法向机器人 {} 发送指令 {}。任务ID: {}", commandType, robotId, task.getId());
log.error("MQTT客户端未连接。无法向机器人 {} 发送指令 {}。任务ID: {}", commandType, robotId, associatedTaskId);
// 任务将保持PENDING状态。后续可由定时任务重试或标记为错误。
return false;
}
log.debug("正在向主题发布消息: {}, QoS: {}, 负载: {}", topic, mqttMessage.getQos(), effectivePayload);
log.debug("正在向主题发布消息: {}, QoS: {}, 负载: {}", topic, mqttMessage.getQos(), finalPayloadJson);
mqttClient.publish(topic, mqttMessage);
log.info("成功向机器人 {} 在主题 {} 上发布指令 {}。任务ID: {}", commandType, robotId, topic, task.getId());
log.info("成功向机器人 {} 在主题 {} 上发布指令 {}。任务ID: {}", commandType, robotId, topic, associatedTaskId);
// 6. 将机器人任务的状态标记为 SENT (已发送)
boolean markedAsSent = robotTaskService.markTaskAsSent(task.getId(), new Date());
boolean markedAsSent = robotTaskService.markTaskAsSent(associatedTaskId, new Date());
if (!markedAsSent) {
log.warn("指令 {} 已发布给机器人 {}，但将任务 {} 标记为 SENT 失败。系统可能处于不一致状态。",
commandType, robotId, task.getId());
commandType, robotId, associatedTaskId);
// 这是一个严重警告。指令已发送，但数据库状态未正确反映。
// 事务应回滚。
}
return true; // 指令已发送，并尝试更新了任务状态
} catch (MqttException e) {
log.error("向机器人 {} 发布指令 {} (任务ID: {}) 时发生MqttException。错误: {}",
commandType, robotId, task.getId(), e.getMessage(), e);
// RobotTask 将保持 PENDING 状态。重试机制或超时任务后续可以处理此问题。
commandType, robotId, associatedTaskId, e.getMessage(), e);
// RobotTask (associatedTaskId) 将保持 PENDING 状态 (或它之前的状态)。事务应回滚。
throw e; // 重新抛出异常，以便Spring的@Transactional进行事务回滚
} catch (Exception e) {
log.error("向机器人 {} 发送指令 {} (任务ID: {}) 时发生意外异常。错误: {}",
commandType, robotId, task.getId(), e.getMessage(), e);
commandType, robotId, associatedTaskId, e.getMessage(), e);
throw e; // 重新抛出异常，以便事务回滚
}
}

20
springboot-init-main/src/main/java/com/yupi/project/service/impl/RobotTaskServiceImpl.java
View File

@@ -44,6 +44,26 @@ public class RobotTaskServiceImpl extends ServiceImpl<RobotTaskMapper, RobotTask
return false;
}
@Override
public boolean hasOtherPendingOrSentTask(String robotId, Long taskIdToExclude) {
if (robotId == null) {
log.warn("Cannot check for other pending/sent tasks: robotId is null.");
return false;
}
QueryWrapper<RobotTask> queryWrapper = new QueryWrapper<>();
queryWrapper.eq("robot_id", robotId)
.in("status", RobotTaskStatusEnum.PENDING, RobotTaskStatusEnum.SENT);
if (taskIdToExclude != null) {
queryWrapper.ne("id", taskIdToExclude); // Exclude the current task ID
}
long count = this.count(queryWrapper);
if (count > 0) {
log.info("Robot {} has {} other PENDING or SENT tasks (excluding task ID: {}).", robotId, count, taskIdToExclude == null ? "N/A" : taskIdToExclude);
return true;
}
return false;
}
@Override
@Transactional
public RobotTask createTask(String robotId, CommandTypeEnum commandType, String payloadJson, Long sessionId) {