242 lines
11 KiB
C++
242 lines
11 KiB
C++
#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;
|
||
}
|
||
}
|