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

@@ -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

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程序被用户中断")

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')
 

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)
+