程序正常退出（界面关闭后（投料启动前后），实现所有线程有序退出）

Reviewed-on: huangxin/ailai#1

CU/Feeding.py

@@ -20,19 +20,23 @@ from enum import Enum, IntEnum
 from COM.COM_Robot import RobotClient, DetectType
 from Model.RobotModel import CMDInstructRequest, MoveType
 from Trace.handeye_calibration import getPosition
-from Trace.handeye_calibration import getxyz,getxyz1
+from Trace.handeye_calibration import getxyz, getxyz1
 from Util.util_math import get_distance
 from Util.util_time import CRisOrFall
-#from Vision.camera_coordinate_dete import Detection
+
+# from Vision.camera_coordinate_dete import Detection
 from Util.util_log import log
 from Model.RobotModel import Instruction
 from EMV.EMV import RelayController
 from CU.drop import DropPositionManager
+
+
 class ResetStatus(Enum):
     RNone = 0
     RStart = 1
     RRunging = 2
-    ROk =3
+    ROk = 3
+
 
 class FeedStatus(IntEnum):
     FNone = 0
@@ -42,13 +46,14 @@ class FeedStatus(IntEnum):
     FPhoto = 4
     FTake = 5
     FBroken1 = 6
-    FBroken2 =7
+    FBroken2 = 7
     FShake = 8
     FDropBag = 9
     FFinished = 10
     FReverse = 11
     FStartReverse = 12
 
+
 class LineType(Enum):
     Straight = 0
     CureMid = 2
@@ -59,15 +64,17 @@ class LineType(Enum):
 class FeedMidStatus(Enum):
     FMid_Start = 1
     FMid_Take = 2
-    FMid_Feed= 3
+    FMid_Feed = 3
+
 
 class FeedPosition:
-    def __init__(self,status:FeedStatus,position:Real_Position):
+    def __init__(self, status: FeedStatus, position: Real_Position):
         self.status = status
         self.position = position
 
+
 class FeedLine:
-    def __init__(self, id, name, feed_positions:list):
+    def __init__(self, id, name, feed_positions: list):
         self.feed_positions = copy.deepcopy(feed_positions)
         self.feeding2end_pos_index = 0
         self.origin2start_pos_index = 0
@@ -75,21 +82,41 @@ class FeedLine:
         self.name = name
         self.id = id
 
-    # --- 新增：用于存储从 ini 文件读取的多个投料点坐标 ---
-    # 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
+        # --- 新增：用于存储从 ini 文件读取的多个投料点坐标 ---
+        # 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
         self.drop_point_list = []
 
-    def get_current_feed_position(self,is_reverse):
-        pos = self.feeding_to_end[ (self.feeding2end_pos_index+1)%len(self.feeding_to_end) if is_reverse else self.feeding2end_pos_index-1]
-        return pos
-    def get_current_take_position(self,is_reverse):
-        pos = self.start_to_take[ (self.start2take_pos_index+1)%len(self.start_to_take) if is_reverse else self.start2take_pos_index-1]
-        return pos
-    def get_current_start_position(self,is_reverse):
-        pos = self.origin_to_start[(self.origin2start_pos_index+1)%len(self.origin_to_start) if is_reverse else  self.origin2start_pos_index-1]
+    def get_current_feed_position(self, is_reverse):
+        pos = self.feeding_to_end[
+            (
+                (self.feeding2end_pos_index + 1) % len(self.feeding_to_end)
+                if is_reverse
+                else self.feeding2end_pos_index - 1
+            )
+        ]
         return pos
 
-    def get_next_feed_position(self,reverse:bool=False):
+    def get_current_take_position(self, is_reverse):
+        pos = self.start_to_take[
+            (
+                (self.start2take_pos_index + 1) % len(self.start_to_take)
+                if is_reverse
+                else self.start2take_pos_index - 1
+            )
+        ]
+        return pos
+
+    def get_current_start_position(self, is_reverse):
+        pos = self.origin_to_start[
+            (
+                (self.origin2start_pos_index + 1) % len(self.origin_to_start)
+                if is_reverse
+                else self.origin2start_pos_index - 1
+            )
+        ]
+        return pos
+
+    def get_next_feed_position(self, reverse: bool = False):
         pos = self.feeding_to_end[self.feeding2end_pos_index]
         if reverse:
             self.feeding2end_pos_index -= 1
@@ -101,7 +128,7 @@ class FeedLine:
                 self.feeding2end_pos_index = 0
         return pos
 
-    def get_next_start_position(self,reverse:bool=False):
+    def get_next_start_position(self, reverse: bool = False):
         pos = self.origin_to_start[self.origin2start_pos_index]
         if reverse:
             self.origin2start_pos_index -= 1
@@ -114,7 +141,7 @@ class FeedLine:
 
         return pos
 
-    def get_next_take_position(self,reverse:bool=False):
+    def get_next_take_position(self, reverse: bool = False):
         pos = self.start_to_take[self.start2take_pos_index]
         if reverse:
             self.start2take_pos_index -= 1
@@ -125,6 +152,7 @@ class FeedLine:
             if self.start2take_pos_index >= len(self.start_to_take):
                 self.start2take_pos_index = 0
         return pos
+
     def get_take_position(self):
         for i in range(len(self.feeding_to_end)):
             if self.feeding_to_end[i].status == FeedStatus.FTake.value:
@@ -140,20 +168,21 @@ class FeedLine:
             if self.feeding_to_end[i].status == FeedStatus.FTake.value:
                 # 计算 XYZ 坐标
                 # xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c)
-                xyz = getxyz1(position.X, position.Y, position.Z, position.a, position.b, position.c)
+                xyz = getxyz1(
+                    position.X,
+                    position.Y,
+                    position.Z,
+                    position.a,
+                    position.b,
+                    position.c,
+                )
                 # 创建 before 和 after 位置
-                befor_take_position = Real_Position().init_position(xyz[0],
-                                                                    xyz[1],
-                                                                    xyz[2],
-                                                                    position.U,
-                                                                    position.V,
-                                                                    position.W)
-                after_take_position = Real_Position().init_position(xyz[0],
-                                                                    xyz[1],
-                                                                    xyz[2],
-                                                                    position.U,
-                                                                    position.V,
-                                                                    position.W)
+                befor_take_position = Real_Position().init_position(
+                    xyz[0], xyz[1], xyz[2], position.U, position.V, position.W
+                )
+                after_take_position = Real_Position().init_position(
+                    xyz[0], xyz[1], xyz[2], position.U, position.V, position.W
+                )
                 # 安全检查索引
                 if i > 0:
                     self.feeding_to_end[i - 1].set_position(befor_take_position)
