From 49a6a10f635180649fac498420b5646f767e0085 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Mon, 12 May 2025 08:06:08 +0000 Subject: [PATCH] feat(task_1): implement new robot movements and behaviors - Add new functions for turning, going straight, and circling - Implement standing up and lying down movements - Update task_1 to use new movement functions - Decode QR code information from image processor - Refactor main.py to run task_5 instead of task_1 --- base_move/go_straight/main.py | 35 ++++++++++--- main.py | 8 ++- task_1/task_1.py | 95 +++++++++++++++++++++++++---------- task_5/task_5.py | 30 +++++++++++ 4 files changed, 134 insertions(+), 34 deletions(-) create mode 100644 task_5/task_5.py diff --git a/base_move/go_straight/main.py b/base_move/go_straight/main.py index 980221d..baf4a4b 100644 --- a/base_move/go_straight/main.py +++ b/base_move/go_straight/main.py @@ -26,17 +26,40 @@ def main(): Ctrl.Send_cmd(msg) Ctrl.Wait_finish(12, 0) + # def circle_90(ctrl, msg, direction="right"): + # msg.mode = 11 + # msg.gait_id = 26 + # msg.duration = 1000 + # msg.vel_des = [0.0, 0.0, -1.9 if direction == "right" else 1] + # msg.life_count += 1 + # ctrl.Send_cmd(msg) + # time.sleep(1) + + # circle_90(Ctrl, msg) + # return True + + # TAG 设置机器人头部朝下 + # msg.mode = 21 # 位置插值控制 + # msg.gait_id = 0 + # # msg.rpy_des = [0, -0.3, 0] # 头部朝下 + # msg.rpy_des = [0, 0, 0] + # msg.pos_des = [0, 0, 0.2] + # msg.duration = 400 # 期望执行时间，0.3秒 + # msg.life_count += 1 + # Ctrl.Send_cmd(msg) + # time.sleep(1) + print("Go forward") msg.mode = 11 - msg.gait_id = 26 # 26 表示快速 trot 步态 - msg.vel_des = [1.0, 1.0, -1.0] - msg.duration = 2000 - msg.step_height = [0.06, 0.06] + msg.gait_id = 3 + msg.vel_des = [1, 0, 0] + # msg.rpy_des = [0, -0.3, 0] + msg.duration = 20000 + msg.step_height = [0.03, 0.03] msg.life_count += 1 Ctrl.Send_cmd(msg) - Ctrl.Wait_finish(11, 26) - + Ctrl.Wait_finish(11, msg.gait_id) except KeyboardInterrupt: pass diff --git a/main.py b/main.py index 69a1169..8729e1c 100644 --- a/main.py +++ b/main.py @@ -16,6 +16,7 @@ from utils.robot_control_response_lcmt import robot_control_response_lcmt from utils.image_raw import ImageProcessor from task_1.task_1 import run_task_1 +from task_5.task_5 import run_task_5 def main(): Ctrl = Robot_Ctrl() @@ -31,10 +32,13 @@ def main(): msg.life_count += 1 Ctrl.Send_cmd(msg) Ctrl.Wait_finish(12, 0) + # time.sleep(1) - run_task_1(Ctrl, msg) + # run_task_1(Ctrl, msg, image_processor) - time.sleep(100) + run_task_5(Ctrl, msg) + + # time.sleep(100) except KeyboardInterrupt: print("\n程序被用户中断") diff --git a/task_1/task_1.py b/task_1/task_1.py index c0da6ed..a093245 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -1,39 +1,82 @@ import time import cv2 +def turn_90(ctrl, msg, direction="right"): + msg.mode = 11 + msg.gait_id = 10 + msg.vel_des = [1.1, 0, -1.5 if direction == "right" else 1.5] + msg.duration = 1500 + msg.step_height = [0.06, 0.06] + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(1.5) + +def turn_right_180(ctrl, msg): + msg.mode = 11 + msg.gait_id = 3 + msg.vel_des = [0.55, 0, -1.0] + msg.duration = 3500 + msg.step_height = [0.06, 0.06] + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(3.5) + +def go_straight(ctrl, msg, time_ms, fb=True): + msg.mode = 11 + msg.gait_id = 26 + msg.vel_des = [1 if fb else -1, 0, 0] + msg.duration = time_ms + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(time_ms / 1000) + +def stand_up(ctrl, msg): + msg.mode = 12 # Recovery stand + msg.gait_id = 0 + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(1.5) + +def down_and_back(ctrl, msg): + # # TAG 趴下 + msg.mode = 7 + msg.gait_id = 1 + msg.duration = 2000 + msg.life_count += 1 + ctrl.Send_cmd(msg) + ctrl.Wait_finish(7, 1) + + stand_up(ctrl, msg) + + go_straight(ctrl, msg, 1500, False) + +def circle_90(ctrl, msg, direction="right"): + msg.mode = 11 + msg.gait_id = 10 + msg.duration = 1000 + msg.vel_des = [0, 0, -2 if direction == "right" else 2] + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(1) + def run_task_1(ctrl, msg, image_processor): print('Running task 1...') # 右前方 - msg.mode = 11 - msg.gait_id = 26 # 26 表示快速 trot 步态 - msg.vel_des = [0.5, 0.5, -1.0] - msg.duration = 1800 - msg.step_height = [0.06, 0.06] - msg.life_count += 1 - ctrl.Send_cmd(msg) - time.sleep(1.8) - - msg.mode = 11 - msg.gait_id = 26 - msg.vel_des = [1, 0, 0] - msg.duration = 200 - msg.life_count += 1 - ctrl.Send_cmd(msg) - time.sleep(0.2) + turn_90(ctrl, msg) # TAG take photo - image = image_processor.get_current_image() - cv2.imwrite('photo.jpg', image) + qrcode_info = image_processor.decode_qrcode() + print(qrcode_info) - # msg.mode = 11 - # msg.gait_id = 26 - # msg.vel_des = [1, 0, 0] - # msg.duration = 1000 - # msg.life_count += 1 - # ctrl.Send_cmd(msg) - # time.sleep(1.0) + # TODO 不同的走向 + turn_right_180(ctrl, msg) + go_straight(ctrl, msg, 200) - + down_and_back(ctrl, msg) + + circle_90(ctrl, msg) + + # turn_90(ctrl, msg, 'left') diff --git a/task_5/task_5.py b/task_5/task_5.py new file mode 100644 index 0000000..3986e74 --- /dev/null +++ b/task_5/task_5.py @@ -0,0 +1,30 @@ +import time + +def run_task_5(ctrl, msg): + # DEBUG + # msg.mode = 11 # 运动模式 + # msg.gait_id = 26 + # msg.vel_des = [0, 0, 2] # 期望速度 + # msg.duration = 0 # 零时长表示持续运动，直到接收到新命令 + # msg.step_height = [0.02, 0.02] # 持续运动时摆动腿的离地高度 + # msg.life_count += 1 + # ctrl.Send_cmd(msg) + # time.sleep(1.1) # 持续5秒钟 + + # msg.mode = 11 + # msg.gait_id = 3 + # msg.vel_des = [1, 0, 0] + # msg.duration = 1000 + # msg.step_height = [0.03, 0.03] + # msg.life_count += 1 + # print(msg.pos_des) + # ctrl.Send_cmd(msg) + # ctrl.Wait_finish(11, msg.gait_id) + + msg.mode = 3 + msg.gait_id = 0 + msg.pos_des = [0, 0, 0.15] + msg.life_count += 1 + ctrl.Send_cmd(msg) + ctrl.Wait_finish(3, msg.gait_id) +