diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 5ff5f60..73c2d42 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -27,6 +27,7 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): image = ctrl.image_processor.get_current_image() while attempts < max_attempts and not aligned: + print(f"尝试次数: {attempts+1}/{max_attempts}") # 检测横向线边缘 edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0) @@ -60,37 +61,6 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): if observe: print(f"需要旋转的角度: {angle_to_rotate:.2f}度") - - # 可视化横向线和校准角度 - if isinstance(image, str): - img = cv2.imread(image) - else: - img = image.copy() - - height, width = img.shape[:2] - center_x = width // 2 - - # 画出检测到的横向线 - line_length = 200 - end_x = edge_point[0] + line_length - end_y = int(edge_point[1] + slope * line_length) - start_x = edge_point[0] - line_length - start_y = int(edge_point[1] - slope * line_length) - cv2.line(img, (start_x, start_y), (end_x, end_y), (0, 255, 0), 2) - - # 画出水平线(目标线) - horizontal_y = edge_point[1] - cv2.line(img, (center_x - line_length, horizontal_y), - (center_x + line_length, horizontal_y), (0, 0, 255), 2) - - # 标记角度 - cv2.putText(img, f"当前斜率: {slope:.4f}", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - cv2.putText(img, f"旋转角度: {angle_to_rotate:.2f}°", (10, 70), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - - cv2.imshow("校准旋转分析", img) - cv2.waitKey(1500 if observe else 1) # 执行旋转 # 如果角度很小,增加一个小的偏移以确保旋转足够 @@ -247,7 +217,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): """ # 首先校准到水平 print("校准到横向线水平") - aligned = align_to_horizontal_line(ctrl, msg, observe=False) + aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平,停止移动") @@ -256,7 +226,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): # 检测横向线 # image = cv2.imread("current_image.jpg") # TEST image = ctrl.image_processor.get_current_image() - edge_point, edge_info = detect_horizontal_track_edge(image, observe=False) + edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) if edge_point is None or edge_info is None: print("无法检测到横向线,停止移动") @@ -352,7 +322,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): # 如果没有提供图像处理器或图像验证失败,则使用里程计数据判断 return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米,则认为成功 -def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left=True, observe=False): +def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False): """ 对准前方横线,然后以计算出来的距离为半径,做一个向左或向右的圆弧旋转。 参数: @@ -367,14 +337,14 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left """ # 1. 对准横线 print("校准到横向线水平") - aligned = align_to_horizontal_line(ctrl, msg, observe=False) + aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平,停止动作") return False # 2. 检测横线并计算距离 image = ctrl.image_processor.get_current_image() - edge_point, edge_info = detect_horizontal_track_edge(image, observe=False) + edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) if edge_point is None or edge_info is None: print("无法检测到横向线,停止动作") return False @@ -395,8 +365,11 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left # 3. 计算圆弧运动参数 angle_rad = math.radians(angle_deg) # 设定角速度(rad/s),可根据实际调整 - w = 0.4 if left else -0.4 # 左转为正,右转为负 - v = w * r # 线速度,正负号与角速度方向一致 + + base_w = 0.6 + + w = base_w if left else -base_w # 左转为正,右转为负 + v = abs(w * r) # 线速度,正负号与角速度方向一致 t = abs(angle_rad / w) # 运动时间,取绝对值保证为正 if observe: diff --git a/task_1/task_1.py b/task_1/task_1.py index babc848..80aad72 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -7,15 +7,17 @@ import os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hori_line -observe = True +observe = False def run_task_1(ctrl, msg): print('Running task 1...') # v2 arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=False, observe=observe) + + return True - move_to_hori_line(ctrl, msg, distance=1, observe=observe) + move_to_hori_line(ctrl, msg, target_distance=1, observe=observe) arc_turn_around_hori_line(ctrl, msg, angle_deg=180, left=True, observe=observe) diff --git a/utils/detect_track.py b/utils/detect_track.py index 2f2054e..452b460 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -3,6 +3,7 @@ import numpy as np from sklearn import linear_model def detect_horizontal_track_edge(image, observe=False, delay=1000): + observe = False # TSET """ 检测正前方横向黄色赛道的边缘,并返回y值最大的边缘点