@@ -176,7 +205,8 @@ class FeedLine:
                 # 直接设置当前点的位置
                 self.feeding_to_end[i].set_position(position)
                 print(
-                    f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})")
+                    f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})"
+                )
                 break  # 假设只有一个丢包点
 
     def get_position_list(self):
@@ -189,11 +219,11 @@ class FeedLine:
             if self.feed_positions[i].status == FeedStatus.FPhoto.value:
                 index_take = i
 
-        self.origin_to_start = self.feed_positions[: index_start+1]
-        self.start_to_take = self.feed_positions[index_start:index_take+1]
+        self.origin_to_start = self.feed_positions[: index_start + 1]
+        self.start_to_take = self.feed_positions[index_start : index_take + 1]
         self.feeding_to_end = self.feed_positions[index_take:]
 
-        for i in range(len(self.feeding_to_end)):  #插入动态中间点
+        for i in range(len(self.feeding_to_end)):  # 插入动态中间点
             if self.feeding_to_end[i].status == FeedStatus.FTake.value:
                 befor_position_model = PositionModel()
                 befor_position_model.init_position(None)
@@ -201,19 +231,26 @@ class FeedLine:
                 after_position_model.init_position(None)
 
                 self.feeding_to_end.insert(i, befor_position_model)
-                self.feeding_to_end.insert(i+2, after_position_model)
+                self.feeding_to_end.insert(i + 2, after_position_model)
                 break
 
+
 class FeedingConfig:
     def __init__(self, num: int, feedLine: FeedLine, photo_locs):
         self.num = num
         self.feedLine = feedLine
         self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
 
-
     def deal_photo_locs(self, photo_loc):
         position_photo = Real_Position()
-        position_photo.init_position(photo_loc[0], photo_loc[1], photo_loc[2], photo_loc[3], photo_loc[4], photo_loc[5])
+        position_photo.init_position(
+            photo_loc[0],
+            photo_loc[1],
+            photo_loc[2],
+            photo_loc[3],
+            photo_loc[4],
+            photo_loc[5],
+        )
         return position_photo
 
     def get_line_info(self):
@@ -224,7 +261,8 @@ class Feeding(QObject):
     need_origin_signal = Signal(str)
     take_no_photo_sigal = Signal()
     update_detect_image = Signal(np.ndarray)
-    log_signal = Signal(int,str)
+    log_signal = Signal(int, str)
+
     def __init__(self, robotClient: RobotClient):
         super().__init__()
         self.feedConfig = None
@@ -246,7 +284,7 @@ class Feeding(QObject):
         self.reset_status = ResetStatus.RNone
         self.reversed_positions = []
         self.current_position = None
-        self.index=1
+        self.index = 1
         self.pos_index = -1
         self.pos_near_index = -1
         self.catch = Catch(self.robotClient)
@@ -257,7 +295,7 @@ class Feeding(QObject):
         self.onekey = False
         self.debug_run_count = 0  # 初始化计数器
         self.mid_take_count = 0
-        #传感器判断抓包参数
+        # 传感器判断抓包参数
         self.sensor2_ready = False  # 传感器2是否检测到料包
         self.motor_stopped_by_sensor2 = False  # 是否由传感器2触发停止电机
         self.sensor_thread = None
@@ -272,9 +310,11 @@ class Feeding(QObject):
         pass
 
     def close_feed(self):
+        self.need_origin_signal.disconnect()
+        self.relay_controller._running = False
         self.is_detected = False
         self.detect_thread.join()
-        self.detect.detection.release()
+        # self.detect.detection.release()
 
     def init_detection_image(self):
         detection_image = cv2.imread(Constant.feed_sign_path)
@@ -290,12 +330,14 @@ class Feeding(QObject):
         # 获取事件坐标
         real_position = Real_Position()
         self.detect.position_index = 0
-        real_position.init_position(self.robotClient.status_model.world_0,
-                                    self.robotClient.status_model.world_1,
-                                    self.robotClient.status_model.world_2,
-                                    self.robotClient.status_model.world_3,
-                                    self.robotClient.status_model.world_4,
-                                    self.robotClient.status_model.world_5)
+        real_position.init_position(
+            self.robotClient.status_model.world_0,
+            self.robotClient.status_model.world_1,
+            self.robotClient.status_model.world_2,
+            self.robotClient.status_model.world_3,
+            self.robotClient.status_model.world_4,
+            self.robotClient.status_model.world_5,
+        )
         # real_position.init_position(0,
         #                             0,
         #                             0,
@@ -312,7 +354,12 @@ class Feeding(QObject):
         if self.feedConfig == None:
             self.feedStatus = FeedStatus.FNone
 
-        if self.feedConfig !=None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(real_position,is_action=True):
+        if (
+            self.feedConfig != None
+            and self.feedConfig.num == 0
+            and self.is_reverse
+            and self.robotClient.origin_position.compare(real_position, is_action=True)
+        ):
             self.feedStatus = FeedStatus.FNone
             self.is_reverse = False
             self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
@@ -338,15 +385,24 @@ class Feeding(QObject):
             else:
                 self.feed_Mid_Status = FeedMidStatus.FMid_Take
 
-            if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position):
+            if (
+                self.feedConfig.feedLine.start_to_take[0]
+                .get_position()
+                .compare(real_position)
+            ):
                 self.next_position(self.is_reverse)
 
         elif self.feedStatus == FeedStatus.FStart:
             self.log_signal.emit(logging.INFO, Constant.str_feed_start)
-            self.relay_controller.open(conveyor2=True)#开电机
-            #self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢
-            #self.sensor2_thread.start()
-            if not self.robotClient.origin_position.compare(real_position,is_action=True) and not self.is_reverse:
+            self.relay_controller.open(conveyor2=True)  # 开电机
+            # self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢
+            # self.sensor2_thread.start()
+            if (
+                not self.robotClient.origin_position.compare(
+                    real_position, is_action=True
+                )
+                and not self.is_reverse
+            ):
                 # QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
                 self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error)
                 self.need_origin_signal.emit(Constant.str_feed_start_error)
