diff --git a/main.py b/main.py index 51e91d9..bc6d7f6 100644 --- a/main.py +++ b/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 diff --git a/res/path/task-5/horizontal_edge_20250528_100447_858352.jpg b/res/path/task-5/horizontal_edge_20250528_100447_858352.jpg new file mode 100644 index 0000000..0ef3783 Binary files /dev/null and b/res/path/task-5/horizontal_edge_20250528_100447_858352.jpg differ diff --git a/res/path/task-5/horizontal_edge_20250528_100604_117417.jpg b/res/path/task-5/horizontal_edge_20250528_100604_117417.jpg new file mode 100644 index 0000000..d823f7b Binary files /dev/null and b/res/path/task-5/horizontal_edge_20250528_100604_117417.jpg differ diff --git a/res/path/task-5/horizontal_edge_20250528_100604_158105.jpg b/res/path/task-5/horizontal_edge_20250528_100604_158105.jpg new file mode 100644 index 0000000..5ebbc53 Binary files /dev/null and b/res/path/task-5/horizontal_edge_20250528_100604_158105.jpg differ diff --git a/res/path/task-5/origin_horizontal_edge_20250528_100447_858352.jpg b/res/path/task-5/origin_horizontal_edge_20250528_100447_858352.jpg new file mode 100644 index 0000000..c6683b3 Binary files /dev/null and b/res/path/task-5/origin_horizontal_edge_20250528_100447_858352.jpg differ diff --git a/res/path/task-5/origin_horizontal_edge_20250528_100604_117417.jpg b/res/path/task-5/origin_horizontal_edge_20250528_100604_117417.jpg new file mode 100644 index 0000000..6208154 Binary files /dev/null and b/res/path/task-5/origin_horizontal_edge_20250528_100604_117417.jpg differ diff --git a/res/path/task-5/origin_horizontal_edge_20250528_100604_158105.jpg b/res/path/task-5/origin_horizontal_edge_20250528_100604_158105.jpg new file mode 100644 index 0000000..e721f5e Binary files /dev/null and b/res/path/task-5/origin_horizontal_edge_20250528_100604_158105.jpg differ diff --git a/task_5/task_5.py b/task_5/task_5.py index 54943ec..b49bf4d 100644 --- a/task_5/task_5.py +++ b/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']