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

base_move/move_base_hori_line.py

@@ -366,14 +366,20 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
     angle_rad = math.radians(angle_deg)
     # 设定角速度（rad/s），可根据实际调整
 
-    base_w = 0.6
+    # 减小基础角速度，增加精度
+    base_w = 0.8  # 原来是0.6
 
     w = base_w if left else -base_w  # 左转为正，右转为负
     v = abs(w * r)  # 线速度，正负号与角速度方向一致
     t = abs(angle_rad / w)  # 运动时间，取绝对值保证为正
-
+    
+    # 应用时间补偿系数，因为实际旋转常会比理论计算多
+    time_compensation = 0.92  # 时间减少到92%
+    t *= time_compensation
+    
     if observe:
-        print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s, 运动时间: {t:.2f}s")
+        print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s")
+        print(f"理论运动时间: {abs(angle_rad / w):.2f}s, 应用补偿系数{time_compensation}后: {t:.2f}s")
 
     # 4. 发送圆弧运动指令
     msg.mode = 11
@@ -404,10 +410,10 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
     
     # 旋转精度监控
     target_angle = angle_rad if left else -angle_rad  # 目标角度（弧度）
-    slowdown_threshold = 0.85  # 当达到目标角度的85%时开始减速
+    slowdown_threshold = 0.8  # 当达到目标角度的80%时开始减速（原来是85%）
     
     # 监控旋转进度并自行控制减速
-    while abs(angle_turned) < abs(angle_rad) * 0.98 and time.time() - start_time < timeout:
+    while abs(angle_turned) < abs(angle_rad) * 0.95 and time.time() - start_time < timeout:
         # 计算已移动距离
         current_position = ctrl.odo_msg.xyz
         dx = current_position[0] - start_position[0]
@@ -430,8 +436,8 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
         if completion_ratio > slowdown_threshold:
             # 剩余比例作为速度系数
             speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
-            # 确保速度系数不会太小
-            speed_factor = max(0.3, speed_factor)
+            # 确保速度系数不会太小，但可以更慢一些
+            speed_factor = max(0.2, speed_factor)  # 原来是0.3
             
             # 应用减速
             current_v = v * speed_factor
@@ -481,6 +487,13 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
         # 计算需要微调的角度
         adjust_angle = target_angle_deg - final_angle_deg
         
+        # 增加补偿系数，因为实际旋转常会超出理论计算值
+        compensation_factor = 0.85  # 只旋转计算值的85%
+        adjust_angle *= compensation_factor
+        
+        if observe:
+            print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度")
+        
         # 使用turn_degree函数进行微调
         turn_degree(ctrl, msg, adjust_angle, absolute=False)