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:
parent
76c00337ed
commit
3de4bb8485
@ -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:
|
||||
|
||||
@ -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)
|
||||
Loading…
x
Reference in New Issue
Block a user