@@ -373,19 +429,23 @@ class Feeding(QObject):
                 if Constant.Debug1:
                     self.log_signal.emit(
                         logging.INFO,
-                        f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次"
+                        f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次",
                     )
-                #这里他写的中间点过完马上进行抓料，和整个流程不符合我就注释掉了，因为实际测试的时候还没到FTake的点他就进行了继电器关闭夹爪的抓取
-                #if self.feedStatus == FeedStatus.FTake:
-                    #self.catch.catch_status = CatchStatus.CTake
+                # 这里他写的中间点过完马上进行抓料，和整个流程不符合我就注释掉了，因为实际测试的时候还没到FTake的点他就进行了继电器关闭夹爪的抓取
+                # if self.feedStatus == FeedStatus.FTake:
+                # self.catch.catch_status = CatchStatus.CTake
 
         elif self.feedStatus == FeedStatus.FPhoto:
             if self.feedConfig.num == 0:
                 self.log_signal.emit(logging.INFO, Constant.str_feed_finish)
                 self.is_reverse = True
                 self.feed_Mid_Status = FeedMidStatus.FMid_Take
-                self.feedConfig.feedLine.start2take_pos_index = len(self.feedConfig.feedLine.start_to_take) - 2
-                self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2
+                self.feedConfig.feedLine.start2take_pos_index = (
+                    len(self.feedConfig.feedLine.start_to_take) - 2
+                )
+                self.feedConfig.feedLine.origin2start_pos_index = (
+                    len(self.feedConfig.feedLine.origin_to_start) - 2
+                )
                 self.next_position(self.is_reverse)
                 self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
                 self.init_detection_image()
@@ -395,15 +455,19 @@ class Feeding(QObject):
                 self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto)
                 self.feed_Mid_Status = FeedMidStatus.FMid_Feed
                 self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_success)
-                self.feedConfig.feedLine.set_take_position(self.detect.detect_position, 0)
-                #self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置
-                '''real_position'''
+                self.feedConfig.feedLine.set_take_position(
+                    self.detect.detect_position, 0
+                )
+                # self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置
+                """real_position"""
                 # 一直等待传感器2信号，永不超时
                 while True:
-                    sensors = self.relay_controller.get_all_device_status('sensors')
+                    sensors = self.relay_controller.get_all_device_status("sensors")
                     sensor2_value = sensors.get(self.relay_controller.SENSOR2, False)
                     if sensor2_value:
