Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新主程序以启用任务5的执行，并修改turn_degree函数以支持相对角度旋转。

Browse Source
This commit is contained in:
Havoc 2025-05-13 18:27:47 +08:00
parent 9492912c8f
commit 9ec783b8b1
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
base_move/turn_degree.py
View File

@ -1,7 +1,7 @@
import math import math
import time import time
def turn_degree(ctrl, msg, degree=90): def turn_degree(ctrl, msg, degree=90, relative=True):
""" """
结合里程计实现精确稳定的旋转指定角度 结合里程计实现精确稳定的旋转指定角度
@ -9,6 +9,7 @@ def turn_degree(ctrl, msg, degree=90):
ctrl: Robot_Ctrl 对象，包含里程计信息 ctrl: Robot_Ctrl 对象，包含里程计信息
msg: robot_control_cmd_lcmt 对象，用于发送命令 msg: robot_control_cmd_lcmt 对象，用于发送命令
degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度 degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
relative: 是否相对于当前角度，默认为True
返回: 返回:
Bool: 是否成功旋转到指定角度 Bool: 是否成功旋转到指定角度
@ -20,7 +21,10 @@ def turn_degree(ctrl, msg, degree=90):
current_yaw = ctrl.odo_msg.rpy[2] 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] 范围 # 标准化目标角度到 [-pi, pi] 范围
if target_yaw > math.pi: if target_yaw > math.pi:

2
main.py
View File

@ -37,7 +37,7 @@ def main():
# run_task_1(Ctrl, msg, image_processor) # run_task_1(Ctrl, msg, image_processor)
# run_task_5(Ctrl, msg) run_task_5(Ctrl, msg)
# time.sleep(100) # time.sleep(100)