From 9ec783b8b1a2d423a9b41d710a83890adb0fa3d9 Mon Sep 17 00:00:00 2001
From: Havoc <2993167370@qq.com>
Date: Tue, 13 May 2025 18:27:47 +0800
Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=B8=BB=E7=A8=8B=E5=BA=8F?=
 =?UTF-8?q?=E4=BB=A5=E5=90=AF=E7=94=A8=E4=BB=BB=E5=8A=A15=E7=9A=84?=
 =?UTF-8?q?=E6=89=A7=E8=A1=8C=EF=BC=8C=E5=B9=B6=E4=BF=AE=E6=94=B9turn=5Fde?=
 =?UTF-8?q?gree=E5=87=BD=E6=95=B0=E4=BB=A5=E6=94=AF=E6=8C=81=E7=9B=B8?=
 =?UTF-8?q?=E5=AF=B9=E8=A7=92=E5=BA=A6=E6=97=8B=E8=BD=AC=E3=80=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 base_move/turn_degree.py | 10 +++++++---
 main.py                  |  2 +-
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/base_move/turn_degree.py b/base_move/turn_degree.py
index 78c9772..02f7740 100644
--- a/base_move/turn_degree.py
+++ b/base_move/turn_degree.py
@@ -1,7 +1,7 @@
 import math
 import time
 
-def turn_degree(ctrl, msg, degree=90):
+def turn_degree(ctrl, msg, degree=90, relative=True):
     """
     结合里程计实现精确稳定的旋转指定角度
     
@@ -9,7 +9,8 @@ def turn_degree(ctrl, msg, degree=90):
     ctrl: Robot_Ctrl 对象，包含里程计信息
     msg: robot_control_cmd_lcmt 对象，用于发送命令
     degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
-    
+    relative: 是否相对于当前角度，默认为True
+
     返回:
     Bool: 是否成功旋转到指定角度
     """
@@ -20,7 +21,10 @@ def turn_degree(ctrl, msg, degree=90):
     current_yaw = ctrl.odo_msg.rpy[2]
     
     # 计算目标朝向
-    target_yaw = current_yaw + target_rad
+    if relative:
+        target_yaw = current_yaw + target_rad
+    else:
+        target_yaw = target_rad
     
     # 标准化目标角度到 [-pi, pi] 范围
     if target_yaw > math.pi:
diff --git a/main.py b/main.py
index 31e4f84..87ae312 100644
--- a/main.py
+++ b/main.py
@@ -37,7 +37,7 @@ def main():
 
         # run_task_1(Ctrl, msg, image_processor)
 
-        # run_task_5(Ctrl, msg)
+        run_task_5(Ctrl, msg)
 
         # time.sleep(100)