-                        self.log_signal.emit(logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取")
+                        self.log_signal.emit(
+                            logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取"
+                        )
                         break  # ✅ 条件满足，跳出循环，继续执行下面的代码
                     else:
                         self.log_signal.emit(logging.INFO, "⏳ 等待传感器2料包信号...")
@@ -411,7 +475,7 @@ class Feeding(QObject):
                 self.next_position()
                 self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
 
-                #self.feedStatus = FeedStatus.FTake
+                # self.feedStatus = FeedStatus.FTake
 
         elif self.feedStatus == FeedStatus.FTake:
             self.log_signal.emit(logging.INFO, Constant.str_feed_take)
@@ -424,31 +488,33 @@ class Feeding(QObject):
                 return
             self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位")
             # 执行抓取动作
-            #self.catch.catch_status = CatchStatus.CTake
-            #if self.catch.catch_status == CatchStatus.CNone:
-                #self.catch.catch_status = CatchStatus.CTake
-            #if self.catch.catch_status == CatchStatus.CTake:
-                #self.log_signal.emit(logging.INFO, "正在执行抓料动作...")
-                #self.catch.catch_status = CatchStatus.COk
-            #if self.catch.catch_status == CatchStatus.COk:
-                #self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
-                #self.catch.catch_status = CatchStatus.CNone
-                # 移动到下一个抓取点
-                # 更新丢包点: 如果需要根据放置情况调整下次抓取
+            # self.catch.catch_status = CatchStatus.CTake
+            # if self.catch.catch_status == CatchStatus.CNone:
+            # self.catch.catch_status = CatchStatus.CTake
+            # if self.catch.catch_status == CatchStatus.CTake:
+            # self.log_signal.emit(logging.INFO, "正在执行抓料动作...")
+            # self.catch.catch_status = CatchStatus.COk
+            # if self.catch.catch_status == CatchStatus.COk:
+            # self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
+            # self.catch.catch_status = CatchStatus.CNone
+            # 移动到下一个抓取点
+            # 更新丢包点: 如果需要根据放置情况调整下次抓取
             next_drop_pos = self.drop_manager.get_next_drop_position(lineid=1)
-            print("next_drop_pos--------",next_drop_pos)
+            print("next_drop_pos--------", next_drop_pos)
             if next_drop_pos:
                 self.feedConfig.feedLine.set_drop_position(next_drop_pos)
-                self.log_signal.emit(logging.INFO, f"已设置放置点: X={next_drop_pos.X:.2f}")
+                self.log_signal.emit(
+                    logging.INFO, f"已设置放置点: X={next_drop_pos.X:.2f}"
+                )
             else:
                 self.log_signal.emit(logging.ERROR, "获取放置点失败")
                 return
-            #self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
+            # self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
             self.relay_controller.open(clamp=True)
             self.next_position()
-            #return
-            #else:
-                #self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
+            # return
+            # else:
+            # self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
 
         elif self.feedStatus == FeedStatus.FBroken1:
 
@@ -456,7 +522,6 @@ class Feeding(QObject):
                 self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
                 self.next_position()
 
-
         elif self.feedStatus == FeedStatus.FBroken2:
 
             if self.get_current_position().get_position().compare(real_position):
@@ -464,7 +529,11 @@ class Feeding(QObject):
                 self.next_position()
 
         elif self.feedStatus == FeedStatus.FShake:
-            if self.get_current_position().get_position().compare(real_position,is_action=True):
+            if (
+                self.get_current_position()
+                .get_position()
+                .compare(real_position, is_action=True)
+            ):
                 # TODO 震动方案
                 self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
                 if self.catch.catch_status == CatchStatus.CNone:
@@ -477,21 +546,25 @@ class Feeding(QObject):
                     # else:
                     self.catch.shake_continue.SetReset()
                     self.next_position()
-                    if self.feedStatus!=FeedStatus.FShake:
+                    if self.feedStatus != FeedStatus.FShake:
                         self.catch.catch_status = CatchStatus.CNone
                     return
 
         elif self.feedStatus == FeedStatus.FDropBag:
-        #"""*** 处理投料点 (FDropBag) 的核心逻辑 ***"""
-        # 1. 确认机械臂是否已精确到达当前目标投料点
-        #    get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed)
-        #    调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取
-        #    由 feeding2end_pos_index 指向的点。
-            if self.get_current_position().get_position().compare(real_position, is_action=True):
+            # """*** 处理投料点 (FDropBag) 的核心逻辑 ***"""
+            # 1. 确认机械臂是否已精确到达当前目标投料点
+            #    get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed)
+            #    调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取
+            #    由 feeding2end_pos_index 指向的点。
+            if (
+                self.get_current_position()
+                .get_position()
+                .compare(real_position, is_action=True)
+            ):
                 # 2. 记录日志：已到达投料点
-                #if not self.sensor2_ready:
-                    #self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2")
-                    #self.relay_controller.open(conveyor2=True)
+                # if not self.sensor2_ready:
+                # self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2")
+                # self.relay_controller.open(conveyor2=True)
 
                 self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
                 # 3. 与 Catch 模块进行状态交互来驱动投料动作
@@ -519,7 +592,10 @@ class Feeding(QObject):
                     # self.feedConfig.feedLine.set_take_position(...)
                     # 4. 更新业务逻辑：减少剩余袋数
                     self.feedConfig.num = self.feedConfig.num - 1
-                    self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
+                    self.log_signal.emit(
+                        logging.INFO,
+                        f"{Constant.str_feed_feed_num}{self.feedConfig.num}",
+                    )
                     # 5. *** 关键步骤 ***: 移动到路径中的下一个点
                     #    next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
                     #    调用 next_Feed()。
@@ -527,17 +603,20 @@ class Feeding(QObject):
 
     def run_reset(self):
         real_position = Real_Position()
-        real_position.init_position(self.robotClient.status_model.world_0,
-                                    self.robotClient.status_model.world_1,
-                                    self.robotClient.status_model.world_2,
-                                    self.robotClient.status_model.world_3,
-                                    self.robotClient.status_model.world_4,
-                                    self.robotClient.status_model.world_5)
+        real_position.init_position(
+            self.robotClient.status_model.world_0,
+            self.robotClient.status_model.world_1,
+            self.robotClient.status_model.world_2,
+            self.robotClient.status_model.world_3,
+            self.robotClient.status_model.world_4,
+            self.robotClient.status_model.world_5,
+        )
         if self.reset_status == ResetStatus.RNone:
             return
 
         if self.reset_status == ResetStatus.RStart:
-            if self.feedConfig == None: return
+            if self.feedConfig == None:
+                return
             # for index in range(len(self.feedConfig.feedLine.positions)):
             #     if self.feedConfig.feedLine.positions[index].status == 2:
             #         start_index = index
@@ -545,23 +624,36 @@ class Feeding(QObject):
             self.pos_near_index = -1
             self.reversed_positions = []
             for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
-                if pos_model.get_position().compare(real_position,is_action=True):
+                if pos_model.get_position().compare(real_position, is_action=True):
                     self.pos_index = index
                     break
 
             if self.pos_index == -1:
-                self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
+                self.log_signal.emit(
+                    logging.ERROR, Constant.str_feed_return_original_position_fail
+                )
                 min_distance = 99999999
-                for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
-                    if get_distance(pos_model.get_position(), real_position) < min_distance:
-                        min_distance = get_distance(pos_model.get_position(), real_position)
+                for index, pos_model in enumerate(
+                    self.feedConfig.feedLine.feed_positions
+                ):
+                    if (
+                        get_distance(pos_model.get_position(), real_position)
+                        < min_distance
+                    ):
+                        min_distance = get_distance(
+                            pos_model.get_position(), real_position
+                        )
                         self.pos_near_index = index
                 if self.pos_near_index != -1:
-                    self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1]
+                    self.reversed_positions = self.feedConfig.feedLine.feed_positions[
+                        : self.pos_near_index + 1
+                    ]
                 else:
                     return False
             else:
-                self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index+1]
+                self.reversed_positions = self.feedConfig.feedLine.feed_positions[
+                    : self.pos_index + 1
+                ]
             self.reversed_positions = list(reversed(self.reversed_positions))
             self.reverse_index = 0
             self.send_emergency_sound()
@@ -570,21 +662,30 @@ class Feeding(QObject):
             self.reset_status = ResetStatus.RRunging
 
         if self.reset_status == ResetStatus.RRunging:
-            if not real_position.compare(self.current_position.get_position(),is_action=True):
+            if not real_position.compare(
+                self.current_position.get_position(), is_action=True
+            ):
                 return
             pos_model = self.reversed_positions[self.reverse_index]
             if pos_model.status == FeedStatus.FTake.value:  # 跳过取袋节点
                 pos_model = self.reversed_positions[self.reverse_index + 1]
-                self.reverse_index = self.reverse_index+1
+                self.reverse_index = self.reverse_index + 1
 
             if pos_model.lineType == LineType.CureMid.value:
                 pos_model1 = self.reversed_positions[self.reverse_index + 1]
-                self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
-                                      real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
+                self.sendTargPosition(
+                    real_position=pos_model.get_position(),
+                    move_type=MoveType.Cure,
+                    real_position1=pos_model1.get_position(),
+                    speed=self.robotClient.reset_speed,
+                )
                 self.current_position = pos_model1
                 self.reverse_index = self.reverse_index + 2
             else:
-                self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
+                self.sendTargPosition(
+                    real_position=pos_model.get_position(),
+                    speed=self.robotClient.reset_speed,
+                )
                 self.current_position = pos_model
                 self.reverse_index = self.reverse_index + 1
 
@@ -598,13 +699,16 @@ class Feeding(QObject):
 
         self.run_reverse = True
         real_position = Real_Position()
