Enhance main.py and task_5.py: Introduced turn_degree_v2 function for improved turning control in main.py. Updated run_task_5 to accept direction parameter, allowing for dynamic movement towards unloading points based on QR code results. Added logic for moving to horizontal line and unloading sequence, enhancing task execution and robot navigation.

main.py

@@ -33,7 +33,7 @@ from task_5.task_5 import run_task_5
 # from task_test.go_to_xy_example import run_go_to_xy_example
 
 
-from base_move.turn_degree import turn_degree
+from base_move.turn_degree import turn_degree, turn_degree_v2
 
 pass_marker = True
 
@@ -56,14 +56,16 @@ def main():
         # print(f"arrow_direction: {arrow_direction}")
         # run_task_2_5(Ctrl, msg, direction=arrow_direction)
 
-        # arrow_direction = 'right' # TEST
+        arrow_direction = 'right' # TEST
 
         # if arrow_direction == 'left':
         #     run_task_4(Ctrl, msg)
         # else:
         #     run_task_3(Ctrl, msg)
 
-        run_task_5(Ctrl, msg)
+        turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
+
+        run_task_5(Ctrl, msg, direction=arrow_direction)
 
         return
 

task_5/task_5.py

@@ -12,7 +12,10 @@ from base_move.turn_degree import turn_degree
 from base_move.go_straight import go_straight, go_straight_with_qrcode
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 from utils.gray_sky_analyzer import analyze_gray_sky_ratio
-from base_move.move_base_hori_line import detect_horizontal_track_edge, detect_horizontal_track_edge_v2, detect_horizontal_track_edge_v3, calculate_distance_to_line
+from base_move.move_base_hori_line import (
+    detect_horizontal_track_edge, detect_horizontal_track_edge_v2, detect_horizontal_track_edge_v3, 
+    calculate_distance_to_line, move_to_hori_line
+)
 
 
 def go_straight_to_horizontal_line_with_qr(ctrl, msg, target_distance=0.5, speed=0.5, 
@@ -201,7 +204,7 @@ def go_straight_to_horizontal_line_with_qr(ctrl, msg, target_distance=0.5, speed
     return res['success'], qr_result, res
 
 
-def run_task_5(ctrl, msg, observe=False):
+def run_task_5(ctrl, msg, direction='left', observe=False):
     """
     走向卸货
     """
@@ -222,6 +225,24 @@ def run_task_5(ctrl, msg, observe=False):
             warning("未扫描到二维码", "二维码")
     else:
         error("未能成功到达横线前指定距离", "失败")
-    
+
+    section('任务5-2：移动到卸货点', "移动")
+    if direction == 'right' and res['qr_result'] == 'B-2' or direction == 'left' and res['qr_result'] == 'B-1':
+        # 直走
+        move_to_hori_line(ctrl, msg, target_distance=0.5, observe=observe)
+    else:
+        move_to_hori_line(ctrl, msg, target_distance=1.2, observe=observe)
+
+    section('任务5-3：卸货', "卸货")
+    ctrl.base_msg.lie_down()
+    time.sleep(3)
+    ctrl.base_msg.stand_up()
+
+    section('任务5-4：返回', "移动")
+    go_straight(ctrl, msg, distance=-1.2, speed=0.5, observe=observe)
+
+    section('任务5-5：转弯', "转弯")
+    turn_degree(ctrl, msg, degree=90)
+
     # 返回移动和扫描结果
     return go_success, res['qr_result']