diff --git a/main.py b/main.py index 33f9c43..fd08e68 100644 --- a/main.py +++ b/main.py @@ -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) diff --git a/task_2/task_2.py b/task_2/task_2.py index 4ab8fa3..209ae3d 100644 --- a/task_2/task_2.py +++ b/task_2/task_2.py @@ -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秒 \ No newline at end of file