-        real_position.init_position(self.robotClient.status_model.world_0,
-                                    self.robotClient.status_model.world_1,
-                                    self.robotClient.status_model.world_2,
-                                    self.robotClient.status_model.world_3,
-                                    self.robotClient.status_model.world_4,
-                                    self.robotClient.status_model.world_5)
-        if self.feedConfig == None: return
+        real_position.init_position(
+            self.robotClient.status_model.world_0,
+            self.robotClient.status_model.world_1,
+            self.robotClient.status_model.world_2,
+            self.robotClient.status_model.world_3,
+            self.robotClient.status_model.world_4,
+            self.robotClient.status_model.world_5,
+        )
+        if self.feedConfig == None:
+            return
         start_index = -1
         # for index in range(len(self.feedConfig.feedLine.positions)):
         #     if self.feedConfig.feedLine.positions[index].status == 2:
@@ -618,14 +722,18 @@ class Feeding(QObject):
                 break
 
         if pos_index == -1:
-            self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
+            self.log_signal.emit(
+                logging.ERROR, Constant.str_feed_return_original_position_fail
+            )
             min_distance = 99999999
             for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
-                if get_distance(pos_model.get_position(), real_position)<min_distance:
+                if get_distance(pos_model.get_position(), real_position) < min_distance:
                     min_distance = get_distance(pos_model.get_position(), real_position)
                     pos_near_index = index
             if pos_near_index != -1:
-                reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_near_index+1]
+                reversed_positions = self.feedConfig.feedLine.feed_positions[
+                    : pos_near_index + 1
+                ]
             else:
                 return False
         else:
@@ -635,10 +743,12 @@ class Feeding(QObject):
         self.send_emergency_sound()
         current_position = PositionModel()
         current_position.init_position(real_position)
-        while self.run_reverse and self.reverse_index!=len(reversed_positions):
-            if self.reverse_index!=0 and not real_position.compare(current_position.get_position()):
+        while self.run_reverse and self.reverse_index != len(reversed_positions):
+            if self.reverse_index != 0 and not real_position.compare(
+                current_position.get_position()
+            ):
                 continue
-            #todo 缺少比对
+            # todo 缺少比对
             pos_model = reversed_positions[self.reverse_index]
             if pos_model.status == FeedStatus.FTake.value:  # 跳过取袋节点
 
@@ -647,16 +757,24 @@ class Feeding(QObject):
 
             if pos_model.lineType == LineType.CureMid.value:
                 pos_model1 = reversed_positions[self.reverse_index + 1]
-                self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
-                                      real_position1=pos_model1.get_position(),speed= self.robotClient.reset_speed)
+                self.sendTargPosition(
+                    real_position=pos_model.get_position(),
+                    move_type=MoveType.Cure,
+                    real_position1=pos_model1.get_position(),
+                    speed=self.robotClient.reset_speed,
+                )
                 current_position = pos_model1
-                self.reverse_index = self.reverse_index+2
+                self.reverse_index = self.reverse_index + 2
             else:
-                self.sendTargPosition(real_position=pos_model.get_position(),speed=self.robotClient.reset_speed)
+                self.sendTargPosition(
+                    real_position=pos_model.get_position(),
+                    speed=self.robotClient.reset_speed,
+                )
                 current_position = pos_model
                 self.reverse_index = self.reverse_index + 1
         self.send_emergency_stop()
         return True
+
     def send_emergency_sound(self):
         self.sendIOControl(Constant.IO_EmergencyPoint, 1)
 
@@ -670,17 +788,30 @@ class Feeding(QObject):
         io_instruction.IO = True
         io_instruction.io_status = IO_Status
         io_instruction.point = IO_bit  # {"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
-        IO_command.dsID = 'HCRemoteCommand'
+        IO_command.dsID = "HCRemoteCommand"
         IO_command.instructions.append(io_instruction)
         self.robotClient.add_sendQuene(IO_command.toString())
-        self.log_signal.emit(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
+        self.log_signal.emit(
+            logging.INFO, f"{Constant.str_feed_io_control}{IO_bit}，{IO_Status}"
+        )
         pass
 
-    def sendTargPosition1(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
+    def sendTargPosition1(
+        self,
+        real_position,
+        move_type: MoveType = MoveType.WORLD,
+        speed=5,
+        real_position1=None,
+    ):
         pass
 
-
-    def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
+    def sendTargPosition(
+        self,
+        real_position,
+        move_type: MoveType = MoveType.WORLD,
+        speed=5,
+        real_position1=None,
+    ):
         position_instruction = Instruction()
         position_instruction.speed = speed
         position_instruction.m0 = real_position.X
@@ -703,24 +834,26 @@ class Feeding(QObject):
         instruction_command.instructions.append(position_instruction)
         request_command = instruction_command.toString()
 
-        log_str = f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：' \
-                  f'X:{position_instruction.m0}-' \
-                  f'Y:{position_instruction.m1}-' \
-                  f'Z:{position_instruction.m2}-' \
-                  f'U:{position_instruction.m3}-' \
-                  f'V:{position_instruction.m4}-' \
-                  f'W:{position_instruction.m5}'
-
+        log_str = (
+            f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：'
+            f"X:{position_instruction.m0}-"
+            f"Y:{position_instruction.m1}-"
+            f"Z:{position_instruction.m2}-"
+            f"U:{position_instruction.m3}-"
+            f"V:{position_instruction.m4}-"
+            f"W:{position_instruction.m5}"
+        )
 
         try:
-            print('fuck1',log_str)
+            print("fuck1", log_str)
             self.log_signal.emit(logging.INFO, log_str)
-            print('fuck2',log_str)
+            print("fuck2", log_str)
         except:
             return
         print(request_command)
         self.robotClient.add_sendQuene(request_command)
         pass
+
     # def get_take_position(self):
     #     if Constant.Debug:
     #         return self.robotClient.status_model.getRealPosition()
@@ -733,59 +866,117 @@ class Feeding(QObject):
     #     position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
     #     position.Z = position.Z+200
     #     return position
-    def next_start(self,reverse=False):
+    def next_start(self, reverse=False):
         start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
-        self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
+        self.feedStatus = (
+            FeedStatus(start_pos.status)
+            if self.feedStatus != FeedStatus.FNone
+            else FeedStatus.FNone
+        )
         if start_pos.lineType == LineType.CureMid.value:
             start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
-            self.feedStatus = FeedStatus(start_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-            self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed, move_type=MoveType.Cure, real_position1=start_pos1.get_position())
+            self.feedStatus = (
+                FeedStatus(start_pos1.status)
+                if self.feedStatus != FeedStatus.FNone
+                else FeedStatus.FNone
+            )
+            self.sendTargPosition(
+                real_position=start_pos.get_position(),
+                speed=self.robotClient.reset_speed,
+                move_type=MoveType.Cure,
+                real_position1=start_pos1.get_position(),
+            )
         elif start_pos.lineType == LineType.WORLD.value:
-            if self.robotClient.status_model.getAnglePosition().is_error_angel_move(start_pos.get_position(),self.robotClient.max_angle_interval):
+            if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
+                start_pos.get_position(), self.robotClient.max_angle_interval
+            ):
                 self.feedStatus = None
                 self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
             else:
-                self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed,move_type=MoveType.AXIS)
+                self.sendTargPosition(
+                    real_position=start_pos.get_position(),
+                    speed=self.robotClient.reset_speed,
+                    move_type=MoveType.AXIS,
+                )
         else:
