From 24f98da1a3ed1852ba70d3cf49c3d0f228d30c00 Mon Sep 17 00:00:00 2001
From: Havoc <2993167370@qq.com>
Date: Thu, 15 May 2025 20:32:51 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=20arc=5Fturn=5Faround=5Fhori?=
 =?UTF-8?q?=5Fline=20=E5=87=BD=E6=95=B0=EF=BC=8C=E6=94=B9=E8=BF=9B?=
 =?UTF-8?q?=E6=97=8B=E8=BD=AC=E8=BF=87=E7=A8=8B=E4=B8=AD=E7=9A=84=E5=87=8F?=
 =?UTF-8?q?=E9=80=9F=E6=8E=A7=E5=88=B6=E9=80=BB=E8=BE=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 增加旋转精度监控，达到目标角度的85%时开始减速
- 直接发送停止命令，替代平滑停止方法
- 添加观察输出信息，便于调试和分析旋转过程中的状态
- 处理旋转完成后的角度微调，确保最终角度准确
---
 base_move/move_base_hori_line.py | 81 +++++++++++++++++++++++++++++---
 1 file changed, 74 insertions(+), 7 deletions(-)

diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py
index 73c2d42..9d84fcf 100644
--- a/base_move/move_base_hori_line.py
+++ b/base_move/move_base_hori_line.py
@@ -398,8 +398,16 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
     start_time = time.time()
     timeout = t + 2  # 给个超时保护
 
-    # 监控旋转进度，但不执行减速（改用stop_smooth）
-    while distance_moved < arc_length * 0.95 and abs(angle_turned) < abs(angle_rad) * 0.95 and time.time() - start_time < timeout:
+    # 初始速度
+    current_v = v
+    current_w = w
+    
+    # 旋转精度监控
+    target_angle = angle_rad if left else -angle_rad  # 目标角度（弧度）
+    slowdown_threshold = 0.85  # 当达到目标角度的85%时开始减速
+    
+    # 监控旋转进度并自行控制减速
+    while abs(angle_turned) < abs(angle_rad) * 0.98 and time.time() - start_time < timeout:
         # 计算已移动距离
         current_position = ctrl.odo_msg.xyz
         dx = current_position[0] - start_position[0]
@@ -415,16 +423,75 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
         while angle_turned < -math.pi:
             angle_turned += 2 * math.pi
             
-        if observe and time.time() % 0.5 < 0.02:  # 每0.5秒左右打印一次
-            print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度")
+        # 计算角度完成比例
+        completion_ratio = abs(angle_turned) / abs(angle_rad)
+        
+        # 当完成一定比例时开始减速
+        if completion_ratio > slowdown_threshold:
+            # 剩余比例作为速度系数
+            speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
+            # 确保速度系数不会太小
+            speed_factor = max(0.3, speed_factor)
+            
+            # 应用减速
+            current_v = v * speed_factor
+            current_w = w * speed_factor
+            
+            # 更新速度命令
+            msg.mode = 11
+            msg.gait_id = 26
+            msg.vel_des = [current_v, 0, current_w]
+            msg.duration = 200  # 短时间更新
+            msg.step_height = [0.06, 0.06]
+            msg.life_count += 1
+            ctrl.Send_cmd(msg)
+            
+            if observe and time.time() % 0.5 < 0.02:  # 每0.5秒左右打印一次
+                print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 当前角速度: {current_w:.3f}rad/s")
+        else:
+            if observe and time.time() % 0.5 < 0.02:  # 每0.5秒左右打印一次
+                print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度")
         
         time.sleep(0.05)
 
-    # 使用平滑停止
-    ctrl.base_msg.stop_smooth(current_vel=[v, 0, w])
+    # 直接发送停止命令，不使用平滑停止
+    msg.mode = 11
+    msg.gait_id = 26
+    msg.vel_des = [0, 0, 0]  # 零速度
+    msg.duration = 200
+    msg.step_height = [0.06, 0.06]
+    msg.life_count += 1
+    ctrl.Send_cmd(msg)
+    
+    # 停下来后的最终角度
+    final_yaw = ctrl.odo_msg.rpy[2]
+    final_angle_turned = final_yaw - start_yaw
+    # 角度归一化处理
+    while final_angle_turned > math.pi:
+        final_angle_turned -= 2 * math.pi
+    while final_angle_turned < -math.pi:
+        final_angle_turned += 2 * math.pi
+    
+    final_angle_deg = math.degrees(final_angle_turned)
+    target_angle_deg = angle_deg if left else -angle_deg
     
     if observe:
-        print("圆弧运动完成，平稳停止")
+        print(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度")
+        
+    # 检查是否需要微调
+    angle_error = abs(final_angle_deg - target_angle_deg)
+    if angle_error > 3.0:  # 如果误差超过3度
+        if observe:
+            print(f"角度误差: {angle_error:.2f}度，进行微调")
+        
+        # 计算需要微调的角度
+        adjust_angle = target_angle_deg - final_angle_deg
+        
+        # 使用turn_degree函数进行微调
+        turn_degree(ctrl, msg, adjust_angle, absolute=False)
+    
+    if observe:
+        print("圆弧转弯完成")
 
     return True