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

Enhance task 2 functionality and add backtracking capability

Browse Source 
- Introduced run_task_2_back function to facilitate reverse navigation.
- Updated main.py to include calls to run_task_2_back alongside run_task_2 for improved task execution.
- Adjusted movement parameters in run_task_2 for better path accuracy and control.
- Added new movement functions for enhanced maneuverability in task 2.
This commit is contained in:
havoc420ubuntu 2025-05-27 07:00:25 +00:00
parent d1356fc330
commit e778530503
2 changed files with 82 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
main.py
View File

@ -21,12 +21,14 @@ from utils.image_raw import ImageProcessor
from utils.base_msg import BaseMsg
from task_1.task_1 import run_task_1
from task_2.task_2 import run_task_2
from task_2.task_2 import run_task_2, run_task_2_back
from task_2_5.task_2_5 import run_task_2_5
from task_4.task_4 import run_task_4
from task_test.task_left_line import run_task_test
# from task_test.go_to_xy_example import run_go_to_xy_example
from base_move.turn_degree import turn_degree
pass_marker = True
def main():
@ -43,11 +45,12 @@ def main():
# time.sleep(100) # TEST
# run_task_1(Ctrl, msg)
# run_task_2(Ctrl, msg)
run_task_2(Ctrl, msg)
turn_degree(Ctrl, msg, -90, absolute=True)
run_task_2_back(Ctrl, msg)
# run_task_2_5(Ctrl, msg)
run_task_4(Ctrl, msg)
# run_task_4(Ctrl, msg)
# run_task_test(Ctrl, msg)

90
task_2/task_2.py
View File

@ -8,6 +8,13 @@ from base_move.go_to_xy import go_to_xy
from base_move.turn_degree import turn_degree
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
from base_move.move_base_hori_line import (
arc_turn_around_hori_line,
go_straight_by_hori_line,
move_to_hori_line,
align_to_horizontal_line
)
# 创建本模块特定的日志记录器
logger = get_logger("任务1")
@ -49,25 +56,31 @@ def run_task_2(ctrl, msg):
# msg.life_count += 1
# Ctrl.Send_cmd(msg)
# time.sleep(1)
print('角度为',ctrl.odo_msg.rpy[2])
#获取微调角度
text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
# 连续三重弯道行走
directions = [
#直线走一段
(0.4,0,0,0,0,0,1.6),
#第一个圆弧
(0.45, 0, 0.77,0,0,0,7),
#微调路径
(0.4,0,0,0,0,0,1.2),
# #第二个圆弧
(0.4,0,-0.71,0,0,0,7.7),
# #第三个圆弧
(0.4,0.05,-0.75,0,0,0,1.2),
# #微调路径
(0.45, 0, 0.8,0,0,0,7.2),
# #第四个圆弧
(0.4,0,-0.71,0,0,0,4)
#第一个圆弧
(0.45, 0, 0.77,0,0,0,6.5),
#微调路径
(0.3,0,0,0,0,0,0.4),
#第二个圆弧
(0.4,0,-0.73,0,0,0,7.9),
#微调路径
(0.3,0,0,0,0,0,1),
# 第三个圆弧
(0.4,0,0.73,0,0,0,6.4),
#微调路径
(0.3,0,0,0,0,0,1.3),
#第四个圆弧
(0.4,0,-0.69,0,0,0,3.6),
#绕出来
(0.3,0,0.6,0,0,0,1),
# #前走一点
# (0.3,0,0,0,0,0,0.5)
] # [vel_x, vel_z] 对应 [左转, 右转, 左转]
@ -83,14 +96,61 @@ def run_task_2(ctrl, msg):
ctrl.Send_cmd(msg)
# 打印当前方向
print(f"开始第 {i+1} 次转弯")
print(f"开始 {text[i]} ")
# 根据转弯方向调整持续时间（假设半圆需要3秒）
time.sleep(t) # 持续3秒
print('角度为',ctrl.odo_msg.rpy[2])
print('x为',ctrl.odo_msg.xyz[0])
print('y为',ctrl.odo_msg.xyz[1])
print('z为',ctrl.odo_msg.xyz[2])
# # 停止
# msg.mode = 7 # PureDamper
# msg.gait_id = 0
# msg.life_count += 1
# Ctrl.Send_cmd(msg)
# Ctrl.Wait_finish(7, 0)
def run_task_2_back(ctrl, msg):
text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
# 连续三重弯道行走
directions = [
(0.3,0,0.6,0,0,0,1),
#第四个圆弧
(0.4,0,0.69,0,0,0,3.6),
#微调路径
(0.3,0,0,0,0,0,1.3),
# 第三个圆弧
(0.4,0,-0.73,0,0,0,6.4),
#微调路径
(0.3,0,0,0,0,0,1),
#第二个圆弧
(0.4,0,0.73,0,0,0,7.9),
#微调路径
(0.3,0,0,0,0,0,0.4),
#第一个圆弧
(0.45, 0, -0.77,0,0,0,6.5),
#直线走一段
(0.4,0,0,0,0,0,1.2),
] # [vel_x, vel_z] 对应 [左转, 右转, 左转]
for i, (vel_x, vel_y, vel_z ,rpy_x,rpy_y,rpy_z,t) in enumerate(directions):
# 设置转向命令
msg.mode = 11 # Locomotion
msg.gait_id = 26 # TROT_FAST
msg.vel_des = [vel_x, vel_y, vel_z] # [x, y, z]，y=0表示不侧移
msg.rpy_des = [rpy_x, rpy_y, rpy_z]
msg.duration = 0 # 连续运动
msg.step_height = [0.03, 0.03]
msg.life_count += 1
ctrl.Send_cmd(msg)
# 打印当前方向
print(f"开始 {text[i]} ")
# 根据转弯方向调整持续时间（假设半圆需要3秒）
time.sleep(t) # 持续3秒