diff --git a/main.py b/main.py index 5b8ac3e..ad423db 100644 --- a/main.py +++ b/main.py @@ -131,6 +131,9 @@ class Robot_Ctrl(object): self.mode_ok = self.rec_msg.mode else: self.mode_ok = 0 + + def reset_offset(self): + self.calibration_offset = self.odo_msg.xyz def msg_handler_o(self, channel, data): self.odo_msg = localization_lcmt().decode(data) diff --git a/task_1/task_1.py b/task_1/task_1.py index 113dd74..ce7791b 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -192,6 +192,6 @@ def run_task_1_back(ctrl, msg, time_sleep=5000): section('任务1-19: 90度旋转', "旋转") turn_degree_v2(ctrl, msg, degree=0, absolute=True) - go_to_xy(ctrl, msg, target_x=-0.2, target_y=0, observe=observe) + go_straight(ctrl, msg, distance=-1.4, observe=observe) success("任务1-back完成", "完成") \ No newline at end of file diff --git a/task_2/task_2.py b/task_2/task_2.py index 90e15a7..9d5429d 100644 --- a/task_2/task_2.py +++ b/task_2/task_2.py @@ -293,7 +293,7 @@ def run_task_2(ctrl, msg, xy_flag=False): return arrow_direction def run_task_2_back(ctrl, msg): - go_to_xy(ctrl, msg, 0.7, 4.8, speed=0.5, observe=True) + go_to_xy(ctrl, msg, 0.7, -11, speed=0.5, observe=True) turn_degree_v2(ctrl, msg, -106.5, absolute=True) print('角度为',ctrl.odo_msg.rpy[2]) diff --git a/task_2_5/task_2_5.py b/task_2_5/task_2_5.py index ba14c45..b8c9634 100644 --- a/task_2_5/task_2_5.py +++ b/task_2_5/task_2_5.py @@ -37,5 +37,5 @@ def run_task_2_5(Ctrl, msg, direction='left'): def run_task_2_5_back(Ctrl, msg, direction='left'): section('任务2.5-back:预备进入任务3', "启动") turn_degree_v2(Ctrl, msg, degree=0, absolute=True) - go_to_x_v2(Ctrl, msg, target_x=1, observe=observe) - go_to_y_v2(Ctrl, msg, target_y=5.2, speed=0.5, observe=True) + # go_to_x_v2(Ctrl, msg, target_x=1, observe=observe) + # go_to_y_v2(Ctrl, msg, target_y=5.2, speed=0.5, observe=True) diff --git a/task_3/task_3.py b/task_3/task_3.py index 6f91fe7..23360a2 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -204,7 +204,7 @@ def pass_up_down(ctrl, msg): max_iterations = 200 # 最大循环次数,作为安全保障 start_height = ctrl.odo_msg.xyz[2] # 记录起始高度 start_y = ctrl.odo_msg.xyz[1] # 记录起始y坐标 - y_target = 10.2 # 目标y坐标,达到此值时停止 + y_target = 10.0 # 目标y坐标,达到此值时停止 # 阶段控制 descending_detected = False # 是否检测到正在下坡 @@ -419,7 +419,7 @@ def run_task_3(ctrl, msg, time_sleep=5000): pass_up_down(ctrl, msg) turn_degree_v2(ctrl, msg, 90, absolute=True) - center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False) + # center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False) section('任务3-2:yellow stop', "开始") go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3) diff --git a/task_4/task_4.py b/task_4/task_4.py index 16df113..cca7657 100644 --- a/task_4/task_4.py +++ b/task_4/task_4.py @@ -51,7 +51,9 @@ def run_task_4_back(ctrl, msg): turn_degree_v2(ctrl, msg, degree=-90, absolute=True) center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False) - go_straight(ctrl, msg, distance=2, speed=1, observe=True) + ctrl.reset_offset() + + go_straight(ctrl, msg, distance=3, speed=1, observe=True) center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False) # 向右移动0.5秒 diff --git a/task_5/task_5.py b/task_5/task_5.py index 8e7631a..fdd8534 100644 --- a/task_5/task_5.py +++ b/task_5/task_5.py @@ -258,6 +258,6 @@ def run_task_5(ctrl, msg, direction='left', observe=False, time_sleep=5000): section('任务5-7:返回', "移动") turn_degree_v2(ctrl, msg, degree=-90, absolute=True) - + # 返回移动和扫描结果 return go_success, res['qr_result']