turn-degress test

base_move/turn_degree.py

@@ -0,0 +1,109 @@
+import math
+import time
+
+def turn_degree(ctrl, msg, degree=90):
+    """
+    结合里程计实现精确稳定的旋转指定角度
+    
+    参数:
+    ctrl: Robot_Ctrl 对象，包含里程计信息
+    msg: robot_control_cmd_lcmt 对象，用于发送命令
+    degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
+    
+    返回:
+    Bool: 是否成功旋转到指定角度
+    """
+    # 将角度转换为弧度
+    target_rad = math.radians(degree)
+    
+    # 获取当前朝向
+    current_yaw = ctrl.odo_msg.rpy[2]
+    
+    # 计算目标朝向
+    target_yaw = current_yaw + target_rad
+    
+    # 标准化目标角度到 [-pi, pi] 范围
+    if target_yaw > math.pi:
+        target_yaw -= 2 * math.pi
+    if target_yaw < -math.pi:
+        target_yaw += 2 * math.pi
+    
+    # 定义允许的误差范围（弧度）
+    limit = 0.04  # 约2.3度
+    
+    # 计算最短旋转方向和距离
+    def circle_dist(target, location):
+        value1 = abs(target - location)
+        value2 = 2 * math.pi - value1
+        direction1 = 1 if target > location else 0  # 1为逆时针，0为顺时针
+        
+        # 计算两个方向哪个距离更短
+        if value1 < value2:
+            return direction1, value1
+        else:
+            return 1 - direction1, value2
+    
+    # 获取旋转方向和距离
+    direction, dist = circle_dist(target_yaw, current_yaw)
+    print(f'开始旋转: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°')
+    
+    if abs(dist) > limit:
+        # 主要转向
+        const_int = 2470  # 转1.57弧度约需2470的duration值
+        
+        # 设置转向命令
+        msg.mode = 11  # Locomotion模式
+        msg.gait_id = 26  # 自变频步态
+        msg.vel_des = [0, 0, 0.5 if direction > 0 else -0.5]  # 转向速度
+        msg.duration = int(const_int * abs(dist))
+        msg.step_height = [0.06, 0.06]  # 抬腿高度
+        msg.life_count += 1
+        
+        # 发送命令
+        ctrl.Send_cmd(msg)
+        
+        # 等待转向完成
+        time.sleep(7 * abs(dist) / 1.57)
+        
+        # 获取当前角度
+        current_yaw = ctrl.odo_msg.rpy[2]
+        print(f'主要转向完成: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°')
+        
+        # 计算剩余误差
+        remaining_dist = target_yaw - current_yaw
+        if remaining_dist > math.pi:
+            remaining_dist -= 2 * math.pi
+        elif remaining_dist < -math.pi:
+            remaining_dist += 2 * math.pi
+        
+        # 精细调整（如果误差大于限制）
+        if abs(remaining_dist) > limit:
+            # 进行微调
+            const_int_tiny = 1200  # 微调使用较小的系数
+            
+            # 设置微调命令
+            msg.mode = 11
+            msg.gait_id = 26
+            msg.vel_des = [0, 0, 0.5 if remaining_dist > 0 else -0.5]
+            msg.duration = int(const_int_tiny * abs(remaining_dist))
+            msg.step_height = [0.06, 0.06]
+            msg.life_count += 1
+            
+            # 发送命令
+            ctrl.Send_cmd(msg)
+            
+            # 等待微调完成
+            time.sleep(5)
+            
+            # 获取最终角度
+            final_yaw = ctrl.odo_msg.rpy[2]
+            print(f'微调完成: 最终角度={math.degrees(final_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°')
+            
+            final_error = abs(target_yaw - final_yaw)
+            if final_error > math.pi:
+                final_error = 2 * math.pi - final_error
+            
+            # 判断是否成功达到目标
+            return final_error <= limit
+    
+    return True

task_5/task_5.py

@@ -6,6 +6,8 @@ import os
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 
 from task_5.detect_arrow_direction import ArrowDetector
+from base_move.turn_degree import turn_degree
 
 def run_task_5(ctrl, msg, image_processor=None):
-    pass
+    turn_degree(ctrl, msg, 90)
+