-            self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed)
+            self.sendTargPosition(
+                real_position=start_pos.get_position(),
+                speed=self.robotClient.reset_speed,
+            )
         pass
 
-    def next_take(self,reverse=False):
+    def next_take(self, reverse=False):
 
         take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
-        self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
+        self.feedStatus = (
+            FeedStatus(take_pos.status)
+            if self.feedStatus != FeedStatus.FNone
+            else FeedStatus.FNone
+        )
         if take_pos.lineType == LineType.CureMid.value:
             take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
-            self.feedStatus = FeedStatus(take_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-            self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position(),speed=self.robotClient.feed_speed)
+            self.feedStatus = (
+                FeedStatus(take_pos1.status)
+                if self.feedStatus != FeedStatus.FNone
+                else FeedStatus.FNone
+            )
+            self.sendTargPosition(
+                real_position=take_pos.get_position(),
+                move_type=MoveType.Cure,
+                real_position1=take_pos1.get_position(),
+                speed=self.robotClient.feed_speed,
+            )
         elif take_pos.lineType == LineType.WORLD.value:
-            if self.robotClient.status_model.getAnglePosition().is_error_angel_move(take_pos.get_position(),
-                                                                                    self.robotClient.max_angle_interval):
+            if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
+                take_pos.get_position(), self.robotClient.max_angle_interval
+            ):
                 self.feedStatus = None
                 self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
             else:
-                self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed,
-                                      move_type=MoveType.AXIS)
+                self.sendTargPosition(
+                    real_position=take_pos.get_position(),
+                    speed=self.robotClient.feed_speed,
+                    move_type=MoveType.AXIS,
+                )
 
         else:
-            self.sendTargPosition(real_position=take_pos.get_position(),speed=self.robotClient.feed_speed)
+            self.sendTargPosition(
+                real_position=take_pos.get_position(), speed=self.robotClient.feed_speed
+            )
         pass
 
-    def next_Feed(self,reverse=False):
+    def next_Feed(self, reverse=False):
         feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
-        self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
+        self.feedStatus = (
+            FeedStatus(feed_pos.status)
+            if self.feedStatus != FeedStatus.FNone
+            else FeedStatus.FNone
+        )
         if feed_pos.lineType == LineType.CureMid.value:
             feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
-            self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
-            self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position(),speed=self.robotClient.feed_speed)
+            self.feedStatus = (
+                FeedStatus(feed_pos1.status)
+                if self.feedStatus != FeedStatus.FNone
+                else FeedStatus.FNone
+            )
+            self.sendTargPosition(
+                real_position=feed_pos.get_position(),
+                move_type=MoveType.Cure,
+                real_position1=feed_pos1.get_position(),
+                speed=self.robotClient.feed_speed,
+            )
         elif feed_pos.lineType == LineType.WORLD.value:
-            if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(),
-                                                                                    self.robotClient.max_angle_interval):
+            if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
+                feed_pos.get_position(), self.robotClient.max_angle_interval
+            ):
                 self.feedStatus = None
                 self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
             else:
-                self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed,
-                                      move_type=MoveType.AXIS)
+                self.sendTargPosition(
+                    real_position=feed_pos.get_position(),
+                    speed=self.robotClient.feed_speed,
+                    move_type=MoveType.AXIS,
+                )
 
         else:
             # if not reverse and self.feedStatus == FeedStatus.FShake:
@@ -797,29 +988,31 @@ class Feeding(QObject):
             #         self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
             #         return
 
+            self.sendTargPosition(
+                real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed
+            )
 
-            self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
-
-    def get_current_position(self,is_reverse=False):
+    def get_current_position(self, is_reverse=False):
         if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
             return self.feedConfig.feedLine.get_current_start_position(self.is_reverse)
         elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
             return self.feedConfig.feedLine.get_current_take_position(self.is_reverse)
         elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
             return self.feedConfig.feedLine.get_current_feed_position(self.is_reverse)
-    def next_position(self,reverse=False):
+
+    def next_position(self, reverse=False):
         if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
             self.next_start(reverse)
             if Constant.Debug1:
-                print('next_start')
+                print("next_start")
         elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
             self.next_take(reverse)
             if Constant.Debug1:
-                print('next_take')
+                print("next_take")
         elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
             self.next_Feed(reverse)
             if Constant.Debug1:
-                print('next_feed')
+                print("next_feed")
 
     def safe_check_columns(self):
         return True
@@ -828,4 +1021,3 @@ class Feeding(QObject):
     def safe_check_person(self):
         return True
         pass
-

EMV/EMV.py

@@ -15,7 +15,7 @@ class RelayController:
     update_detect_image = Signal(np.ndarray)
     log_signal = Signal(int, str)
 
-    def __init__(self, host='192.168.0.18', port=50000):
+    def __init__(self, host="192.168.0.18", port=50000):
         # ===================== 全局线程延时参数 =====================
         self.sensor1_loop_delay = 0.2  # SENSOR1 线程轮询间隔（秒）
         self.sensor1_error_delay = 1.0  # SENSOR1 出错时延时（秒）
@@ -25,7 +25,9 @@ class RelayController:
         self.sensor2_error_delay = 0.5  # SENSOR2 出错时延时（秒）
         self.sensor2_post_action_delay = 0.2  # SENSOR2 每次循环后延时（秒）
         # ===================== 全局动作延时参数 =====================
