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

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;
}
}