diff --git a/main.py b/main.py index 190472b..876ae2a 100644 --- a/main.py +++ b/main.py @@ -21,6 +21,7 @@ from utils.marker_client import MarkerRunner 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_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 @@ -41,11 +42,13 @@ def main(): # time.sleep(100) # TEST # run_task_1(Ctrl, msg) + run_task_2(Ctrl, msg) + # run_task_2_5(Ctrl, msg) # run_task_4(Ctrl, msg) - run_task_test(Ctrl, msg) + # run_task_test(Ctrl, msg) # time.sleep(100) diff --git a/task_2/task_2.py b/task_2/task_2.py new file mode 100644 index 0000000..1d25af3 --- /dev/null +++ b/task_2/task_2.py @@ -0,0 +1,97 @@ +import time +import sys +import os +import math + +# sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) +# 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 +# ) +# from base_move.go_straight import go_straight +# from base_move.turn_degree import turn_degree +from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing + +# 创建本模块特定的日志记录器 +logger = get_logger("任务1") + +observe = True + +def run_task_2(ctrl, msg): + + # print("Recovery stand") + # Ctrl.base_msg.stand_up() + + # # time.sleep(100) # TEST + + # run_task_1(Ctrl, msg) + + # x,y=image_processor.get_current_x() + # print('分别是',x,y) + # # 初始恢复站立 + # msg.mode = 12 # Recovery stand + # msg.gait_id = 0 + # msg.life_count += 1 + # Ctrl.Send_cmd(msg) + # Ctrl.Wait_finish(12, 0) + # print('1') + # x,y=image_processor.get_current_x() + # print('分别是',x,y) + + # # 初始恢复站立 + # msg.mode = 11 # Recovery stand + # msg.gait_id = 27 + # msg.vel_des = [0.3,0,0] + # msg.pos_des[2]=0.24 + # msg.duration = 0 + # msg.step_height = [0.01, 0.01] + # msg.life_count += 1 + # Ctrl.Send_cmd(msg) + # time.sleep(1) + + #获取微调角度 + + + # 连续三重弯道行走 + directions = [ + #直线走一段 + (0.4,0,0,0,0,0,0.5), + #第一个圆弧 + (0.45, 0, 0.77,0,0,0,7.3), + #微调路径 + (0.4,0,0,0,0,0,0.8), + #第二个圆弧 + (0.4,0,-0.72,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.72,0,0,0,4)] # [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"开始第 {i+1} 次转弯") + + # 根据转弯方向调整持续时间(假设半圆需要3秒) + time.sleep(t) # 持续3秒 + + # # 停止 + # msg.mode = 7 # PureDamper + # msg.gait_id = 0 + # msg.life_count += 1 + # Ctrl.Send_cmd(msg) + # Ctrl.Wait_finish(7, 0) \ No newline at end of file