113 lines
3.8 KiB
Python
113 lines
3.8 KiB
Python
import math
|
||
import time
|
||
|
||
def turn_degree(ctrl, msg, degree=90, relative=True):
|
||
"""
|
||
结合里程计实现精确稳定的旋转指定角度
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
|
||
relative: 是否相对于当前角度，默认为True
|
||
|
||
返回:
|
||
Bool: 是否成功旋转到指定角度
|
||
"""
|
||
# 将角度转换为弧度
|
||
target_rad = math.radians(degree)
|
||
|
||
# 获取当前朝向
|
||
current_yaw = ctrl.odo_msg.rpy[2]
|
||
|
||
# 计算目标朝向
|
||
if relative:
|
||
target_yaw = current_yaw + target_rad
|
||
else:
|
||
target_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 |