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

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