修改drop，获取丢包路径

CU/drop.py

@@ -4,101 +4,214 @@ import configparser
 import os
 from typing import Optional
 from Model.Position import Real_Position
+from Model.FeedModel import PositionModel
+
 
 class DropPositionManager:
-    def __init__(self, config_path="drop.ini"):
+    def __init__(self, config_path="CU/drop.ini"):
         self.config_path = config_path
         self.config = configparser.ConfigParser()
         self._load_config()
 
+        # --- 本地缓存 ---
+        self._current_lineid: Optional[int] = None
+        self._current_point_id: Optional[int] = None
+        self._current_path: list = []  # 当前路径点列表
+        self._current_index: int = 0  # 当前路径中的索引
+
     def _load_config(self):
         if not os.path.exists(self.config_path):
             raise FileNotFoundError(f"配置文件不存在: {self.config_path}")
         self.config.read(self.config_path, encoding='utf-8')
 
-    def _save_config(self):
-        with open(self.config_path, 'w', encoding='utf-8') as f:
-            self.config.write(f)
+    def get_next_drop_position(self, lineid: int, point: int) -> Optional[PositionModel]:
+        """
+        获取指定 lineid 和 point 的下一个路径点。
 
-    def get_next_drop_position(self, lineid: int = 1) -> Optional[Real_Position]:
-        print(f"\n🔄 开始获取 lineid={lineid} 的下一个 drop 点...")
+        :param lineid: 路径组 ID（对应 DropLineX）
+        :param point: 投料点 ID（对应 DropPoints{point}）
+        :return: 下一个路径点，若无则返回 None
+        """
+        # 如果 lineid 或 point 改变，重新加载路径
+        if self._current_lineid != lineid or self._current_point_id != point:
+            self._load_point_path(lineid, point)
+            self._current_lineid = lineid
+            self._current_point_id = point
+            self._current_index = 0
 
-        line_section = f"DropLine{lineid}"
-        print(f"🔍 查找路径 section: {line_section}")
+        # 返回当前索引的点
+        if self._current_index < len(self._current_path):
+            next_point = self._current_path[self._current_index]
+            self._current_index += 1
+            pos = next_point.get_position()
+            print(f"🎯 返回点: status={next_point.status}, linetype={next_point.lineType}, "
+                  f"position=({pos.X:.3f}, {pos.Y:.3f}, {pos.Z:.3f})")
+            return next_point
 
-        if not self.config.has_section(line_section):
-            print(f"❌ 错误：配置文件中不存在 section [{line_section}]")
-            print(f"📊 所有 sections: {list(self.config.sections())}")
-            return None
+        # 路径结束
+        print("✅ 当前点集合路径已结束")
+        return None
 
-        current_index = self.config.getint(line_section, "current_index", fallback=0)
-        print(f"📌 当前索引: {current_index} (fallback=0 如果未设置)")
+    def _load_point_path(self, lineid: int, point_id: int):
+        """加载指定 lineid 和 point_id 的完整路径"""
+        self._current_path = []
 
-        points = []
-        found_sections = []
-        matched_sections = []
+        # 检查是否存在 DropPoints{point_id}
+        drop_point_sec = f"DropPoints{point_id}"
+        if not self.config.has_section(drop_point_sec):
+            print(f"❌ 配置错误：不存在 {drop_point_sec}")
+            return
 
+        if self.config.getint(drop_point_sec, "lineid") != lineid:
+            print(f"❌ {drop_point_sec} 不属于 lineid={lineid}")
+            return
+
+        try:
+            drop_pos = self._read_position_from_section(drop_point_sec)
+        except Exception as e:
+            print(f"❌ 读取 {drop_point_sec} 失败: {e}")
+            return
+
+        # 1. 加载 DropMidPoint{point_id}-*（按 level 升序）
+        mid_points = []
         for sec in self.config.sections():
-            if sec.startswith("DropPoints"):
-                found_sections.append(sec)
+            if sec.startswith(f"DropMidPoint{point_id}-") and self.config.getint(sec, "lineid") == lineid:
                 try:
