72 lines
1.9 KiB
Python
72 lines
1.9 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
|
||
from base_move.turn_degree import turn_degree, turn_degree_v2
|
||
from base_move.go_to_xy import go_straight
|
||
from base_move.move_base_hori_line import arc_turn_around_hori_line, align_to_horizontal_line
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
observe = True
|
||
|
||
def run_task_2_5(Ctrl, msg, direction='left'):
|
||
section('任务2.5：预备进入任务3', "启动")
|
||
|
||
go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe)
|
||
|
||
# TEST
|
||
turn_degree(Ctrl, msg, 90, absolute=observe)
|
||
|
||
section('任务2.5-1：第一次旋转', "移动")
|
||
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
Ctrl,
|
||
msg,
|
||
angle_deg=90 if direction == 'left' else -90,
|
||
target_distance=0.2,
|
||
detect_func_version=3,
|
||
observe=observe,
|
||
)
|
||
|
||
go_straight(Ctrl, msg, distance=0.1, speed=0.5, observe=observe)
|
||
|
||
section('任务2.5-2：第二次旋转', "移动")
|
||
|
||
turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
|
||
|
||
# arc_turn_around_hori_line(
|
||
# Ctrl,
|
||
# msg,
|
||
# angle_deg=70,
|
||
# # target_distance=0.5,
|
||
# pass_align=True,
|
||
# radius=res['radius'],
|
||
# detect_func_version=2,
|
||
# observe=observe,
|
||
# )
|
||
|
||
def run_task_2_5_back(Ctrl, msg, direction='left'):
|
||
section('任务2.5-back：预备进入任务3', "启动")
|
||
|
||
arc_turn_around_hori_line(
|
||
Ctrl,
|
||
msg,
|
||
angle_deg=90 if direction == 'left' else -90,
|
||
target_distance=0.3,
|
||
detect_func_version=2,
|
||
observe=observe,
|
||
)
|
||
|
||
turn_degree(Ctrl, msg, degree=90, absolute=True)
|
||
|
||
# arc_turn_around_hori_line(
|
||
# Ctrl,
|
||
# msg,
|
||
# angle_deg=70,
|
||
# target_distance=0.5,
|
||
# pass_align=True,
|
||
# )
|
||
|