refactor(base_move): improve left side track following algorithm

- Update function signature: replace target_distance with min_distance and max_distance
- Remove unused error_threshold variable
- Adjust logging messages for better readability
- Modify success condition to check within range of min_distance and max_distance
- Update task_left_line.py to turn 90 degrees before following left side track
This commit is contained in:
havoc420ubuntu 2025-05-25 17:38:07 +00:00
parent 76c00337ed
commit 3de4bb8485
2 changed files with 11 additions and 15 deletions

View File

@ -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:

View File

@ -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)