-                    file_lineid = self.config.getint(sec, "lineid")
-                    print(f"🔍 {sec}: lineid={file_lineid}")
-                    if file_lineid == lineid:
-                        matched_sections.append(sec)
-                        pos = Real_Position()
-                        pos.X = self.config.getfloat(sec, "x")
-                        pos.Y = self.config.getfloat(sec, "y")
-                        pos.Z = self.config.getfloat(sec, "z")
-                        pos.U = self.config.getfloat(sec, "u")
-                        pos.V = self.config.getfloat(sec, "v")
-                        pos.W = self.config.getfloat(sec, "w")
-                        order = self.config.getint(sec, "order")
-                        points.append((order, pos))
-                        print(f"✅ 匹配到点: {sec}, order={order}, 位置=({pos.X:.3f}, {pos.Y:.3f})")
+                    level = int(sec.split('-')[1])
+                    pos = self._read_position_from_section(sec)
+                    mid_points.append((level, pos))
                 except Exception as e:
-                    print(f"❌ 读取 {sec} 失败: {e}")
+                    print(f"❌ 解析 {sec} 失败: {e}")
+        mid_points.sort(key=lambda x: x[0])
 
-        print(f"📁 找到 DropPoints: {found_sections}")
-        print(f"🟢 匹配 lineid={lineid} 的点: {matched_sections}")
-        print(f"📊 共收集 {len(points)} 个有效点")
+        # 2. 加载 ResetPoint{point_id}-*（按 level 升序）
+        reset_points = []
+        for sec in self.config.sections():
+            if sec.startswith(f"ResetPoint{point_id}-") and self.config.getint(sec, "lineid") == lineid:
+                try:
+                    level = int(sec.split('-')[1])
+                    pos = self._read_position_from_section(sec)
+                    reset_points.append((level, pos))
+                except Exception as e:
+                    print(f"❌ 解析 {sec} 失败: {e}")
+        reset_points.sort(key=lambda x: x[0])
 
-        if not points:
-            print("❌ 没有可用的 drop 点！请检查：lineid 是否匹配，字段是否正确")
-            return None
+        # 3. 组装路径
+        # a. DropMidPoint
+        for _, pos in mid_points:
+            model = PositionModel()
+            model.init_position(pos)
+            model.status = 7  # FMid
+            model.lineType = 4  # WORLD
+            self._current_path.append(model)
 
-        # 按 order 排序
-        points.sort(key=lambda x: x[0])
-        sorted_positions = [pos for _, pos in points]
-        count = len(sorted_positions)
-        actual_index = current_index % count
+        # b. DropPoints
+        main_model = PositionModel()
+        main_model.init_position(drop_pos)
+        main_model.status = 9  # FDropBag
+        main_model.lineType = 4
+        self._current_path.append(main_model)
 
-        selected_pos = sorted_positions[actual_index]
-        print(f"🎯 选择第 {actual_index} 个点: X={selected_pos.X:.3f}, Y={selected_pos.Y:.3f}, Z={selected_pos.Z:.3f}")
+        # c. ResetPoint
+        for _, pos in reset_points:
+            model = PositionModel()
+            model.init_position(pos)
+            model.status = 11  # FReverse
+            model.lineType = 4
+            self._current_path.append(model)
 
-        # 索引 +1 并保存回 ini
-        next_index = current_index + 1
-        self.config.set(line_section, "current_index", str(next_index))
-        self._save_config()
-        print(f"💾 已保存 current_index = {next_index} 到 [{line_section}]")
+        print(f"✅ 已加载 DropLine{lineid} 中 DropPoints{point_id} 的路径，共 {len(self._current_path)} 个点")
 
-        return selected_pos
+    def _read_position_from_section(self, section: str) -> Real_Position:
+        """从配置文件的 section 中读取位置信息"""
+        pos = Real_Position()
+        pos.X = self.config.getfloat(section, "x")
+        pos.Y = self.config.getfloat(section, "y")
+        pos.Z = self.config.getfloat(section, "z")
+        pos.U = self.config.getfloat(section, "u")
+        pos.V = self.config.getfloat(section, "v")
+        pos.W = self.config.getfloat(section, "w")
+        return pos
 
