#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(), ¤t_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其他核心任务一些时间 }