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