+def _get_point_debug_info(manager, pos, model):
+    config = manager.config
+    for sec in config.sections():
+        if sec.startswith("DropPoints"):
+            try:
+                x, y, z = config.getfloat(sec, "x"), config.getfloat(sec, "y"), config.getfloat(sec, "z")
+                if abs(x - pos.X) < 0.001 and abs(y - pos.Y) < 0.001 and abs(z - pos.Z) < 0.001:
+                    point_id = config.getint(sec, "id")
+                    return f"📌 DropPoints{point_id} | id={point_id}"
+            except: pass
 
-# ✅ 主函数：用于测试
+        elif sec.startswith("DropMidPoint"):
+            try:
+                parts = sec.split('-')
+                if len(parts) != 2: continue
+                point_id = int(''.join(filter(str.isdigit, parts[0])))
+                level = int(parts[1])
+                x, y, z = config.getfloat(sec, "x"), config.getfloat(sec, "y"), config.getfloat(sec, "z")
+                if abs(x - pos.X) < 0.001 and abs(y - pos.Y) < 0.001 and abs(z - pos.Z) < 0.001:
+                    return f"📍 DropMidPoint{point_id}-{level} | id={point_id}, level={level}"
+            except: pass
+
+        elif sec.startswith("ResetPoint"):
+            try:
+                parts = sec.split('-')
+                if len(parts) != 2: continue
+                point_id = int(''.join(filter(str.isdigit, parts[0])))
+                level = int(parts[1])
+                x, y, z = config.getfloat(sec, "x"), config.getfloat(sec, "y"), config.getfloat(sec, "z")
+                if abs(x - pos.X) < 0.001 and abs(y - pos.Y) < 0.001 and abs(z - pos.Z) < 0.001:
+                    return f"🔙 ResetPoint{point_id}-{level} | id={point_id}, level={level}"
+            except: pass
+    return "❓ 未知点位"
+
+# 测试
 if __name__ == "__main__":
-    # 创建管理器，加载 drop.ini
     manager = DropPositionManager("drop.ini")
+    lineid = 1
 
-    # 获取第一个 drop 点（lineid=1）
-    pos = manager.get_next_drop_position(lineid=1)
+    print(f"\n🔁 测试：通过 point 参数切换路径集合\n")
 
-    if pos is not None:
-        print(f"\n🎉 成功返回点位: X={pos.X}, Y={pos.Y}, Z={pos.Z}, U={pos.U}, V={pos.V}, W={pos.W}")
-    else:
-        print("\n❌ get_next_drop_position 返回了 None，请根据上面的日志排查")
-        print("💡 常见原因：")
-        print("   1. drop.ini 缺少 current_index")
-        print("   2. lineid 不匹配")
-        print("   3. Real_Position 属性名错误（应为大写 X/Y/Z）")
-        print("   4. 文件路径不对")
+    # 先走 point=1
+    print("=" * 60)
+    print("📦 开始执行 point=1 的路径")
+    print("=" * 60)
+    for i in range(10):
+        pos_model = manager.get_next_drop_position(lineid=lineid, point=1)
+        if pos_model is None:
+            print("🔚 point=1 路径结束")
+            break
+        pos = pos_model.get_position()
+        info = _get_point_debug_info(manager, pos, pos_model)
+        print(f"  🚀 {info}")
+        if pos_model.status == 9:
+            print("  💥 执行【扔包】操作！")
+
+    # 再走 point=2
+    print("\n" + "=" * 60)
+    print("📦 开始执行 point=2 的路径")
+    print("=" * 60)
+    for i in range(10):
+        pos_model = manager.get_next_drop_position(lineid=lineid, point=2)
+        if pos_model is None:
+            print("🔚 point=2 路径结束")
+            break
+        pos = pos_model.get_position()
+        info = _get_point_debug_info(manager, pos, pos_model)
+        print(f"  🚀 {info}")
+        if pos_model.status == 9:
+            print("  💥 执行【扔包】操作！")
+
+    # 再切回 point=1
+    print("\n" + "=" * 60)
+    print("🔄 切换回 point=1，从头开始")
+    print("=" * 60)
+    for i in range(3):
+        pos_model = manager.get_next_drop_position(lineid=lineid, point=1)
+        if pos_model is None:
+            break
+        pos = pos_model.get_position()
+        info = _get_point_debug_info(manager, pos, pos_model)
+        print(f"  🚀 {info}")