diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 81f9048..86bde56 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -1222,7 +1222,7 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None, return True -def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=30, observe=False): +def follow_left_side_track(ctrl, msg, min_distance=600, max_distance=650, speed=0.3, max_time=30, observe=False): """ 控制机器狗向左侧移动并靠近左侧的黄色轨迹线,只进行侧向移动,不进行前进 @@ -1258,11 +1258,9 @@ def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=3 initial_distance = track_info["distance_to_left"] if observe: - info(f"初始距离: {initial_distance:.1f}px, 目标距离: {target_distance}px", "初始") + info(f"初始距离: {initial_distance:.1f}px") # 最大允许误差(像素) - error_threshold = 30 - # 简单比例控制参数 kp = 0.0005 @@ -1284,18 +1282,17 @@ def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=3 current_distance = track_info["distance_to_left"] # 计算误差 (正值表示需要向左移动,负值表示需要向右移动) - error = target_distance - current_distance - + error_distance = current_distance - (min_distance + max_distance) / 2 # 新增:计算误差 if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 - debug(f"当前距离: {current_distance:.1f}px, 目标距离: {target_distance}px, 误差: {error:.1f}px", "跟踪") + debug(f"当前距离: {current_distance:.1f}px, 误差: {error:.1f}px", "跟踪") # 检查是否已达到目标 - if abs(error) < error_threshold: - success(f"已达到目标距离,当前距离: {current_distance:.1f}px, 目标距离: {target_distance}px", "完成") + if min_distance <= current_distance <= max_distance: + success(f"已达到目标距离,当前距离: {current_distance:.1f}px") break # 计算侧向速度 (简单比例控制) - lateral_velocity = kp * error + lateral_velocity = kp * error_distance # 修正:确保 error 已定义 # 限制侧向速度 lateral_velocity = max(-speed, min(speed, lateral_velocity)) @@ -1321,14 +1318,13 @@ def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=3 if track_info is not None: final_distance = track_info["distance_to_left"] - distance_error = abs(final_distance - target_distance) - final_success = distance_error < error_threshold + final_success = min_distance <= final_distance <= max_distance if observe: if final_success: - success(f"成功达到目标位置,最终距离: {final_distance:.1f}px, 目标: {target_distance}px", "成功") + success(f"成功达到目标位置,最终距离: {final_distance:.1f}px", "成功") else: - warning(f"未能精确达到目标位置,最终距离: {final_distance:.1f}px, 目标: {target_distance}px", "警告") + warning(f"未能精确达到目标位置,最终距离: {final_distance:.1f}px", "警告") return final_success else: diff --git a/task_test/task_left_line.py b/task_test/task_left_line.py index c1eb156..666cad3 100644 --- a/task_test/task_left_line.py +++ b/task_test/task_left_line.py @@ -13,6 +13,6 @@ from utils.log_helper import LogHelper, get_logger, section, info, debug, warnin logger = get_logger("任务-test") def run_task_test(ctrl, msg): - turn_degree(ctrl, msg, 0, absolute=True) + turn_degree(ctrl, msg, 90, absolute=True) follow_left_side_track(ctrl, msg) # time.sleep(100) \ No newline at end of file