first commit

CU/sae.py

@@ -0,0 +1,178 @@
+import logging
+from types import SimpleNamespace
+
+# 模拟 Constant
+class Constant:
+    str_feed_take = "[测试] 开始抓料流程"
+    str_feed_take_success = "[测试] ✅ 抓料成功"
+    str_feed_takePhoto_fail = "[测试] ❌ 抓料点位获取失败"
+
+# 模拟 FeedStatus 和 CatchStatus
+class FeedStatus:
+    FTake = "FTake"
+
+class CatchStatus:
+    CNone = "CNone"
+    CTake = "CTake"
+    COk = "COk"
+
+class DetectStatus:
+    DOk = "DOk"
+
+# 模拟 Position 类
+class Position:
+    def __init__(self, x=0, y=0, z=0, a=0, b=0, c=0):
+        self.X = x
+        self.Y = y
+        self.Z = z
+        self.A = a
+        self.B = b
+        self.C = c
+
+    def get_position(self):
+        return self
+
+    def compare(self, other, is_action=False):
+        return (
+            abs(self.X - other.X) < 0.1 and
+            abs(self.Y - other.Y) < 0.1 and
+            abs(self.Z - other.Z) < 0.1
+        )
+
+# 模拟 Detect 类
+class Detect:
+    def __init__(self):
+        self.detect_position = None
+        self.position_index = 0
+        self.detect_status = DetectStatus.DOk
+        # 模拟配置点位（类似 list.ini）
+        self.positions = {
+            0: Position(100, 200, 300, 0, 0, 0),
+            1: Position(150, 250, 350, 0, 0, 0),
+            2: Position(200, 300, 400, 0, 0, 0),
+            3: Position(100, 200, 300, 0, 0, 0),
+            4: Position(150, 250, 350, 0, 0, 0),
+            5: Position(200, 300, 400, 0, 0, 0),
+
+        }
+
+    def run(self):
+        """根据当前索引加载点位"""
+        pos = self.positions.get(self.position_index, None)
+        if pos:
+            self.detect_position = pos
+            return True
+        else:
+            print(f"[❌ 失败] 索引 {self.position_index} 没有对应点位")
+            return False
+
+# 模拟 FeedLine 类
+class FeedLine:
+    def __init__(self):
+        self.drop_position = Position()
+
+    def get_take_position(self):
+        return SimpleNamespace(get_position=lambda: Position(100, 200, 300))
+
+    def set_drop_position(self, position):
+        self.drop_position = position
+
+# 模拟 RelayController
+class RelayController:
+    def get_all_device_status(self, *args):
+        return {"SENSOR2": True}
+
+    def open(self, conveyor2=False):
+        pass
+
+# 模拟主类，用于测试
+class TestRunner:
+    def __init__(self):
+        self.feedStatus = FeedStatus.FTake
+        self.catch = SimpleNamespace(catch_status=CatchStatus.CNone)
+        self.detect = Detect()
+        self.debug_run_count = 0
+        self.sensor2_ready = True
+        self.relay_controller = RelayController()
+
+        # feedConfig 模拟
+        self.feedConfig = SimpleNamespace(feedLine=FeedLine())
+
+        # real_position 模拟
+        self.real_position = Position(100, 200, 300)
+
+    def log_signal(self, level, msg):
+        logging.log(level, msg)
+
+    def next_position(self):
+        self.log_signal(logging.INFO, "[调试] 模拟移动到下一个位置")
+
+    def run_test(self):
+        if self.feedStatus == FeedStatus.FTake:
+            self.log_signal(logging.INFO, Constant.str_feed_take)
+            take_position = self.feedConfig.feedLine.get_take_position()
+            if not take_position or not take_position.get_position():
+                self.log_signal(logging.ERROR, Constant.str_feed_takePhoto_fail)
+                return
+
+            if not take_position.get_position().compare(self.real_position, is_action=True):
+                self.log_signal(logging.INFO, "🟡 机器人尚未到达抓料点位")
+                return
+
+            self.log_signal(logging.INFO, "🟢 机器人已到达抓料点位")
+
+            # 传感器等待逻辑（模拟已触发）
+            sensors = self.relay_controller.get_all_device_status('sensors')
+            sensor2_value = sensors.get("SENSOR2", False)
+            if not sensor2_value:
+                self.log_signal(logging.INFO, "⏳ 等待传感器2料包信号...")
+                return
+
+            self.log_signal(logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取")
+
+            # 执行抓取动作
+            if self.catch.catch_status == CatchStatus.CNone:
+                self.catch.catch_status = CatchStatus.CTake
+            if self.catch.catch_status == CatchStatus.CTake:
+                self.log_signal(logging.INFO, "正在执行抓料动作...")
+                self.catch.catch_status = CatchStatus.COk
+            if self.catch.catch_status == CatchStatus.COk:
+                self.log_signal(logging.INFO, Constant.str_feed_take_success)
+                if not self.sensor2_ready:
+                    self.log_signal(logging.INFO, "抓取完成，重新启动 conveyor2")
+                    self.relay_controller.open(conveyor2=True)
+
+                self.catch.catch_status = CatchStatus.CNone
+                self.detect.detect_status = DetectStatus.DOk
+                self.log_signal(logging.INFO, "修改抓取点")
+                print(f"[调试] 即将丢包点位索引: {self.detect.position_index}")
+                # ✅ 索引递增
+                self.detect.position_index += 1
+                print(f"[调试] 下一个要加载的索引: {self.detect.position_index}")
+
+                # ✅ 加载新点位
+                if not self.detect.run():
+                    self.log_signal(logging.ERROR, "❌ 加载新点位失败，停止流程")
+                    return
+
+                # ✅ 设置丢包点
+                self.feedConfig.feedLine.set_drop_position(self.detect.detect_position)
+
+                self.debug_run_count += 1
+                self.next_position()
+                return
+            else:
+                self.log_signal(logging.ERROR, Constant.str_feed_takePhoto_fail)
+
+# ========================
+# 主程序入口
+# ========================
+if __name__ == "__main__":
+    logging.basicConfig(level=logging.INFO, format='%(asctime)s [%(levelname)s] %(message)s')
+
+    runner = TestRunner()
+    print("🔧 开始测试索引递增和点位加载逻辑...\n")
+
+    for i in range(5):
+        print(f"\n🔄 第 {i+1} 次测试")
+        runner.run_test()