-        self.delay_conveyor = 0.5  # 传送带开/关动作延时（一半时间，我在控制程序和线程都加了一样的延时）
+        self.delay_conveyor = (
+            0.5  # 传送带开/关动作延时（一半时间，我在控制程序和线程都加了一样的延时）
+        )
         self.delay_pusher = 0.05  # 推板开/关动作延时
         self.delay_clamp = 0.5  # 夹爪动作延时
         self.delay_after_pusher = 3.0  # 推板推出后到重启传动带时间
@@ -39,25 +41,40 @@ class RelayController:
         self.host = host
         self.port = port
 
-        self.CONVEYOR1 = 'conveyor1'
-        self.PUSHER = 'pusher'
-        self.CONVEYOR2 = 'conveyor2'
-        self.CLAMP = 'clamp'
-        self.PUSHER1 = 'pusher1'
-        self.SENSOR1 = 'sensor1'
-        self.SENSOR2 = 'sensor2'
+        self.CONVEYOR1 = "conveyor1"
+        self.PUSHER = "pusher"
+        self.CONVEYOR2 = "conveyor2"
+        self.CLAMP = "clamp"
+        self.PUSHER1 = "pusher1"
+        self.SENSOR1 = "sensor1"
+        self.SENSOR2 = "sensor2"
 
         self.valve_commands = {
-            self.CONVEYOR1: {'open': '00000000000601050000FF00', 'close': '000000000006010500000000'},
-            self.PUSHER: {'open': '00000000000601050001FF00', 'close': '000000000006010500010000'},
-            self.CONVEYOR2: {'open': '000100000006020620000012', 'close': '000100000006020620000001'},
-            self.CLAMP: {'open': '00000000000601050003FF00', 'close': '000000000006010500030000'},
-            self.PUSHER1: {'open': '00000000000601050004FF00', 'close': '000000000006010500040000'}
+            self.CONVEYOR1: {
+                "open": "00000000000601050000FF00",
+                "close": "000000000006010500000000",
+            },
+            self.PUSHER: {
+                "open": "00000000000601050001FF00",
+                "close": "000000000006010500010000",
+            },
+            self.CONVEYOR2: {
+                "open": "000100000006020620000012",
+                "close": "000100000006020620000001",
+            },
+            self.CLAMP: {
+                "open": "00000000000601050003FF00",
+                "close": "000000000006010500030000",
+            },
+            self.PUSHER1: {
+                "open": "00000000000601050004FF00",
+                "close": "000000000006010500040000",
+            },
         }
 
         self.read_status_command = {
-            'devices': '000000000006010100000008',
-            'sensors': '000000000006010200000008'
+            "devices": "000000000006010100000008",
+            "sensors": "000000000006010200000008",
         }
 
         self.device_bit_map = {
@@ -82,8 +99,8 @@ class RelayController:
         }
 
         self.sensor_name_map = {
-            self.SENSOR1: '位置传感器1',
-            self.SENSOR2: '位置传感器2',
+            self.SENSOR1: "位置传感器1",
+            self.SENSOR2: "位置传感器2",
         }
 
         # ===================== 状态控制变量 =====================
@@ -91,8 +108,8 @@ class RelayController:
         self._sensor1_thread = None
         self._sensor2_thread = None
 
-        self.required_codes = {'0101', '0103'}  # 有效状态码（传感器1）
-        self.required_codes_1 = {'0102', '0103'}  # 有效状态码（传感器2）
+        self.required_codes = {"0101", "0103"}  # 有效状态码（传感器1）
+        self.required_codes_1 = {"0102", "0103"}  # 有效状态码（传感器2）
 
         self.sensor1_triggered = False
         self.sensor1_last_time = 0
@@ -101,25 +118,33 @@ class RelayController:
         self.motor_stopped_by_sensor2 = False
 
     # ===================== 基础通信方法 =====================
-    def send_command(self, command_hex, retry_count=2, source='unknown'):
+    def send_command(self, command_hex, retry_count=2, source="unknown"):
+        if not self._running:
+            return
+
         byte_data = binascii.unhexlify(command_hex)
         for attempt in range(retry_count):
+            if not self._running:
+                return
+
             try:
                 with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
                     sock.settimeout(10)
                     sock.connect((self.host, self.port))
                     sock.send(byte_data)
                     response = sock.recv(1024)
-                    hex_response = binascii.hexlify(response).decode('utf-8')
-                    #if source == 'sensor':
-                        #print(f"[传感器响应] {hex_response}")
-                    #elif source == 'device':
-                        #print(f"[设备控制响应] {hex_response}")
-                    #else:
-                        #print(f"[通信响应] {hex_response}")
+                    hex_response = binascii.hexlify(response).decode("utf-8")
+                    # if source == 'sensor':
+                    # print(f"[传感器响应] {hex_response}")
+                    # elif source == 'device':
+                    # print(f"[设备控制响应] {hex_response}")
+                    # else:
+                    # print(f"[通信响应] {hex_response}")
                     return response
             except Exception as e:
-                print(f"通信错误 ({source}): {e}, 尝试重连... ({attempt + 1}/{retry_count})")
+                print(
+                    f"通信错误 ({source}): {e}, 尝试重连... ({attempt + 1}/{retry_count})"
+                )
                 time.sleep(5)
         self.trigger_alarm()
         return None
@@ -128,30 +153,38 @@ class RelayController:
         print("警告：连续多次通信失败，请检查设备连接！")
 
     # ===================== 状态读取方法 =====================
-    def get_all_device_status(self, command_type='devices'):
+    def get_all_device_status(self, command_type="devices"):
         command = self.read_status_command.get(command_type)
         if not command:
             print(f"未知的读取类型: {command_type}")
             return {}
 
-        source = 'sensor' if command_type == 'sensors' else 'device'
+        source = "sensor" if command_type == "sensors" else "device"
         response = self.send_command(command, source=source)
         status_dict = {}
 
         if response and len(response) >= 10:
             status_byte = response[9]
             status_bin = f"{status_byte:08b}"[::-1]
-            bit_map = self.device_bit_map if command_type == 'devices' else self.sensor_bit_map
-            name_map = self.device_name_map if command_type == 'devices' else self.sensor_name_map
+            bit_map = (
+                self.device_bit_map
+                if command_type == "devices"
+                else self.sensor_bit_map
+            )
+            name_map = (
+                self.device_name_map
+                if command_type == "devices"
+                else self.sensor_name_map
+            )
 
             for key, bit_index in bit_map.items():
