Merge branch 'main' of ssh://120.27.199.238:222/Havoc420mac/mi-task

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)
         
@@ -61,37 +62,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)
-        
         # 执行旋转
         # 如果角度很小，增加一个小的偏移以确保旋转足够
         if abs(angle_to_rotate) < 3.0:
@@ -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:

task_1/task_1.py

@@ -7,7 +7,7 @@ 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...')
@@ -15,7 +15,9 @@ def run_task_1(ctrl, msg):
     # v2
     arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=False, observe=observe)
 
-    move_to_hori_line(ctrl, msg, distance=1, observe=observe)
+    return True
+    
+    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)
 

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值最大的边缘点