diff --git a/cfg_args.toml b/cfg_args.toml
index 5b963e1..5529f20 100644
--- a/cfg_args.toml
+++ b/cfg_args.toml
@@ -1,5 +1,5 @@
-[kick_ass]
-target_person = 3
+[lane_mode]
+mode_index = 3
 
 [task]
 Subtask_enable = true
diff --git a/cfg_subtask.toml.7154.bak b/cfg_subtask.toml.7154.bak
new file mode 100644
index 0000000..1d473fc
--- /dev/null
+++ b/cfg_subtask.toml.7154.bak
@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[get_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 1
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 2
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 2.5
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = false
+
+[kick_ass]
+pid_kp = 0.8
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1
diff --git a/cfg_subtask.toml.raw.bak b/cfg_subtask.toml.raw.bak
new file mode 100644
index 0000000..1d0c998
--- /dev/null
+++ b/cfg_subtask.toml.raw.bak
@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[get_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.8
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 1.5
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = false
+
+[kick_ass]
+pid_kp = 0.8
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1
diff --git a/majtask.py b/majtask.py
index 826b509..07c5993 100644
--- a/majtask.py
+++ b/majtask.py
@@ -67,6 +67,7 @@ class main_task():
             error_abs = abs(self.lane_error)
 
             if error_abs > 50:
+                # speed = 11
                 speed = 13
             elif error_abs > 45:
                 speed = 15
diff --git a/subtask.py b/subtask.py
index 2586e3c..4c44c60 100644
--- a/subtask.py
+++ b/subtask.py
@@ -1439,7 +1439,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)
@@ -1477,6 +1477,16 @@ class kick_ass():
         by_cmd.send_position_axis_x(1, 120)
         time.sleep(1)
         logger.debug("結束任務，前進四")
+
+        filter.switch_camera(2)
+        for _ in range(3):
+            by_cmd.send_speed_x(15)
+        while True:
+            ret, box = filter.get(tlabel.BASE)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+            by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.02)
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass
diff --git a/subtask_69.py b/subtask_69.py
index 2586e3c..4c44c60 100644
--- a/subtask_69.py
+++ b/subtask_69.py
@@ -1439,7 +1439,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)
@@ -1477,6 +1477,16 @@ class kick_ass():
         by_cmd.send_position_axis_x(1, 120)
         time.sleep(1)
         logger.debug("結束任務，前進四")
+
+        filter.switch_camera(2)
+        for _ in range(3):
+            by_cmd.send_speed_x(15)
+        while True:
+            ret, box = filter.get(tlabel.BASE)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+            by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.02)
         # by_cmd.send_speed_x(25)
         # time.sleep(4)
         pass
diff --git a/subtask_7123.py b/subtask_7123.py
index 2507cec..9aec7de 100644
--- a/subtask_7123.py
+++ b/subtask_7123.py
@@ -1367,7 +1367,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)
diff --git a/subtask_7126.py b/subtask_7126.py
index 9baa9b0..65ed131 100644
--- a/subtask_7126.py
+++ b/subtask_7126.py
@@ -1360,7 +1360,7 @@ class kick_ass():
         logger.info("扫黑除暴初始化")
         self.pos_gap1 = cfg['kick_ass']['pos_gap1']
         self.pos_gap2 = cfg['kick_ass']['pos_gap2']
-        self.target_person = cfg_args['kick_ass']['target_person']
+        self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
         by_cmd.send_position_axis_x(1, 160)
diff --git a/subtask_7154.py b/subtask_7154.py
new file mode 100644
index 0000000..899640a
--- /dev/null
+++ b/subtask_7154.py
@@ -0,0 +1,1487 @@
+from enum import Enum
+from loguru import logger
+from utils import label_filter
+from utils import tlabel
+from utils import LLM
+from utils import CountRecord
+import utils
+import toml
+import zmq
+import time
+import variable as var
+import action as act
+import re
+import threading
+import ctypes
+cfg = None
+cfg_args = None
+by_cmd = None
+filter = None
+llm_bot = None
+
+# 目标检测 socket 客户端
+context = None
+socket = None
+
+context1 = None
+ocr_socket = None
+
+global_skip_queue = None
+
+'''
+description: main.py 里执行 引入全局变量
+param {*} _by_cmd 控制器对象
+return {*}
+'''
+def import_obj(_by_cmd, skip_queue):
+
+    global by_cmd
+    global filter
+    global llm_bot
+
+    global context
+    global socket
+
+    global context1
+    global ocr_socket
+
+    global cfg
+    global cfg_args
+    global global_skip_queue
+    cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
+    cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.toml')
+    by_cmd = _by_cmd
+    global_skip_queue = skip_queue
+
+    # 目标检测 socket 客户端
+    context = zmq.Context()
+    socket = context.socket(zmq.REQ)
+    socket.connect("tcp://localhost:6667")
+    logger.info("subtask yolo client init")
+
+    # ocr socket 客户端
+    # context1 = zmq.Context()
+    # ocr_socket = context1.socket(zmq.REQ)
+    # ocr_socket.connect("tcp://localhost:6668")
+    # logger.info("subtask ocr client init")
+
+    filter = label_filter(socket)
+    if cfg['move_area']['llm_enable']:
+        llm_bot = LLM()
+def car_stop():
+    for _ in range(3):
+        by_cmd.send_speed_x(0)
+        time.sleep(0.2)
+        by_cmd.send_speed_y(0)
+        time.sleep(0.2)
+        by_cmd.send_speed_omega(0)
+# 蓝色球使用
+def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
+
+    not_found_counts = 0
+    ret, error = filter.aim_right(label)
+    while not ret:
+        not_found_counts += 1
+        if not_found_counts >= 20:
+            not_found_counts = 0
+            error = -320 # error > 0 front run
+            logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找")
+            break
+        ret, error = filter.aim_right(label)
+
+    error += offset
+    if abs(error) > 10 and run:
+        if error > 0:
+            by_cmd.send_speed_x(-run_speed)
+        else:
+            by_cmd.send_speed_x(run_speed)
+    # 停的位置已经很接近目标，可以直接使用 distance 校准
+    else:
+        return
+    
+    while True:
+        ret, error = filter.aim_right(label)
+        while not ret:
+            ret, error = filter.aim_right(label)
+        error += offset
+        # if ret:
+        if abs(error) <= 8:
+            car_stop()
+            logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
+            ret, error = filter.aim_right(label)
+            while not ret:
+                not_found_counts += 1
+                if not_found_counts >=30:
+                    return
+                ret, error = filter.aim_right(label)
+            error += offset
+            logger.info(f"calibrate_right_new:停车后的误差是{error}")
+            if abs(error) > 8:
+                logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
+                error = error * 1.5
+                if error > 0:
+                    by_cmd.send_distance_x(-10, int(error))
+                else:
+                    by_cmd.send_distance_x(10, int(-error))
+            
+            break
+'''
+description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车
+如果停车后 error > 8 则使用 distance 校准 
+这个方法仅用于在视野里能找到的情况下进行校准，并不能实现边走边寻找 然后再校准
+param {*} label
+param {*} offset
+param {*} run
+param {*} run_speed
+return {*}
+'''     
+def calibrate_new(label, offset, run = True, run_speed = 3.5):
+    not_found_counts = 0
+    ret, box = filter.get(label)
+    while not ret:
+        not_found_counts += 1
+        if not_found_counts >= 20:
+            not_found_counts = 0
+            error = -320 # error > 0 front run
+            logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找")
+            break
+        ret, box = filter.get(label)
+    
+    # 如果超过二十帧跳出，则此时 box 为空值，需要再校验 ret
+    if ret is True:
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+    if abs(error) > 10 and run:
+        if error > 0:
+            by_cmd.send_speed_x(-run_speed)
+        else:
+            by_cmd.send_speed_x(run_speed)
+    # 停的位置已经很接近目标，可以直接使用 distance 校准
+    else:
+        if abs(error) > 8:
+            logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准")
+            # error = error # 3
+            if error > 0:
+                by_cmd.send_distance_x(-10, int(error))
+            else:
+                by_cmd.send_distance_x(10, int(-error))
+            return
+        logger.info(f"calibrate_new:停车后误差{error}小于 8  不校准")
+        return 
+    while True:
+        ret, box = filter.get(label)
+        while not ret:
+            ret, box = filter.get(label)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret:
+            if abs(error) <= 10: # 5
+                car_stop()
+                logger.info("calibrate_new:行进时 误差小于 10 直接停车")
+
+                ret, box = filter.get(label)
+                while not ret:
+                    ret, box = filter.get(label)
+
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"calibrate_new:停车后的误差是{error}")
+                if abs(error) > 8:
+                    logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准")
+                    error = error * 3
+                    # logger.error(f"error * 3  {error}")
+                    if error > 0:
+                        by_cmd.send_distance_x(-10, int(error))
+                    else:
+                        by_cmd.send_distance_x(10, int(-error))
+                break
+# 弃用 distance 校准
+def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
+    # run_direc == 1 向前
+    stop_error = 0
+    if run_direc == 1:
+        by_cmd.send_speed_x(run_speed)
+    else:
+        by_cmd.send_speed_x(-run_speed)
+        
+    if label == tlabel.TPLATFORM:
+        stop_error = 8
+    else:
+        stop_error = 15
+    while True:
+        ret, box = filter.get(label)
+        while not ret:
+            if not global_skip_queue.empty():
+                _ = global_skip_queue.get()
+                logger.error("跳过 explore_calibrate_new")
+                return False
+            ret, box = filter.get(label)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret:
+            # 校准速度越大 停车的条件越宽泛 20 15
+            if abs(error) <= stop_error:
+                car_stop()
+                logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
+                
+                ret, box = filter.get(label)
+                while not ret:
+                    ret, box = filter.get(label)
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"停车后像素误差:{error}")
+                
+                # logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
+                # if abs(error) < 8:
+                #     error = error * 3
+                # else:
+                # error = error * 2
+                # if error > 0:
+                #     by_cmd.send_distance_x(-10, int(error))
+                # else:
+                #     by_cmd.send_distance_x(10, int(-error))
+                
+                break
+    return True
+
+# 对准应知道是左还是右，右侧需在过滤器中进行翻转
+# flipv 为垂直翻转标志，转右侧开启
+def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5):
+    stop_error = 0
+    error_record = CountRecord(10)
+    if run_direc == 1:
+        by_cmd.send_speed_x(run_speed)
+    else:
+        by_cmd.send_speed_x(-run_speed)
+        
+    if target_label == tlabel.TPLATFORM:
+        stop_error = 8
+    else:
+        stop_error = 15
+    while True:
+        # FIXME 此处待修改
+        ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
+        while not ret1:
+            # 如果找不到目标且跳过任务队列非空 (即指令跳过)
+            if not global_skip_queue.empty():
+                _ = global_skip_queue.get()
+                logger.error("跳过 hanoi_calibrate")
+                return False
+            # 如果找不到目标且发现错误目标 (上次放置任务失败)
+            if ret2:
+                # 如果连续计数超过阈值，则直接返回
+                if error_record(ret2):
+                    return False
+            ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret1:
+            # 校准速度越大 停车的条件越宽泛 20 15
+            if abs(error) <= stop_error:
+                car_stop()
+                logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
+                
+                ret, box = filter.get(target_label)
+                while not ret:
+                    ret, box = filter.get(target_label)
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"停车后像素误差:{error}")
+                break
+    return True
+# 任务类
+class task:  
+    def __init__(self, name, task_template, find_counts=10, enable=True):  
+        self.enable = enable
+        self.task_t = task_template()
+
+        self.counts = 0
+        self.find_counts = int(find_counts)
+        self.name = name
+
+    def init(self):
+        if hasattr(self.task_t, 'init') and callable(getattr(self.task_t, 'init', None)):
+            self.task_t.init()
+        else:
+            logger.warning("[Task ]# 该任务没有 init 方法")
+    def find(self):
+        if hasattr(self.task_t, 'find') and callable(getattr(self.task_t, 'find', None)):
+            return self.task_t.find()
+        else:
+            logger.warning("[Task ]# 该任务没有 find 方法")
+    def exec(self):  
+        # 根据标志位确定是否执行该任务
+        if self.enable is True:
+            if hasattr(self.task_t, 'exec') and callable(getattr(self.task_t, 'exec', None)):
+                logger.info(f"[Task ]# Executing task")  
+                self.task_t.exec()
+            else:
+                logger.warning("[Task ]# 该任务没有 exec 方法")
+        else:
+            logger.warning(f"[Task ]# Skip task")
+            if hasattr(self.task_t, 'nexec') and callable(getattr(self.task_t, 'nexec', None)):
+                self.task_t.nexec()
+            else:
+                logger.warning("[Task ]# 该任务没有 nexec 方法")
+    def after(self):
+        if hasattr(self.task_t, 'after') and callable(getattr(self.task_t, 'after', None)):
+            logger.info(f"[Task ]# {self.name} 正在执行 after")
+            self.task_t.after()
+            logger.debug(f"[Task ]# Task completed")  
+        else:
+            logger.warning("[Task ]# 该任务没有 after 方法")
+
+# 任务队列状态类
+class task_queuem_status(Enum):
+    IDEL      = 0
+    SEARCHING = 1
+    EXECUTING = 2
+
+# 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作
+class task_queuem(task):
+    # task_now = task(None, False)
+    def __init__(self, queue):
+        super(task_queuem, self)
+        self.queue = queue
+        self.status = task_queuem_status.IDEL
+        self.busy = True
+        logger.info(f"[TaskM]# Task num {self.queue.qsize()}")
+        # exec 线程
+        self.exec_thread = None
+    def exec(self, skip_queue):
+        # 如果空闲状态则将下一个队列任务取出
+        if self.status is task_queuem_status.IDEL:
+            if self.queue.qsize() == 0:
+                self.busy = False
+                logger.info(f"[TaskM]# Task queue empty, exit")
+                return False
+            self.task_now = self.queue.get()
+
+            # 如果当前任务没有使能，则直接转入执行状态，由任务执行函数打印未执行信息
+            if self.task_now.enable is True:
+                self.status = task_queuem_status.SEARCHING
+                # 如果使能该任务则执行该任务的初始化动作
+                self.task_now.init()
+            else:
+                self.status = task_queuem_status.EXECUTING
+            logger.info(f"[TaskM]# ---------------------->>>>")
+        # 阻塞搜索任务标志位
+        elif self.status is task_queuem_status.SEARCHING:
+            logger.info(f"[{self.task_now.name}]# Start searching task target")
+            while True:
+                if not skip_queue.empty():
+                    _ = skip_queue.get()
+                    logger.error(f"{self.task_now.name} 任务在 find 中已经被手动跳过")
+                    self.status = task_queuem_status.IDEL   # 空动作不需要阻塞巡线，直接置位
+                    self.task_now.after()                   # 执行任务后处理
+                    self.queue.task_done()                  # 弹出已执行的任务
+                    return True
+                ret = self.task_now.find()
+                self.task_now.counts += ret
+                if self.task_now.counts >= self.task_now.find_counts:
+                    self.status = task_queuem_status.EXECUTING
+                    break
+        # 执行任务函数
+        elif self.status is task_queuem_status.EXECUTING:
+            if self.task_now.enable is True:
+                logger.info(f"[TaskM]# Start execute task function")
+                # self.exec_thread = threading.Thread(target=self.task_now.exec)
+                # # 启动线程
+                # self.exec_thread.start()
+                # while True:
+                #     if not self.exec_thread.is_alive():
+                #         break
+                #     else:
+                #         if not skip_queue.empty():
+                #             car_stop()
+                #             thread_id = self.exec_thread.ident
+                #             res = ctypes.pythonapi.PyThreadState_SetAsyncExc(thread_id, ctypes.py_object(SystemExit))
+                #             _ = skip_queue.get()
+                #             logger.error(f"{self.task_now.name} 任务在 exec 中已经被手动跳过")
+                #             self.status = task_queuem_status.IDEL   # 空动作不需要阻塞巡线，直接置位
+                #             self.task_now.after()                   # 执行任务后处理
+                #             self.queue.task_done()                  # 弹出已执行的任务
+                #             return True
+                self.task_now.exec()                    # 执行当前任务函数
+                self.status = task_queuem_status.IDEL   # 执行完成后为退出巡线阻塞
+                self.task_now.after()                   # 执行任务后处理
+                self.queue.task_done()                  # 弹出已执行的任务
+                logger.info(f"[TaskM]# <<<<----------------------")
+            else:
+                logger.info(f"[TaskM]# Start execute task function (nexec)")
+                self.status = task_queuem_status.IDEL   # 空动作不需要阻塞巡线，直接置位
+                self.task_now.exec()                    # 执行当前任务函数
+                self.task_now.after()                   # 执行任务后处理
+                self.queue.task_done()                  # 弹出已执行的任务
+                logger.info(f"[TaskM]# <<<<----------------------")
+        return True
+
+# 人员施救
+class get_block1():
+    def init(self):
+        var.task_speed = 15
+        act.cmd.camera(0)
+        act.cmd.z2(20, 60, 0)
+        filter.switch_camera(1)
+        # if cfg['get_block']['first_block'] == "blue":
+        #     self.target_label = tlabel.BBLOCK
+        #     self.another_label = tlabel.RBLOCK
+        # else:
+        #     self.target_label = tlabel.RBLOCK
+        #     self.another_label = tlabel.BBLOCK
+        #     cfg['get_block']['first_block'] = "red"
+        self.target_label = [tlabel.RBLOCK, tlabel.BBLOCK]
+        self.target_counts = [0, 0]
+    def find(self):
+        # 目标检测红/蓝方块
+        # ret = filter.find(self.target_label)
+        # if ret > 0:
+        #     return True
+        # return False
+        ret = filter.find_mult(self.target_label)
+        self.target_counts[0] += ret[0]
+        self.target_counts[1] += ret[1]
+        if any(ret):
+            return True
+        return False
+
+
+    def exec(self):
+        car_stop()
+        if self.target_counts[0] > self.target_counts[1]:
+            var.first_block = tlabel.RBLOCK
+            var.second_block = tlabel.BBLOCK
+        else:
+            var.first_block = tlabel.BBLOCK
+            var.second_block = tlabel.RBLOCK
+        calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
+        logger.info("抓取块")
+
+        by_cmd.send_position_axis_z(30, 60)
+        time.sleep(1)
+        by_cmd.send_angle_claw(63)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(1)
+        by_cmd.send_angle_claw(25)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(30, 90)
+        time.sleep(0.5)
+
+        by_cmd.send_angle_claw_arm(175)
+        time.sleep(0.1)
+        by_cmd.send_position_axis_x(1, 100)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 70)
+        time.sleep(0.5)
+        by_cmd.send_angle_claw(63)
+        time.sleep(0.5)
+
+        by_cmd.send_position_axis_z(30, 130)
+        time.sleep(1)
+        by_cmd.send_position_axis_x(1, 140)
+        by_cmd.send_angle_claw_arm(225)
+        time.sleep(0.5)
+        # by_cmd.send_angle_storage(55)
+        # time.sleep(1)
+        by_cmd.send_position_axis_z(30, 60)
+
+        pass
+    def nexec(self):
+        # TODO 完成不执行任务的空动作
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
+        pass
+
+class get_block2():
+    def init(self):
+        while (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+        filter.switch_camera(1)
+        # if cfg['get_block']['first_block'] == "red":
+        #     self.target_label = tlabel.BBLOCK
+        #     self.another_label = tlabel.RBLOCK
+        # else:
+        #     self.target_label = tlabel.RBLOCK
+        #     self.another_label = tlabel.BBLOCK
+    def find(self):
+        # 目标检测红/蓝方块
+        ret = filter.find(var.second_block)
+        if ret > 0:
+            return True
+        return False
+    def exec(self):
+        car_stop()
+        calibrate_new(var.second_block, offset = 16, run = True, run_speed = 5)
+        logger.info("抓取块")
+        time.sleep(0.5)
+        by_cmd.send_angle_claw_arm(225)
+        by_cmd.send_angle_claw(63)
+        time.sleep(0.1)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(1)
+        by_cmd.send_angle_claw(25)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 100)
+        
+        # by_cmd.send_distance_x(15, 100)
+        time.sleep(2)
+
+        pass
+    def nexec(self):
+        # TODO 完成不执行任务的空动作
+        pass
+    def after(self):
+        var.task_speed = 0
+        var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
+        # 任务检查间隔
+        time.sleep(7)
+
+
+# 紧急转移
+class put_block():
+    def init(self):
+        var.task_speed = 0
+        while (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+        logger.info("紧急转移初始化")
+        filter.switch_camera(1)
+    def find(self):
+        # 目标检测医院
+        ret, box = filter.get(tlabel.HOSPITAL)
+        if ret > 0:
+            width = box[0][2] - box[0][0]
+            if width > 130:
+                return True
+        return False
+    def exec(self):
+        logger.info("找到医院")
+        car_stop()
+        calibrate_new(tlabel.HOSPITAL, offset = 7, run = False)
+
+        by_cmd.send_distance_x(10, 100)
+        by_cmd.send_position_axis_z(30, 0)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(1)
+        by_cmd.send_angle_claw(63)
+        time.sleep(1)
+
+        # 放置第二個塊
+        by_cmd.send_angle_storage(20)
+        by_cmd.send_position_axis_x(1, 110)
+        by_cmd.send_position_axis_z(30, 120)
+        time.sleep(1.5)
+        by_cmd.send_angle_claw_arm(180)
+        by_cmd.send_angle_claw(85)
+        # by_cmd.send_angle_storage(0)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30,70)
+        time.sleep(1)
+        by_cmd.send_angle_claw(25)
+        by_cmd.send_distance_x(-10, 110)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 110)
+        time.sleep(1)
+        by_cmd.send_angle_claw_arm(225)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 0)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(1.5)
+        by_cmd.send_angle_claw(45)
+        time.sleep(1)
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
+        # 下一动作预备位置
+        while by_cmd.send_position_axis_z(30, 130) == -1:
+            pass
+        time.sleep(1)
+        while by_cmd.send_position_axis_x(1, 0) == -1:
+            pass
+        while by_cmd.send_angle_claw_arm(45) == -1:
+            pass
+        # 任务检查间隔
+        # time.sleep(2)
+
+
+# 整装上阵
+class get_bball():
+    def init(self):
+        while (by_cmd.send_angle_camera(90) == -1):
+            by_cmd.send_angle_camera(90)
+        # by_cmd.send_position_axis_z(30, 135)
+        # time.sleep(0.5)
+        # by_cmd.send_position_axis_x(1, 0)
+        # time.sleep(2)
+        # by_cmd.send_angle_claw_arm(45)
+        
+        while (by_cmd.send_angle_storage(0) == -1):
+            by_cmd.send_angle_storage(0)
+
+        # 调试 临时换源
+        filter.switch_camera(1)
+        logger.info("整装上阵初始化")
+        # time.sleep(0.5)
+        
+        self.record = CountRecord(5)
+    def find(self):
+        # 目标检测蓝球
+        # ret = filter.find(tlabel.BBALL)
+        ret = filter.find_mult([tlabel.BBALL, tlabel.YBALL])
+        # ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
+        # TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关
+        # if ret:
+        #     if self.record(tlabel.BBALL):
+        #         return True
+        if ret[0] or ret[1]:
+            return True
+        return False
+    def exec(self):
+        logger.info("找到蓝色球")
+        car_stop()
+        time.sleep(0.5)
+        for _ in range(3):
+            calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
+            logger.info("抓蓝色球")
+            time.sleep(0.5)
+            by_cmd.send_angle_claw_arm(45)
+            by_cmd.send_angle_claw(54)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1.2)
+            by_cmd.send_angle_claw(25)
+            time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(1)
+            by_cmd.send_position_axis_x(1, 60)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, -40)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw_arm(80)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(54)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw_arm(45)
+            time.sleep(1)
+            by_cmd.send_position_axis_z(30, 135)
+            # 继续向前走
+            # by_cmd.send_speed_x(4)
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
+        # 下一动作预备位置
+        by_cmd.send_angle_claw(30)
+        time.sleep(0.5)
+        while by_cmd.send_position_axis_z(30, 0) == -1:
+            pass
+        time.sleep(1)
+        # # 任务检查间隔
+        # time.sleep(1)
+
+
+# 通信抢修
+class up_tower():
+    def init(self):
+        logger.info("通信抢修初始化")
+        while (by_cmd.send_angle_camera(90) == -1):
+            by_cmd.send_angle_camera(90)
+        filter.switch_camera(1)
+    def find(self):
+        # 目标检测通信塔
+        ret = filter.find(tlabel.TOWER)
+        if ret:
+            return True
+        return False
+    def exec(self):
+        logger.info("找到塔")
+        car_stop()
+        time.sleep(1)
+        calibrate_new(tlabel.TOWER, offset = 20, run = True)
+        time.sleep(1)
+        # calibrate(tlabel.TOWER, 27, False, 6)
+        by_cmd.send_distance_x(-10, 120)
+        time.sleep(1)
+        # 上古參數
+        # by_cmd.send_distance_y(-10, 50)
+        # 6_9 模型參數
+        by_cmd.send_distance_y(-10, 40)
+        # 7_12_3 模型參數
+        # by_cmd.send_distance_y(-10, 50)
+        # time.sleep(1)
+        car_stop()
+        # FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定
+        # time.sleep(3)
+        # by_cmd.send_speed_y(-10)
+        # time.sleep(0.15)
+        
+        by_cmd.send_angle_zhuan(10)
+        time.sleep(12)
+        by_cmd.send_speed_y(10)
+        time.sleep(0.3)
+        car_stop()
+        by_cmd.send_angle_zhuan(0)
+        # while True:
+        #     pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
+        # 下一动作预备位置
+        by_cmd.send_position_axis_z(30, 0)
+        # 任务检查间隔
+        time.sleep(4)
+
+
+# 高空排险
+class get_rball():
+    def init(self):
+        filter.switch_camera(1)
+        logger.info("高空排险初始化")
+        while (by_cmd.send_angle_camera(180) == -1):
+            by_cmd.send_angle_camera(180)
+        self.record = CountRecord(3)
+    def find(self):
+        # 目标检测红球
+        ret = filter.find(tlabel.RBALL)
+        if ret > 0:
+            # TODO 连续两帧才开始减速
+            if self.record(tlabel.RBALL):
+                var.task_speed = 5
+            return True
+        else:
+            return False
+    def exec(self):
+        logger.info("找到红球")
+        var.task_speed = 0
+        car_stop()
+        time.sleep(0.5)
+        # 靠近塔
+        by_cmd.send_angle_scoop(20)
+        # 上古參數
+        # by_cmd.send_distance_y(-15, 45) # 50
+        # 6_9 參數
+        by_cmd.send_distance_y(-15, 35)
+        time.sleep(2)
+        # 7_12_3 參數
+        # by_cmd.send_distance_y(-15, 45)
+        # time.sleep(2)
+        car_stop()
+        calibrate_new(tlabel.RBALL,offset = 44, run = True)
+        time.sleep(1)
+        logger.info("抓红球")
+        # by_cmd.send_angle_scoop(15)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(30, 170)
+        time.sleep(2.5)
+        by_cmd.send_angle_scoop(7)
+        time.sleep(0.5)
+        by_cmd.send_speed_y(15)
+        time.sleep(0.2)
+        car_stop()
+        # by_cmd.send_angle_omega(-55,30)
+        # while True:
+        #     pass
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["get_rball"]["pid_kp"], cfg["get_rball"]["pid_ki"], cfg["get_rball"]["pid_kd"])
+        # 任务检查间隔
+        time.sleep(2)
+
+
+# 派发物资
+class put_bball():
+    def init(self):
+        logger.info("派发物资初始化")
+        filter.switch_camera(1)
+        while by_cmd.send_position_axis_z(30, 0) == -1:
+            pass
+        while by_cmd.send_angle_camera(90) == -1:
+            pass
+    def find(self):
+        ret = filter.find(tlabel.BASKET)
+        if ret > 0:
+            return True
+        else:
+            return False
+    def exec(self):
+        logger.info("找到篮筐")
+        car_stop()
+        calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6)
+        # by_cmd.send_distance_x(10, 10)
+        # 向左运动
+        # by_cmd.send_distance_y(-10, 35)
+        # by_cmd.send_angle_storage(10)
+        # time.sleep(1)
+
+        by_cmd.send_angle_storage(50)
+        logger.info("把球放篮筐里")
+
+        time.sleep(1)
+        # by_cmd.send_distance_y(10, 55)
+        by_cmd.send_angle_storage(20)
+        # time.sleep(1)
+        # car_stop()
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
+        # 任务检查间隔
+        time.sleep(2)
+
+
+# 物资盘点
+class put_hanoi1():
+    def init(self):
+        logger.info("物资盘点 1 初始化")
+        filter.switch_camera(2)
+    def find(self):
+        ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK])
+        if label == tlabel.RMARK:
+            if abs(error) < 55:
+                logger.info("向右拐")
+                utils.direction_right += 1
+                return True
+            return False
+        elif label == tlabel.LMARK:
+            if abs(error) < 50:
+                logger.info("向左拐")
+                utils.direction_left += 1
+                return True
+            return False
+        else:
+            return False
+    def exec(self):
+        global direction
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        time.sleep(0.2)
+        
+        by_cmd.send_position_axis_z(30, 130)
+
+        # 校准牌子
+        if utils.direction_right > utils.direction_left:
+            ret, error = filter.aim_near(tlabel.RMARK)
+            while not ret:
+                ret, error = filter.aim_near(tlabel.RMARK)
+            utils.direction = tlabel.RMARK
+            logger.info("应该向右转")
+        else:
+            ret, error = filter.aim_near(tlabel.LMARK)
+            while not ret:
+                ret, error = filter.aim_near(tlabel.LMARK)
+            utils.direction = tlabel.LMARK
+            logger.info("应该向左转")
+
+
+        # 校准 omega
+        for _ in range(10):
+            ret, box = filter.get(utils.direction)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+            by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.2)
+        car_stop()
+
+
+        # 前进
+        # by_cmd.send_distance_x(10, 200)
+        # by_cmd.send_distance_x(10, 180)
+        # by_cmd.send_distance_x(10, 180)
+        # time.sleep(1.5)
+        # car_stop()
+        
+        while True:
+            by_cmd.send_speed_x(8.5)
+            ret, box = filter.get(utils.direction)
+            if ret:
+                if abs(box[0][2] - box[0][0]) > 41:
+                    car_stop()
+                    break
+        
+        # 根据方向初始化执行器位置
+        if utils.direction is tlabel.RMARK:
+            # FIXME 右侧的爪子会被 storage 挡住
+            by_cmd.send_position_axis_x(1, 0)
+            by_cmd.send_angle_claw_arm(45)
+            by_cmd.send_angle_storage(0)
+        else:
+            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_angle_claw_arm(225)
+            by_cmd.send_angle_storage(55)
+        time.sleep(1)
+
+        by_cmd.send_position_axis_z(30, 10)
+
+        if utils.direction_right > utils.direction_left:
+            utils.direction = tlabel.RMARK
+            # by_cmd.send_angle_omega(-25,430)
+            # by_cmd.send_angle_omega(-55,194)
+            # by_cmd.send_angle_omega(-45,238)
+            # by_cmd.send_angle_omega(-45,252)
+            by_cmd.send_angle_omega(-45,260)
+            time.sleep(2)
+            while (by_cmd.send_angle_camera(90) == -1):
+                by_cmd.send_angle_camera(90)
+        else:
+            utils.direction = tlabel.LMARK
+            # by_cmd.send_angle_omega(25,430)
+            # by_cmd.send_angle_omega(55,194)
+            # by_cmd.send_angle_omega(45,238)
+            # by_cmd.send_angle_omega(45,252)
+            by_cmd.send_angle_omega(45,260)
+            time.sleep(2)
+            while (by_cmd.send_angle_camera(0) == -1):
+                by_cmd.send_angle_camera(0)
+        time.sleep(0.5)
+        filter.switch_camera(1)
+    def nexec(self):
+        pass
+    def after(self):
+        # var.switch_lane_model = True
+        if utils.direction == tlabel.RMARK:
+            var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
+        else:
+            var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
+        pass
+        # time.sleep(1.5)
+
+class put_hanoi2():
+    def __init__(self):
+        
+        if cfg['put_hanoi2']['first_target'] == "lp":
+            self.target_label = tlabel.LPILLER
+        elif cfg['put_hanoi2']['first_target'] == "mp":
+            self.target_label = tlabel.MPILLER
+        elif cfg['put_hanoi2']['first_target'] == "sp":
+            self.target_label = tlabel.SPILLER
+    def init(self):
+        logger.info("物资盘点 2 初始化")
+        var.task_speed = 8.5
+        if utils.direction == tlabel.RMARK:
+            # 15
+            self.offset = 14
+            # self.platform_offset = -25
+            # self.platform_offset = -19
+            self.platform_offset = -15
+        else:
+            self.offset = 14
+            # self.platform_offset = -30
+            # self.platform_offset = -19
+            self.platform_offset = -15
+        # 延时，防止过早看到 tplatform（虽然此现象相当少见且诡异）
+        time.sleep(1.5)
+
+    def find(self):
+        # ret, box = filter.get(self.target_label)
+        ret, box = filter.get(tlabel.TPLATFORM)
+        if ret:
+            error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset
+            if error > 0:
+                return True
+        return False
+    def exec(self): 
+        logger.info(f"direction:{utils.direction.name}")
+        var.task_speed = 0
+        car_stop()
+
+        # if utils.direction is tlabel.RMARK:
+        #     by_cmd.send_distance_y(10, 50)
+        #     time.sleep(1)
+
+        # time.sleep(0.5)
+        # by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
+        # time.sleep(1)
+        ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
+        if not ret:
+            logger.error("跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("抓大平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_angle_claw(63)
+            time.sleep(2)
+            by_cmd.send_angle_claw(40)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, 30)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_angle_claw(63)
+            time.sleep(2)
+            by_cmd.send_angle_claw(40)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, 30)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1)
+            pass
+        ret = explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
+        if not ret:
+            logger.error("跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("放大平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_x(1, 150)
+            time.sleep(1.5)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(1)
+            by_cmd.send_angle_claw(81)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
+            time.sleep(1)
+            pass
+        else:
+            by_cmd.send_position_axis_x(1, 40)
+            time.sleep(1.5)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(1)
+            by_cmd.send_angle_claw(81)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
+            time.sleep(1)
+
+        ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed =  5)
+        if not ret:
+            logger.error("跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("抓中平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_angle_claw(55)
+            time.sleep(2)
+            by_cmd.send_angle_claw(35)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_angle_claw(55)
+            time.sleep(2)
+            by_cmd.send_angle_claw(35)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1)
+            pass
+        # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
+        if not ret:
+            logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("放中平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 120)
+            time.sleep(2)
+            by_cmd.send_position_axis_x(1, 150)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(55)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 120)
+            time.sleep(2)
+            by_cmd.send_position_axis_x(1, 40)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(55)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1)
+
+        ret = explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
+        if not ret:
+            logger.error("跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("抓小平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_angle_claw(50)
+            time.sleep(2)
+            by_cmd.send_angle_claw(27)
+            time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 10)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 0)
+            time.sleep(2)
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_angle_claw(50)
+            time.sleep(2)
+            by_cmd.send_angle_claw(27)
+            time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 10)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 170)
+            time.sleep(2)
+            pass
+        # ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        if not ret:
+            logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("放小平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 190) # 170
+            time.sleep(1.5)
+            by_cmd.send_position_axis_x(1, 150)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(80)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
+
+            # by_cmd.send_speed_y(10)
+            # time.sleep(0.12)
+            # car_stop()
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 190)
+            time.sleep(1.5)
+            by_cmd.send_position_axis_x(1, 40)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(80)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1.5)
+        # while True:
+        #     pass
+        by_cmd.send_speed_x(12)
+        time.sleep(1.5)
+    def nexec(self):
+        pass
+    def after(self):
+        # var.switch_lane_model = False
+        if utils.direction is tlabel.RMARK:
+            var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
+        else:
+            var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) 
+        # time.sleep(2)
+        pass
+
+class put_hanoi3():
+    def init(self):
+        while by_cmd.send_position_axis_z(30, 130) == -1:
+            pass
+        time.sleep(3)
+        logger.info("完成任务，爪回左侧")
+        while by_cmd.send_angle_claw_arm(128) == -1:
+            pass
+        time.sleep(0.5)
+        while by_cmd.send_position_axis_x(1, 0) == -1:
+            pass
+        time.sleep(1)
+        # while by_cmd.send_angle_claw_arm(225) == -1:
+        #     pass
+        while by_cmd.send_angle_claw(85) == -1:
+            pass
+    def find(self):
+        time.sleep(1)
+        return True
+    def exec(self):
+        while by_cmd.send_position_axis_z(30, 100) == -1:
+            pass
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        by_cmd.send_position_axis_x(1, 150)
+        var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
+
+# 应急避险 第一阶段 找目标牌
+class move_area1():
+    def init(self):
+        logger.info("应急避险第一阶段初始化")
+        while (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+    def find(self):
+        ret = filter.find(tlabel.SIGN)
+        if ret:
+            return True
+        return False
+    def exec(self):
+        logger.info("找到标示牌")
+        # 停车 ocr 识别文字 调用大模型
+        car_stop()
+        time.sleep(1)
+        # calibrate_new(tlabel.SIGN, offset = 8, run = True)
+        calibrate_new(tlabel.SIGN, offset = -30, run = True, run_speed = 5)
+        time.sleep(1)
+
+        by_cmd.send_position_axis_x(1, 50)
+        time.sleep(1)
+
+        # filter_w = (148, 560)
+        # filter_h = (165, 390)
+        
+        counts = 0
+        while True:
+            ocr_socket.send_string("")
+            resp = ocr_socket.recv_pyobj()
+            var.llm_text = ''
+            counts += 1
+            if resp.get('code') == 0:
+                for item in resp.get('content'):
+                    
+                    if item['probability']['average'] < 0.80:
+                        continue
+                    # box = item['location']
+                    # center_x = box['left'] + box['width'] / 2
+                    # center_y = box['top'] + box['height'] / 2
+                    # if center_x < filter_w[0] or center_x > filter_w[1] \
+                    #         or center_y < filter_h[0] or center_y > filter_h[1]:
+                    #     continue
+                    var.llm_text += item['words']
+                break
+            if counts >= 2:
+                var.skip_llm_task_flag = True
+                return
+        logger.error(var.llm_text)
+        if len(var.llm_text) < 3:
+            var.skip_llm_task_flag = True
+            return
+
+        var.task_speed = 9 # 12
+
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        # 任务检查间隔
+        by_cmd.send_position_axis_x(1, 150)
+        # time.sleep(1)
+        by_cmd.send_angle_claw_arm(225)
+
+        pass
+
+# 应急避险 第二阶段 找停车区域
+class move_area2():
+    def init(self):
+        logger.info("应急避险第二阶段初始化")
+        self.offset = 15
+        self.delta_x = 0
+        self.delta_y = 0
+        self.delta_omage = 0
+    def find(self):
+        # time.sleep(0.001)
+        if var.skip_llm_task_flag:
+            return 5000
+        ret, box = filter.get(tlabel.SHELTER)
+        if ret:
+            error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
+            if abs(error) < 20:
+                return 5000
+        return False
+    def sub_light(self, delay_time):
+        by_cmd.send_light(1)
+        time.sleep(delay_time)
+        by_cmd.send_light(0)
+    def sub_beep(self,delay_time):
+        by_cmd.send_beep(1)
+        time.sleep(delay_time)
+        by_cmd.send_beep(0)
+    def sub_move(self, x, y):
+        # FIXME 如果同時有 xy，是否會造成 delay 不足
+        self.delta_x += x
+        self.delta_y += y
+        
+        if x != 0:
+            delay_time = int(abs(x) * 500)
+            if x > 0:
+                by_cmd.send_distance_x(15, delay_time)
+            else:
+                by_cmd.send_distance_x(-15, delay_time)
+        elif y != 0:
+            delay_time = int(abs(y) * 500)
+            if y > 0: # 向左
+                by_cmd.send_distance_y(-15, delay_time)
+            else:
+                by_cmd.send_distance_y(15, delay_time)
+        time.sleep(delay_time / 500)
+        car_stop()
+        pass
+    def sub_turn(self, angle):
+        self.delta_omage += angle
+        delay_time = int(abs(angle) * 400 / 90)
+        if angle < 0:
+            # 左转
+            by_cmd.send_angle_omega(+55, delay_time)
+        else:
+            # 右转
+            by_cmd.send_angle_omega(-55, delay_time)
+        time.sleep(delay_time / 300 * 1.5)
+    def exec(self):
+        var.task_speed = 0
+        if var.skip_llm_task_flag:
+            logger.error("ocr 识别出错 直接跳过改任务")
+            return
+        logger.info("开始寻找停车区域")
+        car_stop()
+        calibrate_new(tlabel.SHELTER, offset = 15, run = True)
+        time.sleep(0.5)
+        # 调用大模型 然后执行动作
+        try:
+            resp = llm_bot.get_command_json(var.llm_text)
+            logger.info(resp)
+        except:
+            logger.error("大模型超时，跳过任务")
+            return
+        
+        
+        try:
+            resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0])
+            if len(resp_commands) == 0:
+                return
+            # 进入停车区域
+            # by_cmd.send_speed_y(15)
+            by_cmd.send_distance_y(25, 180)
+            time.sleep(1)
+            # time.sleep(1.25)
+            car_stop()
+            logger.info(resp_commands)
+            for command in resp_commands:
+                logger.info(command)
+                if command['func'] == 'move':
+                    self.sub_move(float(command['x']), float(command['y']))
+                elif command['func'] == 'light':
+                    self.sub_light(int(command['time']))
+                elif command['func'] == 'beep':
+                    self.sub_beep(int(command['time']))
+                elif command['func'] == 'turn':
+                    self.sub_turn(int(command['angle']))
+                    pass
+                else:
+                    continue
+                time.sleep(0.5)
+        except:
+            pass
+        time.sleep(1)
+        # 回到原位
+        
+        delay_time = int(abs(self.delta_omage) * 400 / 90)
+        if int(abs(self.delta_omage)) == 360:
+            delay_time = 0
+        if self.delta_omage < 0:
+            # 左转
+            by_cmd.send_angle_omega(-55, delay_time)
+        else:
+            # 右转
+            by_cmd.send_angle_omega(55, delay_time)
+        time.sleep(delay_time / 300 * 1.5)
+        if self.delta_y > 0:
+            # 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了
+            delay_time = 180 - (self.delta_y * 500)
+        else:
+            delay_time = 180 + (abs(self.delta_y) * 500)
+        # 离开停车区域
+        by_cmd.send_distance_y(-25, delay_time)
+
+        time.sleep(delay_time * 5e-3)
+        
+        car_stop()
+
+        # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值
+        # by_cmd.send_distance_y(-15, 300)
+        pass
+    def nexec(self):
+        logger.warning("正在跳過大模型任務")
+        time.sleep(2)
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
+        by_cmd.send_position_axis_z(30, 0)
+        while by_cmd.send_angle_claw(90) == -1:
+            pass
+        time.sleep(2)
+
+# 扫黑除暴
+class kick_ass():
+    def init(self):
+        while (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+        logger.info("扫黑除暴初始化")
+        self.pos_gap1 = cfg['kick_ass']['pos_gap1']
+        self.pos_gap2 = cfg['kick_ass']['pos_gap2']
+        self.target_person = cfg_args['lane_mode']['mode_index']
+
+        # by_cmd.send_angle_claw(15)
+        by_cmd.send_position_axis_x(1, 160)
+
+    def find(self):
+        ret = filter.find(tlabel.SIGN)
+        if ret:
+            return True
+        return False
+    def exec(self):
+        logger.info("找到标示牌")
+        # 停的晚无需校准 omage
+        car_stop()
+        time.sleep(1)
+        calibrate_new(tlabel.SIGN, offset = 8, run = True)
+        by_cmd.send_angle_claw(15)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(30, 60)
+        by_cmd.send_position_axis_x(1, 130)
+
+        if self.target_person == 1:
+            target_distance = self.pos_gap1
+        else:
+            target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
+        by_cmd.send_distance_x(10, target_distance)
+        
+        logger.info(f"target distance {target_distance}")
+        time.sleep(1.5 + (self.target_person - 1) * 0.7 )
+        car_stop()
+        
+        # by_cmd.send_angle_claw_arm(225)
+        # time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(3)
+        by_cmd.send_position_axis_x(1, 120)
+        time.sleep(1)
+        logger.debug("結束任務，前進四")
+        # by_cmd.send_speed_x(25)
+        # time.sleep(4)
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
\ No newline at end of file
diff --git a/subtask_raw.py b/subtask_raw.py
new file mode 100644
index 0000000..095f114
--- /dev/null
+++ b/subtask_raw.py
@@ -0,0 +1,1496 @@
+from enum import Enum
+from loguru import logger
+from utils import label_filter
+from utils import tlabel
+from utils import LLM
+from utils import CountRecord
+import utils
+import toml
+import zmq
+import time
+import variable as var
+import action as act
+import re
+import threading
+import ctypes
+cfg = None
+cfg_args = None
+by_cmd = None
+filter = None
+llm_bot = None
+
+# 目标检测 socket 客户端
+context = None
+socket = None
+
+context1 = None
+ocr_socket = None
+
+global_skip_queue = None
+
+'''
+description: main.py 里执行 引入全局变量
+param {*} _by_cmd 控制器对象
+return {*}
+'''
+def import_obj(_by_cmd, skip_queue):
+
+    global by_cmd
+    global filter
+    global llm_bot
+
+    global context
+    global socket
+
+    global context1
+    global ocr_socket
+
+    global cfg
+    global cfg_args
+    global global_skip_queue
+    cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
+    cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.toml')
+    by_cmd = _by_cmd
+    global_skip_queue = skip_queue
+
+    # 目标检测 socket 客户端
+    context = zmq.Context()
+    socket = context.socket(zmq.REQ)
+    socket.connect("tcp://localhost:6667")
+    logger.info("subtask yolo client init")
+
+    # ocr socket 客户端
+    # context1 = zmq.Context()
+    # ocr_socket = context1.socket(zmq.REQ)
+    # ocr_socket.connect("tcp://localhost:6668")
+    # logger.info("subtask ocr client init")
+
+    filter = label_filter(socket)
+    if cfg['move_area']['llm_enable']:
+        llm_bot = LLM()
+def car_stop():
+    for _ in range(3):
+        by_cmd.send_speed_x(0)
+        time.sleep(0.2)
+        by_cmd.send_speed_y(0)
+        time.sleep(0.2)
+        by_cmd.send_speed_omega(0)
+# 蓝色球使用
+def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
+
+    not_found_counts = 0
+    ret, error = filter.aim_right(label)
+    while not ret:
+        not_found_counts += 1
+        if not_found_counts >= 20:
+            not_found_counts = 0
+            error = -320 # error > 0 front run
+            logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找")
+            break
+        ret, error = filter.aim_right(label)
+
+    error += offset
+    if abs(error) > 10 and run:
+        if error > 0:
+            by_cmd.send_speed_x(-run_speed)
+        else:
+            by_cmd.send_speed_x(run_speed)
+    # 停的位置已经很接近目标，可以直接使用 distance 校准
+    else:
+        return
+    
+    while True:
+        ret, error = filter.aim_right(label)
+        while not ret:
+            ret, error = filter.aim_right(label)
+        error += offset
+        # if ret:
+        if abs(error) <= 8:
+            car_stop()
+            logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
+            ret, error = filter.aim_right(label)
+            while not ret:
+                not_found_counts += 1
+                if not_found_counts >=30:
+                    return
+                ret, error = filter.aim_right(label)
+            error += offset
+            logger.info(f"calibrate_right_new:停车后的误差是{error}")
+            if abs(error) > 8:
+                logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
+                error = error * 1.5
+                if error > 0:
+                    by_cmd.send_distance_x(-10, int(error))
+                else:
+                    by_cmd.send_distance_x(10, int(-error))
+            
+            break
+'''
+description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车
+如果停车后 error > 8 则使用 distance 校准 
+这个方法仅用于在视野里能找到的情况下进行校准，并不能实现边走边寻找 然后再校准
+param {*} label
+param {*} offset
+param {*} run
+param {*} run_speed
+return {*}
+'''     
+def calibrate_new(label, offset, run = True, run_speed = 3.5):
+    not_found_counts = 0
+    ret, box = filter.get(label)
+    while not ret:
+        not_found_counts += 1
+        if not_found_counts >= 20:
+            not_found_counts = 0
+            error = -320 # error > 0 front run
+            logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找")
+            break
+        ret, box = filter.get(label)
+    
+    # 如果超过二十帧跳出，则此时 box 为空值，需要再校验 ret
+    if ret is True:
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+    if abs(error) > 10 and run:
+        if error > 0:
+            by_cmd.send_speed_x(-run_speed)
+        else:
+            by_cmd.send_speed_x(run_speed)
+    # 停的位置已经很接近目标，可以直接使用 distance 校准
+    else:
+        if abs(error) > 8:
+            logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准")
+            # error = error # 3
+            if error > 0:
+                by_cmd.send_distance_x(-10, int(error))
+            else:
+                by_cmd.send_distance_x(10, int(-error))
+            return
+        logger.info(f"calibrate_new:停车后误差{error}小于 8  不校准")
+        return 
+    while True:
+        ret, box = filter.get(label)
+        while not ret:
+            ret, box = filter.get(label)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret:
+            if abs(error) <= 10: # 5
+                car_stop()
+                logger.info("calibrate_new:行进时 误差小于 10 直接停车")
+
+                ret, box = filter.get(label)
+                while not ret:
+                    ret, box = filter.get(label)
+
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"calibrate_new:停车后的误差是{error}")
+                if abs(error) > 8:
+                    logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准")
+                    error = error * 3
+                    # logger.error(f"error * 3  {error}")
+                    if error > 0:
+                        by_cmd.send_distance_x(-10, int(error))
+                    else:
+                        by_cmd.send_distance_x(10, int(-error))
+                break
+# 弃用 distance 校准
+def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
+    # run_direc == 1 向前
+    stop_error = 0
+    if run_direc == 1:
+        by_cmd.send_speed_x(run_speed)
+    else:
+        by_cmd.send_speed_x(-run_speed)
+        
+    if label == tlabel.TPLATFORM:
+        stop_error = 8
+    else:
+        stop_error = 15
+    while True:
+        ret, box = filter.get(label)
+        while not ret:
+            if not global_skip_queue.empty():
+                _ = global_skip_queue.get()
+                logger.error("跳过 explore_calibrate_new")
+                return False
+            ret, box = filter.get(label)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret:
+            # 校准速度越大 停车的条件越宽泛 20 15
+            if abs(error) <= stop_error:
+                car_stop()
+                logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
+                
+                ret, box = filter.get(label)
+                while not ret:
+                    ret, box = filter.get(label)
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"停车后像素误差:{error}")
+                
+                # logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
+                # if abs(error) < 8:
+                #     error = error * 3
+                # else:
+                # error = error * 2
+                # if error > 0:
+                #     by_cmd.send_distance_x(-10, int(error))
+                # else:
+                #     by_cmd.send_distance_x(10, int(-error))
+                
+                break
+    return True
+
+# 对准应知道是左还是右，右侧需在过滤器中进行翻转
+# flipv 为垂直翻转标志，转右侧开启
+def hanoi_calibrate(target_label, error_label, offset, run_direc ,run_speed = 3.5):
+    stop_error = 0
+    error_record = CountRecord(10)
+    if run_direc == 1:
+        by_cmd.send_speed_x(run_speed)
+    else:
+        by_cmd.send_speed_x(-run_speed)
+        
+    if target_label == tlabel.TPLATFORM:
+        stop_error = 8
+    else:
+        stop_error = 15
+    while True:
+        ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
+        while not ret1:
+            # 如果找不到目标且跳过任务队列非空 (即指令跳过)
+            if not global_skip_queue.empty():
+                _ = global_skip_queue.get()
+                logger.error("跳过 hanoi_calibrate")
+                return False
+            # 如果找不到目标且发现错误目标 (上次放置任务失败)
+            if ret2:
+                # 如果连续计数超过阈值，则直接返回
+                if error_record(ret2):
+                    return False
+            ret1, ret2, box = filter.get_two_hanoi(target_label, error_label, utils.direction == tlabel.RMARK)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret1:
+            # 校准速度越大 停车的条件越宽泛 20 15
+            if abs(error) <= stop_error:
+                car_stop()
+                logger.info("explore_calibrate_new:行进时 误差小于 15 直接停车")
+                
+                ret, box = filter.get(target_label)
+                while not ret:
+                    ret, box = filter.get(target_label)
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"停车后像素误差:{error}")
+                break
+    return True
+# 任务类
+class task:  
+    def __init__(self, name, task_template, find_counts=10, enable=True):  
+        self.enable = enable
+        self.task_t = task_template()
+
+        self.counts = 0
+        self.find_counts = int(find_counts)
+        self.name = name
+
+    def init(self):
+        if hasattr(self.task_t, 'init') and callable(getattr(self.task_t, 'init', None)):
+            self.task_t.init()
+        else:
+            logger.warning("[Task ]# 该任务没有 init 方法")
+    def find(self):
+        if hasattr(self.task_t, 'find') and callable(getattr(self.task_t, 'find', None)):
+            return self.task_t.find()
+        else:
+            logger.warning("[Task ]# 该任务没有 find 方法")
+    def exec(self):  
+        # 根据标志位确定是否执行该任务
+        if self.enable is True:
+            if hasattr(self.task_t, 'exec') and callable(getattr(self.task_t, 'exec', None)):
+                logger.info(f"[Task ]# Executing task")  
+                self.task_t.exec()
+            else:
+                logger.warning("[Task ]# 该任务没有 exec 方法")
+        else:
+            logger.warning(f"[Task ]# Skip task")
+            if hasattr(self.task_t, 'nexec') and callable(getattr(self.task_t, 'nexec', None)):
+                self.task_t.nexec()
+            else:
+                logger.warning("[Task ]# 该任务没有 nexec 方法")
+    def after(self):
+        if hasattr(self.task_t, 'after') and callable(getattr(self.task_t, 'after', None)):
+            logger.info(f"[Task ]# {self.name} 正在执行 after")
+            self.task_t.after()
+            logger.debug(f"[Task ]# Task completed")  
+        else:
+            logger.warning("[Task ]# 该任务没有 after 方法")
+
+# 任务队列状态类
+class task_queuem_status(Enum):
+    IDEL      = 0
+    SEARCHING = 1
+    EXECUTING = 2
+
+# 任务队列类 非 EXECUTEING 时均执行 huigui，注意互斥操作
+class task_queuem(task):
+    # task_now = task(None, False)
+    def __init__(self, queue):
+        super(task_queuem, self)
+        self.queue = queue
+        self.status = task_queuem_status.IDEL
+        self.busy = True
+        logger.info(f"[TaskM]# Task num {self.queue.qsize()}")
+        # exec 线程
+        self.exec_thread = None
+    def exec(self, skip_queue):
+        # 如果空闲状态则将下一个队列任务取出
+        if self.status is task_queuem_status.IDEL:
+            if self.queue.qsize() == 0:
+                self.busy = False
+                logger.info(f"[TaskM]# Task queue empty, exit")
+                return False
+            self.task_now = self.queue.get()
+
+            # 如果当前任务没有使能，则直接转入执行状态，由任务执行函数打印未执行信息
+            if self.task_now.enable is True:
+                self.status = task_queuem_status.SEARCHING
+                # 如果使能该任务则执行该任务的初始化动作
+                self.task_now.init()
+            else:
+                self.status = task_queuem_status.EXECUTING
+            logger.info(f"[TaskM]# ---------------------->>>>")
+        # 阻塞搜索任务标志位
+        elif self.status is task_queuem_status.SEARCHING:
+            logger.info(f"[{self.task_now.name}]# Start searching task target")
+            while True:
+                if not skip_queue.empty():
+                    _ = skip_queue.get()
+                    logger.error(f"{self.task_now.name} 任务在 find 中已经被手动跳过")
+                    self.status = task_queuem_status.IDEL   # 空动作不需要阻塞巡线，直接置位
+                    self.task_now.after()                   # 执行任务后处理
+                    self.queue.task_done()                  # 弹出已执行的任务
+                    return True
+                ret = self.task_now.find()
+                self.task_now.counts += ret
+                if self.task_now.counts >= self.task_now.find_counts:
+                    self.status = task_queuem_status.EXECUTING
+                    break
+        # 执行任务函数
+        elif self.status is task_queuem_status.EXECUTING:
+            if self.task_now.enable is True:
+                logger.info(f"[TaskM]# Start execute task function")
+                # self.exec_thread = threading.Thread(target=self.task_now.exec)
+                # # 启动线程
+                # self.exec_thread.start()
+                # while True:
+                #     if not self.exec_thread.is_alive():
+                #         break
+                #     else:
+                #         if not skip_queue.empty():
+                #             car_stop()
+                #             thread_id = self.exec_thread.ident
+                #             res = ctypes.pythonapi.PyThreadState_SetAsyncExc(thread_id, ctypes.py_object(SystemExit))
+                #             _ = skip_queue.get()
+                #             logger.error(f"{self.task_now.name} 任务在 exec 中已经被手动跳过")
+                #             self.status = task_queuem_status.IDEL   # 空动作不需要阻塞巡线，直接置位
+                #             self.task_now.after()                   # 执行任务后处理
+                #             self.queue.task_done()                  # 弹出已执行的任务
+                #             return True
+                self.task_now.exec()                    # 执行当前任务函数
+                self.status = task_queuem_status.IDEL   # 执行完成后为退出巡线阻塞
+                self.task_now.after()                   # 执行任务后处理
+                self.queue.task_done()                  # 弹出已执行的任务
+                logger.info(f"[TaskM]# <<<<----------------------")
+            else:
+                logger.info(f"[TaskM]# Start execute task function (nexec)")
+                self.status = task_queuem_status.IDEL   # 空动作不需要阻塞巡线，直接置位
+                self.task_now.exec()                    # 执行当前任务函数
+                self.task_now.after()                   # 执行任务后处理
+                self.queue.task_done()                  # 弹出已执行的任务
+                logger.info(f"[TaskM]# <<<<----------------------")
+        return True
+
+# 人员施救
+class get_block1():
+    def init(self):
+        var.task_speed = 15
+        act.cmd.camera(0)
+        act.cmd.z2(20, 60, 0)
+        filter.switch_camera(1)
+        # if cfg['get_block']['first_block'] == "blue":
+        #     self.target_label = tlabel.BBLOCK
+        #     self.another_label = tlabel.RBLOCK
+        # else:
+        #     self.target_label = tlabel.RBLOCK
+        #     self.another_label = tlabel.BBLOCK
+        #     cfg['get_block']['first_block'] = "red"
+        self.target_label = [tlabel.RBLOCK, tlabel.BBLOCK]
+        self.target_counts = [0, 0]
+    def find(self):
+        # 目标检测红/蓝方块
+        # ret = filter.find(self.target_label)
+        # if ret > 0:
+        #     return True
+        # return False
+        ret = filter.find_mult(self.target_label)
+        self.target_counts[0] += ret[0]
+        self.target_counts[1] += ret[1]
+        if any(ret):
+            return True
+        return False
+
+
+    def exec(self):
+        car_stop()
+        if self.target_counts[0] > self.target_counts[1]:
+            var.first_block = tlabel.RBLOCK
+            var.second_block = tlabel.BBLOCK
+        else:
+            var.first_block = tlabel.BBLOCK
+            var.second_block = tlabel.RBLOCK
+        calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
+        logger.info("抓取块")
+
+        by_cmd.send_position_axis_z(30, 60)
+        time.sleep(1)
+        by_cmd.send_angle_claw(63)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(1)
+        by_cmd.send_angle_claw(25)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(30, 90)
+        time.sleep(0.5)
+
+        by_cmd.send_angle_claw_arm(175)
+        time.sleep(0.1)
+        by_cmd.send_position_axis_x(1, 100)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 70)
+        time.sleep(0.5)
+        by_cmd.send_angle_claw(63)
+        time.sleep(0.5)
+
+        by_cmd.send_position_axis_z(30, 130)
+        time.sleep(1)
+        by_cmd.send_position_axis_x(1, 140)
+        by_cmd.send_angle_claw_arm(225)
+        time.sleep(0.5)
+        # by_cmd.send_angle_storage(55)
+        # time.sleep(1)
+        by_cmd.send_position_axis_z(30, 60)
+
+        pass
+    def nexec(self):
+        # TODO 完成不执行任务的空动作
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
+        pass
+
+class get_block2():
+    def init(self):
+        while (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+        filter.switch_camera(1)
+        # if cfg['get_block']['first_block'] == "red":
+        #     self.target_label = tlabel.BBLOCK
+        #     self.another_label = tlabel.RBLOCK
+        # else:
+        #     self.target_label = tlabel.RBLOCK
+        #     self.another_label = tlabel.BBLOCK
+    def find(self):
+        # 目标检测红/蓝方块
+        ret = filter.find(var.second_block)
+        if ret > 0:
+            return True
+        return False
+    def exec(self):
+        car_stop()
+        calibrate_new(var.second_block, offset = 16, run = True, run_speed = 5)
+        logger.info("抓取块")
+        time.sleep(0.5)
+        by_cmd.send_angle_claw_arm(225)
+        by_cmd.send_angle_claw(63)
+        time.sleep(0.1)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(1)
+        by_cmd.send_angle_claw(25)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 100)
+        
+        # by_cmd.send_distance_x(15, 100)
+        time.sleep(2)
+
+        pass
+    def nexec(self):
+        # TODO 完成不执行任务的空动作
+        pass
+    def after(self):
+        var.task_speed = 0
+        var.pid_turning.set(cfg["get_block"]["pid_kp"], cfg["get_block"]["pid_ki"], cfg["get_block"]["pid_kd"])
+        # 任务检查间隔
+        time.sleep(7)
+
+
+# 紧急转移
+class put_block():
+    def init(self):
+        var.task_speed = 0
+        while (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+        logger.info("紧急转移初始化")
+        filter.switch_camera(1)
+    def find(self):
+        # 目标检测医院
+        ret, box = filter.get(tlabel.HOSPITAL)
+        if ret > 0:
+            width = box[0][2] - box[0][0]
+            if width > 130:
+                return True
+        return False
+    def exec(self):
+        logger.info("找到医院")
+        car_stop()
+        calibrate_new(tlabel.HOSPITAL, offset = 7, run = False)
+
+        by_cmd.send_distance_x(10, 100)
+        by_cmd.send_position_axis_z(30, 0)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(1)
+        by_cmd.send_angle_claw(63)
+        time.sleep(1)
+
+        # 放置第二個塊
+        by_cmd.send_angle_storage(20)
+        by_cmd.send_position_axis_x(1, 110)
+        by_cmd.send_position_axis_z(30, 120)
+        time.sleep(1.5)
+        by_cmd.send_angle_claw_arm(180)
+        by_cmd.send_angle_claw(85)
+        # by_cmd.send_angle_storage(0)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30,70)
+        time.sleep(1)
+        by_cmd.send_angle_claw(25)
+        by_cmd.send_distance_x(-10, 110)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 110)
+        time.sleep(1)
+        by_cmd.send_angle_claw_arm(225)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 0)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(1.5)
+        by_cmd.send_angle_claw(45)
+        time.sleep(1)
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
+        # 下一动作预备位置
+        while by_cmd.send_position_axis_z(30, 130) == -1:
+            pass
+        time.sleep(1)
+        while by_cmd.send_position_axis_x(1, 0) == -1:
+            pass
+        while by_cmd.send_angle_claw_arm(45) == -1:
+            pass
+        # 任务检查间隔
+        # time.sleep(2)
+
+
+# 整装上阵
+class get_bball():
+    def init(self):
+        while (by_cmd.send_angle_camera(90) == -1):
+            by_cmd.send_angle_camera(90)
+        # by_cmd.send_position_axis_z(30, 135)
+        # time.sleep(0.5)
+        # by_cmd.send_position_axis_x(1, 0)
+        # time.sleep(2)
+        # by_cmd.send_angle_claw_arm(45)
+        
+        while (by_cmd.send_angle_storage(0) == -1):
+            by_cmd.send_angle_storage(0)
+
+        # 调试 临时换源
+        filter.switch_camera(1)
+        logger.info("整装上阵初始化")
+        # time.sleep(0.5)
+        
+        self.record = CountRecord(5)
+    def find(self):
+        # 目标检测蓝球
+        # ret = filter.find(tlabel.BBALL)
+        ret = filter.find_mult([tlabel.BBALL, tlabel.YBALL])
+        # ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL)
+        # TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关
+        # if ret:
+        #     if self.record(tlabel.BBALL):
+        #         return True
+        if ret[0] or ret[1]:
+            return True
+        return False
+    def exec(self):
+        logger.info("找到蓝色球")
+        car_stop()
+        time.sleep(0.5)
+        for _ in range(3):
+            calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5)
+            logger.info("抓蓝色球")
+            time.sleep(0.5)
+            by_cmd.send_angle_claw_arm(45)
+            by_cmd.send_angle_claw(54)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1.2)
+            by_cmd.send_angle_claw(25)
+            time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(1)
+            by_cmd.send_position_axis_x(1, 60)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, -40)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw_arm(80)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(54)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw_arm(45)
+            time.sleep(1)
+            by_cmd.send_position_axis_z(30, 135)
+            # 继续向前走
+            # by_cmd.send_speed_x(4)
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
+        # 下一动作预备位置
+        by_cmd.send_angle_claw(30)
+        time.sleep(0.5)
+        while by_cmd.send_position_axis_z(30, 0) == -1:
+            pass
+        time.sleep(1)
+        # # 任务检查间隔
+        # time.sleep(1)
+
+
+# 通信抢修
+class up_tower():
+    def init(self):
+        logger.info("通信抢修初始化")
+        while (by_cmd.send_angle_camera(90) == -1):
+            by_cmd.send_angle_camera(90)
+        filter.switch_camera(1)
+    def find(self):
+        # 目标检测通信塔
+        ret = filter.find(tlabel.TOWER)
+        if ret:
+            return True
+        return False
+    def exec(self):
+        logger.info("找到塔")
+        car_stop()
+        time.sleep(1)
+        calibrate_new(tlabel.TOWER, offset = 20, run = True)
+        time.sleep(1)
+        # calibrate(tlabel.TOWER, 27, False, 6)
+        by_cmd.send_distance_x(-10, 120)
+        time.sleep(1)
+        # 上古參數
+        # by_cmd.send_distance_y(-10, 50)
+        # 6_9 模型參數
+        by_cmd.send_distance_y(-10, 40)
+        # 7_12_3 模型參數
+        # by_cmd.send_distance_y(-10, 50)
+        # time.sleep(1)
+        car_stop()
+        # FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定
+        # time.sleep(3)
+        # by_cmd.send_speed_y(-10)
+        # time.sleep(0.15)
+        
+        by_cmd.send_angle_zhuan(10)
+        time.sleep(12)
+        by_cmd.send_speed_y(10)
+        time.sleep(0.3)
+        car_stop()
+        by_cmd.send_angle_zhuan(0)
+        # while True:
+        #     pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["up_tower"]["pid_kp"], cfg["up_tower"]["pid_ki"], cfg["up_tower"]["pid_kd"])
+        # 下一动作预备位置
+        by_cmd.send_position_axis_z(30, 0)
+        # 任务检查间隔
+        time.sleep(4)
+
+
+# 高空排险
+class get_rball():
+    def init(self):
+        filter.switch_camera(1)
+        logger.info("高空排险初始化")
+        while (by_cmd.send_angle_camera(180) == -1):
+            by_cmd.send_angle_camera(180)
+        self.record = CountRecord(3)
+    def find(self):
+        # 目标检测红球
+        ret = filter.find(tlabel.RBALL)
+        if ret > 0:
+            # TODO 连续两帧才开始减速
+            if self.record(tlabel.RBALL):
+                var.task_speed = 5
+            return True
+        else:
+            return False
+    def exec(self):
+        logger.info("找到红球")
+        var.task_speed = 0
+        car_stop()
+        time.sleep(0.5)
+        # 靠近塔
+        by_cmd.send_angle_scoop(20)
+        # 上古參數
+        # by_cmd.send_distance_y(-15, 45) # 50
+        # 6_9 參數
+        by_cmd.send_distance_y(-15, 35)
+        time.sleep(2)
+        # 7_12_3 參數
+        # by_cmd.send_distance_y(-15, 45)
+        # time.sleep(2)
+        car_stop()
+        calibrate_new(tlabel.RBALL,offset = 44, run = True)
+        time.sleep(1)
+        logger.info("抓红球")
+        # by_cmd.send_angle_scoop(15)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(30, 170)
+        time.sleep(2.5)
+        by_cmd.send_angle_scoop(7)
+        time.sleep(0.5)
+        by_cmd.send_speed_y(15)
+        time.sleep(0.2)
+        car_stop()
+        # by_cmd.send_angle_omega(-55,30)
+        # while True:
+        #     pass
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["get_rball"]["pid_kp"], cfg["get_rball"]["pid_ki"], cfg["get_rball"]["pid_kd"])
+        # 任务检查间隔
+        time.sleep(2)
+
+
+# 派发物资
+class put_bball():
+    def init(self):
+        logger.info("派发物资初始化")
+        filter.switch_camera(1)
+        while by_cmd.send_position_axis_z(30, 0) == -1:
+            pass
+        while by_cmd.send_angle_camera(90) == -1:
+            pass
+    def find(self):
+        ret = filter.find(tlabel.BASKET)
+        if ret > 0:
+            return True
+        else:
+            return False
+    def exec(self):
+        logger.info("找到篮筐")
+        car_stop()
+        calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6)
+        # by_cmd.send_distance_x(10, 10)
+        # 向左运动
+        # by_cmd.send_distance_y(-10, 35)
+        # by_cmd.send_angle_storage(10)
+        # time.sleep(1)
+
+        by_cmd.send_angle_storage(50)
+        logger.info("把球放篮筐里")
+
+        time.sleep(1)
+        # by_cmd.send_distance_y(10, 55)
+        by_cmd.send_angle_storage(20)
+        # time.sleep(1)
+        # car_stop()
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
+        # 任务检查间隔
+        time.sleep(2)
+
+
+# 物资盘点
+class put_hanoi1():
+    def init(self):
+        logger.info("物资盘点 1 初始化")
+        filter.switch_camera(2)
+    def find(self):
+        ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK])
+        if label == tlabel.RMARK:
+            if abs(error) < 55:
+                logger.info("向右拐")
+                utils.direction_right += 1
+                return True
+            return False
+        elif label == tlabel.LMARK:
+            if abs(error) < 50:
+                logger.info("向左拐")
+                utils.direction_left += 1
+                return True
+            return False
+        else:
+            return False
+    def exec(self):
+        global direction
+        for _ in range(3):
+            by_cmd.send_speed_x(0)
+            time.sleep(0.2)
+            by_cmd.send_speed_omega(0)
+        time.sleep(0.2)
+        
+        by_cmd.send_position_axis_z(30, 130)
+
+        # 校准牌子
+        if utils.direction_right > utils.direction_left:
+            ret, error = filter.aim_near(tlabel.RMARK)
+            while not ret:
+                ret, error = filter.aim_near(tlabel.RMARK)
+            utils.direction = tlabel.RMARK
+            logger.info("应该向右转")
+        else:
+            ret, error = filter.aim_near(tlabel.LMARK)
+            while not ret:
+                ret, error = filter.aim_near(tlabel.LMARK)
+            utils.direction = tlabel.LMARK
+            logger.info("应该向左转")
+
+
+        # 校准 omega
+        for _ in range(10):
+            ret, box = filter.get(utils.direction)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+            by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.2)
+        car_stop()
+
+
+        # 前进
+        # by_cmd.send_distance_x(10, 200)
+        # by_cmd.send_distance_x(10, 180)
+        # by_cmd.send_distance_x(10, 180)
+        # time.sleep(1.5)
+        # car_stop()
+        
+        while True:
+            by_cmd.send_speed_x(8.5)
+            ret, box = filter.get(utils.direction)
+            if ret:
+                if abs(box[0][2] - box[0][0]) > 41:
+                    car_stop()
+                    break
+        
+        # 根据方向初始化执行器位置
+        if utils.direction is tlabel.RMARK:
+            # FIXME 右侧的爪子会被 storage 挡住
+            by_cmd.send_position_axis_x(1, 0)
+            by_cmd.send_angle_claw_arm(45)
+            by_cmd.send_angle_storage(0)
+        else:
+            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_angle_claw_arm(225)
+            by_cmd.send_angle_storage(55)
+        time.sleep(1)
+
+        by_cmd.send_position_axis_z(30, 10)
+
+        if utils.direction_right > utils.direction_left:
+            utils.direction = tlabel.RMARK
+            # by_cmd.send_angle_omega(-25,430)
+            # by_cmd.send_angle_omega(-55,194)
+            # by_cmd.send_angle_omega(-45,238)
+            # by_cmd.send_angle_omega(-45,252)
+            by_cmd.send_angle_omega(-45,260)
+            time.sleep(2)
+            while (by_cmd.send_angle_camera(90) == -1):
+                by_cmd.send_angle_camera(90)
+        else:
+            utils.direction = tlabel.LMARK
+            # by_cmd.send_angle_omega(25,430)
+            # by_cmd.send_angle_omega(55,194)
+            # by_cmd.send_angle_omega(45,238)
+            # by_cmd.send_angle_omega(45,252)
+            by_cmd.send_angle_omega(45,260)
+            time.sleep(2)
+            while (by_cmd.send_angle_camera(0) == -1):
+                by_cmd.send_angle_camera(0)
+        time.sleep(0.5)
+        filter.switch_camera(1)
+    def nexec(self):
+        pass
+    def after(self):
+        # var.switch_lane_model = True
+        if utils.direction == tlabel.RMARK:
+            var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.1, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
+        else:
+            var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
+        pass
+        # time.sleep(1.5)
+
+class put_hanoi2():
+    def __init__(self):
+        
+        if cfg['put_hanoi2']['first_target'] == "lp":
+            self.target_label = tlabel.LPILLER
+        elif cfg['put_hanoi2']['first_target'] == "mp":
+            self.target_label = tlabel.MPILLER
+        elif cfg['put_hanoi2']['first_target'] == "sp":
+            self.target_label = tlabel.SPILLER
+    def init(self):
+        logger.info("物资盘点 2 初始化")
+        var.task_speed = 8.5
+        if utils.direction == tlabel.RMARK:
+            # 15
+            self.offset = 14
+            # self.platform_offset = -25
+            # self.platform_offset = -19
+            self.platform_offset = -15
+        else:
+            self.offset = 14
+            # self.platform_offset = -30
+            # self.platform_offset = -19
+            self.platform_offset = -15
+        # 延时，防止过早看到 tplatform（虽然此现象相当少见且诡异）
+        time.sleep(1.5)
+
+    def find(self):
+        # ret, box = filter.get(self.target_label)
+        ret, box = filter.get(tlabel.TPLATFORM)
+        if ret:
+            error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset
+            if error > 0:
+                return True
+        return False
+    def exec(self): 
+        logger.info(f"direction:{utils.direction.name}")
+        var.task_speed = 0
+        car_stop()
+
+        # if utils.direction is tlabel.RMARK:
+        #     by_cmd.send_distance_y(10, 50)
+        #     time.sleep(1)
+
+        # time.sleep(0.5)
+        # by_cmd.send_distance_x(-8, cfg['put_hanoi2']['pos_gap'] - 30)
+        # time.sleep(1)
+        ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
+        if not ret:
+            logger.error("跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("抓大平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_angle_claw(63)
+            time.sleep(2)
+            by_cmd.send_angle_claw(40)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, 30)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_angle_claw(63)
+            time.sleep(2)
+            by_cmd.send_angle_claw(40)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, 30)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1)
+            pass
+        ret = explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
+        if not ret:
+            logger.error("跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("放大平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_x(1, 150)
+            time.sleep(1.5)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(1)
+            by_cmd.send_angle_claw(81)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
+            time.sleep(1)
+            pass
+        else:
+            by_cmd.send_position_axis_x(1, 40)
+            time.sleep(1.5)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(1)
+            by_cmd.send_angle_claw(81)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
+            time.sleep(1)
+
+        ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed =  5)
+        if not ret:
+            logger.error("跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("抓中平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_angle_claw(55)
+            time.sleep(2)
+            by_cmd.send_angle_claw(35)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_angle_claw(55)
+            time.sleep(2)
+            by_cmd.send_angle_claw(35)
+            time.sleep(0.5)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1)
+            pass
+        # ret = explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.LPILLER, tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
+        if not ret:
+            logger.error("在放中平台的时候出现问题 跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("放中平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 120)
+            time.sleep(2)
+            by_cmd.send_position_axis_x(1, 150)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(55)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 120)
+            time.sleep(2)
+            by_cmd.send_position_axis_x(1, 40)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(55)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1)
+
+        ret = explore_calibrate_new(tlabel.SPILLER, offset = self.offset, run_direc = 1, run_speed = 5)
+        if not ret:
+            logger.error("跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("抓小平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 150)
+            by_cmd.send_angle_claw(50)
+            time.sleep(2)
+            by_cmd.send_angle_claw(27)
+            time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 10)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 0)
+            time.sleep(2)
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_x(1, 40)
+            by_cmd.send_angle_claw(50)
+            time.sleep(2)
+            by_cmd.send_angle_claw(27)
+            time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 10)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 170)
+            time.sleep(2)
+            pass
+        # ret = explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        ret = hanoi_calibrate(tlabel.MPILLER, tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
+        if not ret:
+            logger.error("在放小平台的时候出现问题 跳过物资盘点 2 exec")
+            return
+        time.sleep(0.5)
+        logger.info("放小平台")
+        if utils.direction is tlabel.RMARK:
+            by_cmd.send_position_axis_z(30, 190) # 170
+            time.sleep(1.5)
+            by_cmd.send_position_axis_x(1, 150)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(80)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
+
+            # by_cmd.send_speed_y(10)
+            # time.sleep(0.12)
+            # car_stop()
+            pass
+        else:
+            by_cmd.send_position_axis_z(30, 190)
+            time.sleep(1.5)
+            by_cmd.send_position_axis_x(1, 40)
+            time.sleep(2)
+            by_cmd.send_distance_axis_z(30, -20)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(80)
+            time.sleep(0.5)
+            by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1.5)
+        # while True:
+        #     pass
+        by_cmd.send_speed_x(12)
+        time.sleep(1.2)
+    def nexec(self):
+        pass
+    def after(self):
+        # var.switch_lane_model = False
+        if utils.direction is tlabel.RMARK:
+            var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"])
+        else:
+            var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) 
+        # time.sleep(2)
+        pass
+
+class put_hanoi3():
+    def init(self):
+        while by_cmd.send_position_axis_z(30, 130) == -1:
+            pass
+        time.sleep(3)
+        logger.info("完成任务，爪回左侧")
+        while by_cmd.send_angle_claw_arm(128) == -1:
+            pass
+        time.sleep(0.5)
+        while by_cmd.send_position_axis_x(1, 0) == -1:
+            pass
+        time.sleep(1)
+        # while by_cmd.send_angle_claw_arm(225) == -1:
+        #     pass
+        while by_cmd.send_angle_claw(85) == -1:
+            pass
+    def find(self):
+        time.sleep(1)
+        return True
+    def exec(self):
+        while by_cmd.send_position_axis_z(30, 100) == -1:
+            pass
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        by_cmd.send_position_axis_x(1, 150)
+        var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
+
+# 应急避险 第一阶段 找目标牌
+class move_area1():
+    def init(self):
+        logger.info("应急避险第一阶段初始化")
+        while (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+    def find(self):
+        ret = filter.find(tlabel.SIGN)
+        if ret:
+            return True
+        return False
+    def exec(self):
+        logger.info("找到标示牌")
+        # 停车 ocr 识别文字 调用大模型
+        car_stop()
+        time.sleep(1)
+        # calibrate_new(tlabel.SIGN, offset = 8, run = True)
+        calibrate_new(tlabel.SIGN, offset = -30, run = True, run_speed = 5)
+        time.sleep(1)
+
+        by_cmd.send_position_axis_x(1, 50)
+        time.sleep(1)
+
+        # filter_w = (148, 560)
+        # filter_h = (165, 390)
+        
+        counts = 0
+        while True:
+            ocr_socket.send_string("")
+            resp = ocr_socket.recv_pyobj()
+            var.llm_text = ''
+            counts += 1
+            if resp.get('code') == 0:
+                for item in resp.get('content'):
+                    
+                    if item['probability']['average'] < 0.80:
+                        continue
+                    # box = item['location']
+                    # center_x = box['left'] + box['width'] / 2
+                    # center_y = box['top'] + box['height'] / 2
+                    # if center_x < filter_w[0] or center_x > filter_w[1] \
+                    #         or center_y < filter_h[0] or center_y > filter_h[1]:
+                    #     continue
+                    var.llm_text += item['words']
+                break
+            if counts >= 2:
+                var.skip_llm_task_flag = True
+                return
+        logger.error(var.llm_text)
+        if len(var.llm_text) < 3:
+            var.skip_llm_task_flag = True
+            return
+
+        var.task_speed = 9 # 12
+
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        # 任务检查间隔
+        by_cmd.send_position_axis_x(1, 150)
+        # time.sleep(1)
+        by_cmd.send_angle_claw_arm(225)
+
+        pass
+
+# 应急避险 第二阶段 找停车区域
+class move_area2():
+    def init(self):
+        logger.info("应急避险第二阶段初始化")
+        self.offset = 15
+        self.delta_x = 0
+        self.delta_y = 0
+        self.delta_omage = 0
+    def find(self):
+        # time.sleep(0.001)
+        if var.skip_llm_task_flag:
+            return 5000
+        ret, box = filter.get(tlabel.SHELTER)
+        if ret:
+            error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
+            if abs(error) < 20:
+                return 5000
+        return False
+    def sub_light(self, delay_time):
+        by_cmd.send_light(1)
+        time.sleep(delay_time)
+        by_cmd.send_light(0)
+    def sub_beep(self,delay_time):
+        by_cmd.send_beep(1)
+        time.sleep(delay_time)
+        by_cmd.send_beep(0)
+    def sub_move(self, x, y):
+        # FIXME 如果同時有 xy，是否會造成 delay 不足
+        self.delta_x += x
+        self.delta_y += y
+        
+        if x != 0:
+            delay_time = int(abs(x) * 500)
+            if x > 0:
+                by_cmd.send_distance_x(15, delay_time)
+            else:
+                by_cmd.send_distance_x(-15, delay_time)
+        elif y != 0:
+            delay_time = int(abs(y) * 500)
+            if y > 0: # 向左
+                by_cmd.send_distance_y(-15, delay_time)
+            else:
+                by_cmd.send_distance_y(15, delay_time)
+        time.sleep(delay_time / 500)
+        car_stop()
+        pass
+    def sub_turn(self, angle):
+        self.delta_omage += angle
+        delay_time = int(abs(angle) * 400 / 90)
+        if angle < 0:
+            # 左转
+            by_cmd.send_angle_omega(+55, delay_time)
+        else:
+            # 右转
+            by_cmd.send_angle_omega(-55, delay_time)
+        time.sleep(delay_time / 300 * 1.5)
+    def exec(self):
+        var.task_speed = 0
+        if var.skip_llm_task_flag:
+            logger.error("ocr 识别出错 直接跳过改任务")
+            return
+        logger.info("开始寻找停车区域")
+        car_stop()
+        calibrate_new(tlabel.SHELTER, offset = 15, run = True)
+        time.sleep(0.5)
+        # 调用大模型 然后执行动作
+        try:
+            resp = llm_bot.get_command_json(var.llm_text)
+            logger.info(resp)
+        except:
+            logger.error("大模型超时，跳过任务")
+            return
+        
+        
+        try:
+            resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0])
+            if len(resp_commands) == 0:
+                return
+            # 进入停车区域
+            # by_cmd.send_speed_y(15)
+            by_cmd.send_distance_y(25, 180)
+            time.sleep(1)
+            # time.sleep(1.25)
+            car_stop()
+            logger.info(resp_commands)
+            for command in resp_commands:
+                logger.info(command)
+                if command['func'] == 'move':
+                    self.sub_move(float(command['x']), float(command['y']))
+                elif command['func'] == 'light':
+                    self.sub_light(int(command['time']))
+                elif command['func'] == 'beep':
+                    self.sub_beep(int(command['time']))
+                elif command['func'] == 'turn':
+                    self.sub_turn(int(command['angle']))
+                    pass
+                else:
+                    continue
+                time.sleep(0.5)
+        except:
+            pass
+        time.sleep(1)
+        # 回到原位
+        
+        delay_time = int(abs(self.delta_omage) * 400 / 90)
+        if int(abs(self.delta_omage)) == 360:
+            delay_time = 0
+        if self.delta_omage < 0:
+            # 左转
+            by_cmd.send_angle_omega(-55, delay_time)
+        else:
+            # 右转
+            by_cmd.send_angle_omega(55, delay_time)
+        time.sleep(delay_time / 300 * 1.5)
+        if self.delta_y > 0:
+            # 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了
+            delay_time = 180 - (self.delta_y * 500)
+        else:
+            delay_time = 180 + (abs(self.delta_y) * 500)
+        # 离开停车区域
+        by_cmd.send_distance_y(-25, delay_time)
+
+        time.sleep(delay_time * 5e-3)
+        
+        car_stop()
+
+        # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值
+        # by_cmd.send_distance_y(-15, 300)
+        pass
+    def nexec(self):
+        logger.warning("正在跳過大模型任務")
+        time.sleep(2)
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
+        by_cmd.send_position_axis_z(30, 0)
+        while by_cmd.send_angle_claw(90) == -1:
+            pass
+        time.sleep(2)
+
+# 扫黑除暴
+class kick_ass():
+    def init(self):
+        while (by_cmd.send_angle_camera(0) == -1):
+            by_cmd.send_angle_camera(0)
+        logger.info("扫黑除暴初始化")
+        self.pos_gap1 = cfg['kick_ass']['pos_gap1']
+        self.pos_gap2 = cfg['kick_ass']['pos_gap2']
+        self.target_person = cfg_args['lane_mode']['mode_index']
+
+        # by_cmd.send_angle_claw(15)
+        by_cmd.send_position_axis_x(1, 160)
+
+    def find(self):
+        ret = filter.find(tlabel.SIGN)
+        if ret:
+            return True
+        return False
+    def exec(self):
+        logger.info("找到标示牌")
+        # 停的晚无需校准 omage
+        car_stop()
+        time.sleep(1)
+        calibrate_new(tlabel.SIGN, offset = 8, run = True)
+        by_cmd.send_angle_claw(15)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(30, 60)
+        by_cmd.send_position_axis_x(1, 130)
+
+        if self.target_person == 1:
+            target_distance = self.pos_gap1
+        else:
+            target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
+        by_cmd.send_distance_x(10, target_distance)
+        
+        logger.info(f"target distance {target_distance}")
+        time.sleep(1.5 + (self.target_person - 1) * 0.7 )
+        car_stop()
+        
+        # by_cmd.send_angle_claw_arm(225)
+        # time.sleep(0.5)
+        by_cmd.send_position_axis_x(1, 20)
+        time.sleep(3)
+        by_cmd.send_position_axis_x(1, 120)
+        time.sleep(1)
+        logger.debug("結束任務，前進四")
+
+        filter.switch_camera(2)
+        for _ in range(3):
+            by_cmd.send_speed_x(15)
+        while True:
+            ret, box = filter.get(tlabel.BASE)
+            if ret:
+                error = (box[0][2] + box[0][0] - 320) / 2
+            by_cmd.send_speed_omega(-error * 0.8)
+            time.sleep(0.02)
+        # by_cmd.send_speed_x(25)
+        # time.sleep(4)
+        pass
+    def nexec(self):
+        pass
+    def after(self):
+        var.pid_turning.set(cfg["kick_ass"]["pid_kp"], cfg["kick_ass"]["pid_ki"], cfg["kick_ass"]["pid_kd"])
\ No newline at end of file
diff --git a/test/test_cali.py b/test/test_cali.py
index cc2a702..7680e58 100644
--- a/test/test_cali.py
+++ b/test/test_cali.py
@@ -34,15 +34,16 @@ def signal_handler(sig, frame):
 
 offset = 0
 signal.signal(signal.SIGTERM, signal_handler)
+cmd_py_obj.send_speed_x(15)
 while True:
-    time.sleep(0.2)
-    ret, box = filter.get(tlabel.LMARK)
-    if ret:
-        # 宽度大于 41 停车
-        print(f"width: {box[0][2] - box[0][0]} height: {box[0][3] - box[0][1]}")
+
+    time.sleep(0.02)
+    ret, box = filter.get(tlabel.BASE)
     # if ret:
-    #     error = (box[0][2] + box[0][0] - 320) / 2 + offset
-    #     print(error)
-        
-    #     cmd_py_obj.send_speed_omega(-error * 0.8)
+        # 宽度大于 41 停车
+        # print(f"width: {box[0][2] - box[0][0]} height: {box[0][3] - box[0][1]}")
+    if ret:
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        print(error)
+        cmd_py_obj.send_speed_omega(-error * 0.8)
 car_stop()
\ No newline at end of file
diff --git a/test/test_filter.py b/test/test_filter.py
index 0fca1f3..0eefd78 100644
--- a/test/test_filter.py
+++ b/test/test_filter.py
@@ -16,18 +16,18 @@ socket.connect("tcp://localhost:6667")
 logger.info("subtask yolo client init")
 
 filter = label_filter(socket)
-filter.switch_camera(1)
+filter.switch_camera(2)
 
 
-find_counts = 0
-offset = 19
-label = [tlabel.LPILLER, tlabel.MPILLER, tlabel.SPILLER]
-label = tlabel.TPLATFORM
+# find_counts = 0
+# offset = 19
+# label = [tlabel.LPILLER, tlabel.MPILLER, tlabel.SPILLER]
+# label = tlabel.TPLATFORM
 while True:
     time.sleep(0.2)
-    ret, box = filter.get(tlabel.TPLATFORM)
+    ret, box = filter.get(tlabel.BASE)
     if ret:
-        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        error = (box[0][2] + box[0][0] - 320) / 2
         logger.error(error)
 
     # label = tlabel.HOSPITAL
diff --git a/variable.py b/variable.py
index 29421cf..2c44c18 100644
--- a/variable.py
+++ b/variable.py
@@ -9,7 +9,8 @@ task_speed = 0
 pid_argv = {"kp" : 1.2, "ki" : 0, "kd" : 0} # 1.2
 
 # 转向 pid 对象
-pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=60) # FIXME 6_9 模型为 50
+# pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=70) # FIXME 6_9 模型为 60
+pid_turning = PidWrap(pid_argv["kp"], pid_argv["ki"], pid_argv["kd"], output_limits=60) # FIXME 6_9 模型为 60
 
 llm_text = ''