-                status_dict[key] = status_bin[bit_index] == '1'
+                status_dict[key] = status_bin[bit_index] == "1"
         else:
             print(f"[{command_type}] 读取状态失败或响应无效")
 
         return status_dict
 
-    def get_all_sensor_responses(self, command_type='sensors'):
+    def get_all_sensor_responses(self, command_type="sensors"):
         """
         获取所有传感器的原始 Modbus 响应字符串
         示例：{'sensor1': '00000000000401020101', 'sensor2': '00000000000401020100'}
@@ -161,12 +194,12 @@ class RelayController:
             print(f"未知的读取类型: {command_type}")
             return {}
 
-        source = 'sensor' if command_type == 'sensors' else 'device'
+        source = "sensor" if command_type == "sensors" else "device"
         response = self.send_command(command, source=source)
         responses = {}
 
         if response and len(response) >= 10:
-            hex_response = binascii.hexlify(response).decode('utf-8')
+            hex_response = binascii.hexlify(response).decode("utf-8")
             print(f"[原始响应][{command_type}] {hex_response}")
 
             # 假设传感器数据从第 9 字节开始，长度为 2 字节
@@ -192,7 +225,7 @@ class RelayController:
     def is_valid_sensor_status(self, sensor_name):
         stable_count = 0
         for _ in range(int(self.sensor_stable_duration / self.sensor1_loop_delay)):
-            responses = self.get_all_sensor_responses('sensors')
+            responses = self.get_all_sensor_responses("sensors")
             response = responses.get(sensor_name)
 
             if not response:
@@ -211,7 +244,7 @@ class RelayController:
     def is_valid_sensor_status_1(self, sensor_name):
         stable_count = 0
         for _ in range(int(self.sensor_stable_duration / self.sensor2_loop_delay)):
-            responses = self.get_all_sensor_responses('sensors')
+            responses = self.get_all_sensor_responses("sensors")
             response = responses.get(sensor_name)
 
             if not response:
@@ -229,49 +262,57 @@ class RelayController:
         return False
 
     # ===================== 动作控制方法 =====================
-    def open(self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False):
+    def open(
+        self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False
+    ):
         status = self.get_all_device_status()
         if conveyor1 and not status.get(self.CONVEYOR1, False):
-            self.send_command(self.valve_commands[self.CONVEYOR1]['open'])
+            self.send_command(self.valve_commands[self.CONVEYOR1]["open"])
             time.sleep(self.delay_conveyor)
         if pusher and not status.get(self.PUSHER, False):
-            self.send_command(self.valve_commands[self.PUSHER]['open'])
+            self.send_command(self.valve_commands[self.PUSHER]["open"])
             time.sleep(self.delay_pusher)
         if conveyor2 and not status.get(self.CONVEYOR2, False):
-            self.send_command(self.valve_commands[self.CONVEYOR2]['open'])
+            self.send_command(self.valve_commands[self.CONVEYOR2]["open"])
             time.sleep(self.delay_conveyor)
         if clamp and not status.get(self.CLAMP, False):
-            self.send_command(self.valve_commands[self.CLAMP]['open'])
+            self.send_command(self.valve_commands[self.CLAMP]["open"])
             time.sleep(self.delay_clamp)
         if pusher1 and not status.get(self.PUSHER1, False):
-            self.send_command(self.valve_commands[self.PUSHER1]['open'])
+            self.send_command(self.valve_commands[self.PUSHER1]["open"])
             time.sleep(self.delay_pusher)
 
-    def close(self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False):
+    def close(
+        self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False
+    ):
         if conveyor1:
-            self.send_command(self.valve_commands[self.CONVEYOR1]['close'])
+            self.send_command(self.valve_commands[self.CONVEYOR1]["close"])
             time.sleep(self.delay_conveyor)
         if pusher:
-            self.send_command(self.valve_commands[self.PUSHER]['close'])
+            self.send_command(self.valve_commands[self.PUSHER]["close"])
             time.sleep(self.delay_pusher)
         if conveyor2:
-            self.send_command(self.valve_commands[self.CONVEYOR2]['close'])
+            self.send_command(self.valve_commands[self.CONVEYOR2]["close"])
             time.sleep(self.delay_conveyor)
         if clamp:
-            self.send_command(self.valve_commands[self.CLAMP]['close'])
+            self.send_command(self.valve_commands[self.CLAMP]["close"])
             time.sleep(self.delay_clamp)
         if pusher1:
-            self.send_command(self.valve_commands[self.PUSHER1]['close'])
+            self.send_command(self.valve_commands[self.PUSHER1]["close"])
             time.sleep(self.delay_pusher)
 
     # ===================== 传感器处理线程 =====================
     def handle_sensor1(self):
+        print("传感器线程1启动！！！")
         while self._running:
             try:
                 if self.is_valid_sensor_status(self.SENSOR1):
                     current_time = time.time()
-                    if not self.sensor1_triggered and (
-                            current_time - self.sensor1_last_time) > self.sensor1_debounce_time:
+                    if (
+                        not self.sensor1_triggered
+                        and (current_time - self.sensor1_last_time)
+                        > self.sensor1_debounce_time
+                    ):
                         self.sensor1_triggered = True
                         self.sensor1_last_time = current_time
                         # 1.停止包装机皮带电机：关闭 conveyor1
@@ -293,7 +334,7 @@ class RelayController:
                         time.sleep(1)
                         # 5. 状态检查（可选）
                         status = self.get_all_device_status()
-                        if status.get('conveyor1') and not status.get('pusher'):
+                        if status.get("conveyor1") and not status.get("pusher"):
                             print("流程完成1：皮带运行中，推板已收回")
                         else:
                             print("状态异常，请检查设备")
@@ -306,6 +347,7 @@ class RelayController:
                 time.sleep(self.sensor1_error_delay)
 
     def handle_sensor2(self):
+        print("传感器线程12启动！！！")
         while self._running:
             try:
                 if self.is_valid_sensor_status_1(self.SENSOR2):
@@ -357,7 +399,7 @@ class RelayController:
         self._sensor1_thread.start()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     controller = RelayController()
     controller.start()
     try: