191 lines
11 KiB
C++
191 lines
11 KiB
C++
#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);
|
||
const char* cmd_ack_val = nullptr; // 用于存储指令的中文ACK值
|
||
|
||
if (strcmp(commandType, "MOVE") == 0 || strcmp(commandType, "MOVE_TO_SPOT") == 0) {
|
||
cmd_ack_val = "移动到指定点";
|
||
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
|
||
// 注意：这里的ACK是针对指令接收无效的情况，移动完成的ACK在loop中处理
|
||
} else {
|
||
Serial.println("指令无效: 机器人当前不处于可接收MOVE指令的状态。");
|
||
mqtt_publish_ack(taskId, false, "Robot busy or not in correct state for MOVE", current_robot_status, nullptr, cmd_ack_val);
|
||
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, cmd_ack_val);
|
||
current_active_task_id = "";
|
||
}
|
||
} else if (strcmp(commandType, "STOP_CHARGE") == 0) {
|
||
cmd_ack_val = "停止充电";
|
||
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 更新逻辑可以更精细，例如，如果充电时就知道spotId
|
||
mqtt_publish_ack(taskId, true, ("Charging stopped. Duration: " + String(charge_duration) + "s").c_str(), current_robot_status, robot_current_location.c_str(), cmd_ack_val);
|
||
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, nullptr);
|
||
} else {
|
||
Serial.println("指令无效: 机器人未在充电状态。");
|
||
mqtt_publish_ack(taskId, false, "Robot not in CHARGING state for STOP_CHARGE", current_robot_status, nullptr, cmd_ack_val);
|
||
}
|
||
current_active_task_id = ""; // STOP_CHARGE任务通常是瞬时完成
|
||
} else if (strcmp(commandType, "START_CHARGE") == 0) {
|
||
cmd_ack_val = "开始充电";
|
||
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(), cmd_ack_val);
|
||
current_active_task_id = "";
|
||
} else {
|
||
Serial.print("未知指令类型: "); Serial.println(commandType);
|
||
// 对于未知指令，不确定后端是否能处理没有正确 command_ack 的ACK，或者是否应该发送ACK
|
||
// 为安全起见，可以不发送，或发送一个带通用错误信息的（但后端可能不认）
|
||
// 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);
|
||
current_robot_status = MOVING; // 明确在开始导航前设置状态为MOVING
|
||
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, current_active_task_id.c_str(), target_spot_for_move.c_str(), nullptr, 0, 0, nullptr, nullptr); // 上报MOVING状态
|
||
|
||
bool move_success = navigation_move_to_spot(target_spot_for_move.c_str(), ¤t_robot_status, &robot_current_location);
|
||
|
||
const char* move_cmd_ack_val = "移动到指定点"; // MOVE 指令的中文ACK值
|
||
|
||
if (move_success) {
|
||
Serial.println("移动成功，准备充电。");
|
||
// current_robot_status 已在 navigation_move_to_spot 中被设为 CHARGING (如果它内部这么做)
|
||
// 或者在这里强制设置，确保到达即充逻辑
|
||
current_robot_status = CHARGING;
|
||
robot_charge_start_time_ms = millis();
|
||
relay_start_charging();
|
||
// robot_current_location 应该由 navigation_move_to_spot 更新为实际到达的spotId
|
||
mqtt_publish_ack(current_active_task_id.c_str(), true, "Robot arrived and started charging.", current_robot_status, robot_current_location.c_str(), move_cmd_ack_val);
|
||
} else {
|
||
Serial.println("移动失败或超时。");
|
||
// current_robot_status 应该已在 navigation_move_to_spot 中被设为 FAULTED
|
||
if(current_robot_status != FAULTED) { // 以防万一，如果导航模块未正确设置
|
||
current_robot_status = FAULTED;
|
||
// robot_current_location 可能还是旧的或 EN_ROUTE_TO_...
|
||
}
|
||
mqtt_publish_ack(current_active_task_id.c_str(), false, "Failed to reach target spot.", current_robot_status, robot_current_location.c_str(), move_cmd_ack_val);
|
||
}
|
||
target_spot_for_move = ""; // 清除移动目标
|
||
// current_active_task_id = ""; // 在ACK发送后清除，确保ACK能使用正确的ID。如果ACK失败，任务ID可能还需要。
|
||
// 考虑到ACK是即时发送，可以在这里清除
|
||
if(current_active_task_id.length() > 0 && (strcmp(current_active_task_id.c_str(), "") != 0) ) {
|
||
// 只有在 active_task_id 有效时才清除，避免重复清除或错误清除
|
||
}
|
||
current_active_task_id = ""; // 移动任务完成后清除
|
||
// 立即发布一次最终状态
|
||
mqtt_publish_status_update(current_robot_status, robot_current_location.c_str(), robot_battery_level, nullptr, nullptr, (current_robot_status == CHARGING || current_robot_status == COMPLETED || current_robot_status == FAULTED) ? robot_current_location.c_str() : nullptr, (current_robot_status == CHARGING) ? (millis()-robot_charge_start_time_ms)/1000 : 0, (current_robot_status == FAULTED) ? 1 : 0 /*示例错误码*/, (current_robot_status == FAULTED) ? "Navigation or other error" : nullptr, nullptr);
|
||
}
|
||
|
||
// --- 定期任务 ---
|
||
// 状态上报
|
||
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其他核心任务一些时间
|
||
}
|