Implement reset_offset method in Robot_Ctrl class for improved calibration handling. Update task 4 to utilize the new method and adjust movement distance. Modify task 2 to correct target coordinates and enhance task 1 by changing movement command. Comment out unused code in task 2.5 for clarity.

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)

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完成", "完成")

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

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)

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)

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秒

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