chuyiwen/AutoControlSystem-G
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改drop，获取丢包路径

Browse Source
This commit is contained in:
chuyiwen
2025-09-03 19:50:58 +08:00
parent d295b83373
commit 2f45c4c38f
31 changed files with 812 additions and 177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

184
CU/Feeding.py
View File

@ -42,12 +42,13 @@ class FeedStatus(IntEnum):
FPhoto = 4
FTake = 5
FBroken1 = 6
FBroken2 =7
FDropMid = 7 # 暂时替换为扔包中间点
FShake = 8
FDropBag = 9
FFinished = 10
FReverse = 11
FStartReverse = 12
FDropReset = 10
FFinished = 11
FReverse = 12
FStartReverse = 13
class LineType(Enum):
Straight = 0
@ -60,6 +61,7 @@ class FeedMidStatus(Enum):
FMid_Start = 1
FMid_Take = 2
FMid_Feed= 3
FMid_Reverse = 4
class FeedPosition:
def __init__(self,status:FeedStatus,position:Real_Position):
@ -74,10 +76,18 @@ class FeedLine:
self.start2take_pos_index = 0
self.name = name
self.id = id
self.drop_manager = DropPositionManager("CU/drop.ini")
# --- 新增：用于存储从 ini 文件读取的多个投料点坐标 ---
# 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
self.drop_point_list = []
self.current_index = 1
def get_current_index(self):
return self.current_index
def set_current_index(self, index):
self.current_index = index
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]
@ -166,6 +176,7 @@ class FeedLine:
print("Warning: No position after FTake to update.")
break # 抓料点暂时就一个
def set_drop_position(self, position: Real_Position):
"""
设置 FDropBag 位置，只设置当前点，不处理前后点。
@ -179,6 +190,23 @@ class FeedLine:
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})")
break # 假设只有一个丢包点
def get_drop_path(self) -> list:
"""获取动态扔包路径"""
if self.drop_manager is None:
return []
path = []
while True:
pos_model = self.drop_manager.get_next_drop_position(
self.id,
self.current_index
)
if pos_model is None:
break
path.append(pos_model)
return path
def get_position_list(self):
index_start = -1
for i in range(len(self.feed_positions)):
@ -189,20 +217,46 @@ class FeedLine:
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
index_take = i
# 开始插入动态扔包点，按照 动态扔包中间点，扔包点，动态复位点的顺序
for i in range(len(self.feed_positions)):
if self.feed_positions[i].status == FeedStatus.FDropBag.value:
index_drop = i
# LineID = self.id
# 开始插入动态扔包中间点
# 开始插入动态扔包点
# 开始插入复位中间点
test_path = self.get_drop_path()
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)): #插入动态中间点
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
befor_position_model = PositionModel()
befor_position_model.init_position(None)
after_position_model = PositionModel()
after_position_model.init_position(None)
# 将总list的drop部分，替换为动态路径
self.feed_positions = self.feed_positions[:index_drop] + test_path
self.feeding_to_end = self.feed_positions[index_take:index_drop]
print(self.feed_positions)
# 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)
# after_position_model = PositionModel()
# 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)
# break
self.feeding_to_end.insert(i, befor_position_model)
self.feeding_to_end.insert(i+2, after_position_model)
break
class FeedingConfig:
def __init__(self, num: int, feedLine: FeedLine, photo_locs):
@ -234,7 +288,6 @@ class Feeding(QObject):
self.relay_controller = RelayController()
self.sensor_thread = None
self.detection_image = None
self.init_detection_image()
self.pause = False
self.cRis_photo = CRisOrFall()
self.cRis_shake = CRisOrFall()
@ -267,8 +320,7 @@ class Feeding(QObject):
self.sensor2_thread = None
# --- 新增: 用于码垛模式的投料点索引 ---
self.current_drop_index = 0
self.drop_manager = DropPositionManager("CU/drop.ini")
self.current_drop_index = 1
pass
def close_feed(self):
@ -276,10 +328,6 @@ class Feeding(QObject):
self.detect_thread.join()
self.detect.detection.release()
def init_detection_image(self):
detection_image = cv2.imread(Constant.feed_sign_path)
self.update_detect_image.emit(detection_image)
def run_detect(self):
while self.is_detected:
self.detect.run()
@ -289,7 +337,7 @@ class Feeding(QObject):
self.catch.run()
# 获取事件坐标
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,
@ -388,7 +436,6 @@ class Feeding(QObject):
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()
return
if not Constant.Debug:
@ -399,6 +446,7 @@ class Feeding(QObject):
#self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置
'''real_position'''
# 一直等待传感器2信号，永不超时
# TODO:逻辑需改变，不能用while循环
while True:
sensors = self.relay_controller.get_all_device_status('sensors')
sensor2_value = sensors.get(self.relay_controller.SENSOR2, False)
@ -408,10 +456,11 @@ class Feeding(QObject):
else:
self.log_signal.emit(logging.INFO, "⏳ 等待传感器2料包信号...")
time.sleep(1) # 每秒检查一次
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)
@ -420,9 +469,11 @@ class Feeding(QObject):
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
return
if not take_position.get_position().compare(real_position, is_action=True):
self.log_signal.emit(logging.INFO, "🟡 机器人尚未到达抓料点位")
self.log_signal.emit(logging.INFO, "机器人尚未到达抓料点位")
return
self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位")
self.log_signal.emit(logging.INFO, "机器人已到达抓料点位")
# 执行抓取动作
#self.catch.catch_status = CatchStatus.CTake
#if self.catch.catch_status == CatchStatus.CNone:
@ -435,17 +486,14 @@ class Feeding(QObject):
#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)
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}")
else:
self.log_signal.emit(logging.ERROR, "获取放置点失败")
return
#self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
self.relay_controller.open(clamp=True)
self.next_position()
self.next_position(self.is_reverse)
#self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
#return
#else:
#self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
@ -457,11 +505,13 @@ class Feeding(QObject):
self.next_position()
elif self.feedStatus == FeedStatus.FBroken2:
elif self.feedStatus == FeedStatus.FDropMid:
if self.get_current_position().get_position().compare(real_position):
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
self.next_position()
else:
self.next_position()
elif self.feedStatus == FeedStatus.FShake:
if self.get_current_position().get_position().compare(real_position,is_action=True):
@ -524,6 +574,12 @@ class Feeding(QObject):
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
# 调用 next_Feed()。
self.next_position()
elif self.feedStatus == FeedStatus.FDropReset:
self.next_position()
elif self.feedStatus == None:
print(self.feedStatus)
pass
def run_reset(self):
real_position = Real_Position()
@ -741,11 +797,14 @@ class Feeding(QObject):
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):
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.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.AXIS)
# 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)
else:
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed)
pass
@ -759,13 +818,17 @@ class Feeding(QObject):
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):
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)
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(), speed=self.robotClient.feed_speed,
move_type=MoveType.AXIS)
# 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)
else:
self.sendTargPosition(real_position=take_pos.get_position(),speed=self.robotClient.feed_speed)
@ -779,13 +842,20 @@ class Feeding(QObject):
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):
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)
# 暂时不考虑世界坐标到关节坐标的转换，强行不判断接近
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(), speed=self.robotClient.feed_speed,
move_type=MoveType.AXIS)
# 注释掉 by chuyiwen
# if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(),
# self.robotClient.max_angle_interval):
# self.feedStatus = FeedStatus.FNone
# 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)
else:
# if not reverse and self.feedStatus == FeedStatus.FShake:
@ -807,6 +877,8 @@ class Feeding(QObject):
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):
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
self.next_start(reverse)

BIN
CU/__pycache__/Catch.cpython-39.pyc
View File

Binary file not shown.

BIN
CU/__pycache__/Command.cpython-39.pyc
View File

Binary file not shown.

BIN
CU/__pycache__/Detect.cpython-39.pyc
View File

Binary file not shown.

BIN
CU/__pycache__/EMV.cpython-39.pyc
View File

Binary file not shown.

BIN
CU/__pycache__/Feeding.cpython-39.pyc
View File

Binary file not shown.

BIN
CU/__pycache__/drop.cpython-39.pyc
View File

Binary file not shown.

374
CU/drop.ini
View File

@ -1,88 +1,374 @@
[DropLine1]
id = 1
name = 50kg码垛路径
current_index = 92
current_index = 22
[DropLine2]
id = 2
name = 35kg码垛路径
current_index = 0
[DropMidPoint1-1]
x = 15.3
y = -84
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
[DropMidPoint1-2]
x = 15.3
y = -184
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
[DropMidPoint1-3]
x = 15.3
y = -284
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
[DropPoints1]
x = 15.3
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 0
[ResetPoint1-3]
x = 15.3
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
[ResetPoint1-2]
x = 15.3
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
[ResetPoint1-1]
x = 15.3
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
[DropMidPoint2-1]
x = 1379.783
y = -403.215
z = -200.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 7
linetype = 0
[DropMidPoint2-2]
x = 1379.783
y = -403.215
z = -250.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 7
linetype = 0
[DropMidPoint2-3]
x = 1379.783
y = -403.215
z = -300.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 7
linetype = 0
[DropPoints2]
x = 1379.783
y = -403.215
z = -334.279
u = 18.350
v = -0.985981
w = -125.710434
id = 3
order = 0
lineid = 1
status = 9
linetype = 0
[DropPoints2]
x = 1377.858
y = -215.07
z = -334.278
u = 18.349
v = -0.985981
w = -125.710434
id = 3
id = 2
order = 1
lineid = 1
status = 9
linetype = 0
[DropPoints3]
x = 1377.870
y = 190.876
z = -334.276
u = 18.347
[ResetPoint2-3]
x = 1379.783
y = -403.215
z = -366.0
u = 18.350
v = -0.985981
w = -125.710434
id = 3
order = 2
id = 2
order = 0
lineid = 1
status = 11
linetype = 0
[ResetPoint2-2]
x = 1379.783
y = -403.215
z = -466.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 11
linetype = 0
[ResetPoint2-1]
x = 1379.783
y = -403.215
z = -566.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 11
linetype = 0
[DropMidPoint3-1]
x = 1379.783
y = -403.215
z = -500.0
u = 18.350
v = -0.985981
w = -125.710434
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
action =
[DropMidPoint3-2]
x = 1379.783
y = -403.215
z = -400.0
u = 18.350
v = -0.985981
w = -125.710434
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
[DropMidPoint3-3]
x = 1379.783
y = -403.215
z = -300.0
u = 18.350
v = -0.985981
w = -125.710434
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
[DropPoints3]
x = 1379.783
y = -403.215
z = -334.279
u = 18.350
v = -0.985981
w = -125.710434
id = 1
order = 1
lineid = 1
status = 9
linetype = 0
[ResetPoint3-3]
x = 1379.783
y = -403.215
z = -200.0
u = 18.350
v = -0.985981
w = -125.710434
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
[ResetPoint3-2]
x = 1379.783
y = -403.215
z = -100.0
u = 18.350
v = -0.985981
w = -125.710434
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
[ResetPoint3-1]
x = 1379.783
y = -403.215
z = -50.0
u = 18.350
v = -0.985981
w = -125.710434
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
[DropMidPoint4-1]
x = 1379.783
y = -403.215
z = -200.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 7
linetype = 0
[DropMidPoint4-2]
x = 1379.783
y = -403.215
z = -250.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 7
linetype = 0
[DropMidPoint4-3]
x = 1379.783
y = -403.215
z = -300.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 7
linetype = 0
[DropPoints4]
x = 1874.282
y = -328.797
z = -334.274
u = 18.346
x = 1379.783
y = -403.215
z = -334.279
u = 18.350
v = -0.985981
w = -125.710434
id = 3
order = 3
id = 2
order = 1
lineid = 1
status = 9
linetype = 0
[DropPoints5]
x = 1874.282
y = -67.365
z = -334.274
u = 18.346
[ResetPoint4-3]
x = 1379.783
y = -403.215
z = -366.0
u = 18.350
v = -0.985981
w = -125.710434
id = 3
order = 4
id = 2
order = 0
lineid = 1
status = 9
status = 11
linetype = 0
[DropPoints6]
x = 1874.282
y = 330.375
z = -334.274
u = 18.346
[ResetPoint4-2]
x = 1379.783
y = -403.215
z = -466.0
u = 18.350
v = -0.985981
w = -125.710434
id = 3
order = 5
id = 2
order = 0
lineid = 1
status = 9
status = 11
linetype = 0
[ResetPoint4-1]
x = 1379.783
y = -403.215
z = -566.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
order = 0
lineid = 1
status = 11
linetype = 0

245
CU/drop.py
View File

@ -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}")

15
CU/list.ini
View File

@ -1,12 +1,5 @@
[positions]
0 = -569.543396, -1299.659543, -1269.931256, -151.12764, 0.258, 0.258
1 = -569.543396, -1299.659543, -1269.931256, -151.12764, 0.258, 0.258
2 = -569.543396, -1299.659543, -1269.931256, -151.12764, 0.258, 0.258
3 = -569.543396, -1299.659543, -1269.931256, -151.12764, 0.258, 0.258
4 = -42.885, -1432.385, -300.451, -59.044, 7.258, 7.258
5 = -42.885, -1432.385, -280.451, -59.044, 7.258, 7.258
6 = -42.885, -1432.385, -410.451, -59.044, 7.258, 7.258
7 = -42.885, -1432.385, -380.451, -59.044, 7.258, 7.258
8 = -42.885, -1432.385, -340.451, -59.044, 7.258, 7.258
9 = -42.885, -1432.385, -300.451, -59.044, 7.258, 7.258
10 = -42.885, -1432.385, -280.451, -59.044, 7.258, 7.258
0 = -569.543396, -1299.659543, -1069.931256, -151.12764, 0.258, 0.258
1 = -569.543396, -1299.659543, -1069.931256, -151.12764, 0.258, 0.258
2 = -569.543396, -1299.659543, -1069.931256, -151.12764, 0.258, 0.258
3 = -569.543396, -1299.659543, -1069.931256, -151.12764, 0.258, 0.258