From 2793daf8fe3565b67d704dced219dd1d898efa5b Mon Sep 17 00:00:00 2001
From: Havoc <2993167370@qq.com>
Date: Tue, 20 May 2025 10:04:14 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=20go=5Fstraight=20=E5=87=BD?=
 =?UTF-8?q?=E6=95=B0=E4=B8=AD=E7=9A=84=E6=96=B9=E5=90=91=E8=AE=A1=E7=AE=97?=
 =?UTF-8?q?=E9=80=BB=E8=BE=91=EF=BC=8C=E4=BD=BF=E7=94=A8=20IMU=20=E6=95=B0?=
 =?UTF-8?q?=E6=8D=AE=E6=9D=A5=E7=A1=AE=E5=AE=9A=E8=A7=92=E5=BA=A6=E5=81=8F?=
 =?UTF-8?q?=E5=B7=AE=EF=BC=8C=E6=8F=90=E5=8D=87=E4=BA=86=E6=9C=BA=E5=99=A8?=
 =?UTF-8?q?=E4=BA=BA=E5=9C=A8=E5=89=8D=E8=BF=9B=E5=92=8C=E5=90=8E=E9=80=80?=
 =?UTF-8?q?=E6=97=B6=E7=9A=84=E8=BF=90=E5=8A=A8=E7=B2=BE=E5=BA=A6=E3=80=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 base_move/go_straight.py | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/base_move/go_straight.py b/base_move/go_straight.py
index c5b9b56..d51919f 100644
--- a/base_move/go_straight.py
+++ b/base_move/go_straight.py
@@ -104,10 +104,11 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False):
             dy = current_position[1] - start_position[1]
             distance_moved = math.sqrt(dx*dx + dy*dy)
             
-            # 计算移动方向与初始朝向的偏差
-            movement_direction = math.atan2(current_position[1] - last_position[1], 
-                                           current_position[0] - last_position[0]) if distance_moved > 0.05 else start_yaw
-            yaw_error = movement_direction - start_yaw
+            # 根据前进或后退确定期望方向
+            expected_direction = start_yaw if forward else (start_yaw + math.pi) % (2 * math.pi)
+            
+            # 使用IMU朝向数据计算角度偏差
+            yaw_error = current_yaw - expected_direction
             # 角度归一化
             while yaw_error > math.pi:
                 yaw_error -= 2 * math.pi