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

米厂码垛修改

Browse Source
This commit is contained in:
fujinliang
2025-09-10 09:16:57 +08:00
parent 2f45c4c38f
commit 65dde435c8
47 changed files with 11258 additions and 10485 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
CU/CsvToIni.py Normal file
View File

@ -0,0 +1,58 @@
import csv
def format_value(val):
"""格式化值：整数保持原样，小数保留3位"""
try:
num_val = float(val)
if num_val.is_integer():
return int(num_val)
return round(num_val, 3)
except ValueError:
return val
def csv_to_ini(csv_file, ini_file):
"""
将CSV文件转换为INI格式
:param csv_file: 输入的CSV文件路径
:param ini_file: 输出的INI文件路径
"""
try:
with open(csv_file, 'r', newline='', encoding='utf-8') as csv_input:
# 读取CSV文件
reader = csv.DictReader(csv_input)
# 确定所需字段
required_fields = ['name', 'x', 'y', 'z', 'u', 'v', 'w',
'id', 'order', 'lineid', 'status', 'linetype']
with open(ini_file, 'w', encoding='utf-8') as ini_output:
# 处理每一行数据
for row in reader:
# 检查是否所有必需字段都存在
if not all(field in row for field in required_fields):
missing = [field for field in required_fields if field not in row]
print(f"警告: 行 {reader.line_num} 缺少字段 {', '.join(missing)}，跳过该行")
continue
# 写入section头
ini_output.write(f"[{row['name']}]\n")
# 写入数值字段
for field in required_fields[1:]: # 跳过name字段
formatted = format_value(row[field])
ini_output.write(f"{field} = {formatted}\n")
# 区块间添加空行
ini_output.write("\n")
print(f"转换成功！INI文件已保存至: {ini_file}")
except Exception as e:
print(f"处理过程中出错: {str(e)}")
if __name__ == "__main__":
# 在此输入您的文件路径
input_csv = "D:/aa.csv" # 替换为实际CSV文件路径
output_ini = "D:/output.ini" # 输出的INI文件路径
csv_to_ini(input_csv, output_ini)

3
CU/EMV.py
View File

@ -1,6 +1,7 @@
import socket
import binascii
import time
import Constant
# 网络继电器的 IP 和端
HOST = '192.168.0.18'
@ -24,6 +25,8 @@ valve_commands = {
# 将十六进制字符串转换为字节数据并发送
def send_command(command):
if Constant.DebugPosition:
return True
byte_data = binascii.unhexlify(command)
# 创建套接字并连接到继电器
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:

177
CU/Feeding.py
View File

@ -1,4 +1,5 @@
import copy
from dis import stack_effect
import logging
import random
import threading
@ -51,9 +52,13 @@ class FeedStatus(IntEnum):
FStartReverse = 13
class LineType(Enum):
#直线
Straight = 0
#曲线中间点
CureMid = 2
#曲线终点
CureEnd = 3
#关节（自由路径）
WORLD = 4
@ -69,19 +74,25 @@ class FeedPosition:
self.position = position
class FeedLine:
def __init__(self, id, name, feed_positions:list):
def __init__(self, id, name, feed_positions:list,remain_count:int):
self.feed_positions = copy.deepcopy(feed_positions)
self.feeding2end_pos_index = 0
self.origin2start_pos_index = 0
self.start2take_pos_index = 0
self.name = name
self.id = id
self.drop_manager = DropPositionManager("CU/drop.ini")
# 初始化各个阶段的位置列表
self.feeding_to_end = []
# --- 新增：用于存储从 ini 文件读取的多个投料点坐标 ---
# 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
self.drop_point_list = []
self.current_index = 1
#读取码垛点位置
self.current_index = remain_count+1
#记录feed_positions当前扔包点索引
self.current_dropbag_index=0
def get_current_index(self):
return self.current_index
@ -198,6 +209,8 @@ class FeedLine:
path = []
while True:
#current_index,当前扔包点id
#drop.ini定义point1,point2,current_index对应后面的数字
pos_model = self.drop_manager.get_next_drop_position(
self.id,
self.current_index
@ -217,12 +230,13 @@ 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
#开始插入动态扔包点，按照 动态扔包中间点，扔包点，动态复位点的顺序
#记录feed_positions扔包点初始位置,后续动态增加路径仍从此获取
if self.current_dropbag_index==0:
for i in range(len(self.feed_positions)):
if self.feed_positions[i].status == FeedStatus.FDropBag.value:
self.current_dropbag_index = i
break
# 开始插入动态扔包中间点
@ -230,16 +244,16 @@ class FeedLine:
# 开始插入复位中间点
test_path = self.get_drop_path()
# 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]
# 将总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)
# self.feed_positions = self.feed_positions[:index_drop] + test_path
# self.feeding_to_end = self.feed_positions[index_take:index_drop]
self.feeding_to_end = self.feed_positions[index_take:]
# print(self.feed_positions)
# for i in range(len(self.feeding_to_end)): #插入动态中间点
# if self.feeding_to_end[i].status == FeedStatus.FTake.value:
@ -252,15 +266,32 @@ class FeedLine:
# self.feeding_to_end.insert(i+2, after_position_model)
# break
def set_feeding_to_end(self):
for i in range(len(self.feed_positions)):
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
index_take = i
break
# 开始插入动态扔包点，按照 动态扔包中间点，扔包点，动态复位点的顺序
# for i in range(len(self.feed_positions)):
# if self.feed_positions[i].status == FeedStatus.FDropBag.value:
# index_drop = i
# break
index_drop=self.current_dropbag_index
test_path = self.get_drop_path()
self.current_index+=1
# 将总list的drop部分，替换为动态路径
self.feed_positions = self.feed_positions[:index_drop] + test_path
# self.feeding_to_end = self.feed_positions[index_take:index_drop]
self.feeding_to_end = self.feed_positions[index_take:index_drop]+test_path
class FeedingConfig:
def __init__(self, num: int, feedLine: FeedLine, photo_locs):
def __init__(self, num: int, feedLine: FeedLine, photo_locs,remain_count:int):
#需码垛数量，如50，或30
self.num = num
#已经码垛数量
self.remain_count=remain_count
self.feedLine = feedLine
self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
@ -279,6 +310,8 @@ class Feeding(QObject):
take_no_photo_sigal = Signal()
update_detect_image = Signal(np.ndarray)
log_signal = Signal(int,str)
#码垛完成通知
stack_finish_signal=Signal()
def __init__(self, robotClient: RobotClient):
super().__init__()
self.feedConfig = None
@ -326,7 +359,8 @@ class Feeding(QObject):
def close_feed(self):
self.is_detected = False
self.detect_thread.join()
self.detect.detection.release()
if self.detect.detection:
self.detect.detection.release()
def run_detect(self):
while self.is_detected:
@ -337,13 +371,19 @@ class Feeding(QObject):
self.catch.run()
# 获取事件坐标
real_position = Real_Position()
real_position.init_position(self.robotClient.status_model.world_0,
real_position.init_position_joint_and_world(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)
self.robotClient.status_model.world_5,
self.robotClient.status_model.axis_0,
self.robotClient.status_model.axis_1,
self.robotClient.status_model.axis_2,
self.robotClient.status_model.axis_3,
self.robotClient.status_model.axis_4,
self.robotClient.status_model.axis_5
)
# real_position.init_position(0,
# 0,
# 0,
@ -359,8 +399,9 @@ 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.is_reverse and real_position.compare(self.robotClient.origin_position,is_action=True):
self.feedStatus = FeedStatus.FNone
self.is_reverse = False
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
@ -386,7 +427,7 @@ 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 real_position.compare(self.feedConfig.feedLine.start_to_take[0].get_position()):
self.next_position(self.is_reverse)
elif self.feedStatus == FeedStatus.FStart:
@ -394,7 +435,7 @@ class Feeding(QObject):
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:
if not real_position.compare(self.robotClient.origin_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)
@ -412,7 +453,7 @@ class Feeding(QObject):
elif self.feedStatus == FeedStatus.FMid:
feed_pos = self.get_current_position(self.is_reverse)
if feed_pos.get_position().compare(real_position):
if real_position.compare(feed_pos.get_position()):
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
self.next_position(self.is_reverse)
# 增加计数器逻辑
@ -428,18 +469,26 @@ class Feeding(QObject):
#self.catch.catch_status = CatchStatus.CTake
elif self.feedStatus == FeedStatus.FPhoto:
if self.feedConfig.num == 0:
#码垛的数量和配置的数量一致时
if self.feedConfig.remain_count >=self.feedConfig.num:
#关闭，暂停
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.remain_count=0
self.next_position(self.is_reverse)
#码垛完成信号通知
self.stack_finish_signal.emit()
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
return
if not Constant.Debug:
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto)
if(self.feed_Mid_Status == FeedMidStatus.FMid_Feed):
self.feedConfig.feedLine.set_feeding_to_end()
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)
@ -456,10 +505,9 @@ 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
elif self.feedStatus == FeedStatus.FTake:
@ -468,7 +516,7 @@ class Feeding(QObject):
if not take_position or not take_position.get_position():
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
return
if not take_position.get_position().compare(real_position, is_action=True):
if not real_position.compare(take_position.get_position(), is_action=True):
self.log_signal.emit(logging.INFO, "机器人尚未到达抓料点位")
return
self.log_signal.emit(logging.INFO, "机器人已到达抓料点位")
@ -500,21 +548,19 @@ class Feeding(QObject):
elif self.feedStatus == FeedStatus.FBroken1:
if self.get_current_position().get_position().compare(real_position):
if real_position.compare(self.get_current_position().get_position()):
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
self.next_position()
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:
if real_position.compare(self.get_current_position().get_position(),is_action=True):
self.log_signal.emit(logging.INFO, Constant.str_feed_drop_mid)
self.next_position()
elif self.feedStatus == FeedStatus.FShake:
if self.get_current_position().get_position().compare(real_position,is_action=True):
if real_position.compare(self.get_current_position().get_position(),is_action=True):
# TODO 震动方案
self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
if self.catch.catch_status == CatchStatus.CNone:
@ -537,13 +583,14 @@ class Feeding(QObject):
# 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):
if real_position.compare(self.get_current_position().get_position(),is_action=True):
# 2. 记录日志：已到达投料点
#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 模块进行状态交互来驱动投料动作
# a. 初始状态 (CNone): 触发投料动作
@ -567,15 +614,21 @@ class Feeding(QObject):
# (后续增加) 视觉确认: 拍照确认袋子已放置
# self.detection.get_position(...)
# 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.feedConfig.remain_count = self.feedConfig.remain_count + 1
# self.feedConfig.num = self.feedConfig.num - 1
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.remain_count}')
# 5. *** 关键步骤 ***: 移动到路径中的下一个点
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
# 调用 next_Feed()。
self.next_position()
elif self.feedStatus == FeedStatus.FDropReset:
self.next_position()
elif self.feedStatus == FeedStatus.FDropReset:
if real_position.compare(self.get_current_position().get_position()):
self.log_signal.emit(logging.INFO, Constant.str_feed_drop_reset)
self.next_position()
elif self.feedStatus == None:
print(self.feedStatus)
@ -583,12 +636,18 @@ class Feeding(QObject):
def run_reset(self):
real_position = Real_Position()
real_position.init_position(self.robotClient.status_model.world_0,
real_position.init_position_joint_and_world(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)
self.robotClient.status_model.world_5,
self.robotClient.status_model.axis_0,
self.robotClient.status_model.axis_1,
self.robotClient.status_model.axis_2,
self.robotClient.status_model.axis_3,
self.robotClient.status_model.axis_4,
self.robotClient.status_model.axis_5)
if self.reset_status == ResetStatus.RNone:
return
@ -601,7 +660,7 @@ 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 real_position.compare(pos_model.get_position(),is_action=True):
self.pos_index = index
break
@ -639,6 +698,11 @@ class Feeding(QObject):
real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
self.current_position = pos_model1
self.reverse_index = self.reverse_index + 2
elif pos_model.lineType==LineType.WORLD.value:
#关节移动
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed,move_type=MoveType.AXIS)
self.current_position = pos_model
self.reverse_index = self.reverse_index + 1
else:
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
self.current_position = pos_model
@ -654,12 +718,18 @@ class Feeding(QObject):
self.run_reverse = True
real_position = Real_Position()
real_position.init_position(self.robotClient.status_model.world_0,
real_position.init_position_joint_and_world(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)
self.robotClient.status_model.world_5,
self.robotClient.status_model.axis_0,
self.robotClient.status_model.axis_1,
self.robotClient.status_model.axis_2,
self.robotClient.status_model.axis_3,
self.robotClient.status_model.axis_4,
self.robotClient.status_model.axis_5)
if self.feedConfig == None: return
start_index = -1
# for index in range(len(self.feedConfig.feedLine.positions)):
@ -669,7 +739,7 @@ class Feeding(QObject):
pos_near_index = -1
reversed_positions = []
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
if pos_model.get_position().compare(real_position):
if real_position.compare(pos_model.get_position()):
pos_index = index
break
@ -769,9 +839,8 @@ class Feeding(QObject):
try:
print('fuck1',log_str)
self.log_signal.emit(logging.INFO, log_str)
print('fuck2',log_str)
print(log_str)
except:
return
print(request_command)
@ -843,8 +912,8 @@ class Feeding(QObject):
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:
# 暂时不考虑世界坐标到关节坐标的转换，强行不判断接近
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
# 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)

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.

922
CU/drop copy.ini Normal file
View File

@ -0,0 +1,922 @@
[DropLine1]
id = 1
name = 50kg码垛路径
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 = 4
[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 = 4
[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 = 4
[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 = 4
[ResetPoint1-3]
x = 15.3
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint1-2]
x = 15.3
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint1-1]
x = 15.3
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint2-1]
x = 5.7
y = -84
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint2-2]
x = 5.7
y = -184
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint2-3]
x = 5.7
y = -284
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropPoints2]
x = 5.7
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint2-3]
x = 5.7
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint2-2]
x = 5.7
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint2-1]
x = 5.7
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint3-1]
x = 7.2
y = -84
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
action =
[DropMidPoint3-2]
x = 7.2
y = -184
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint3-3]
x = 7.2
y = -284
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
[DropPoints3]
x = 7.2
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint3-3]
x = 7.2
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint3-2]
x = 7.2
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint3-1]
x = 7.2
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint4-1]
x = 7.3
y = -84
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint4-2]
x = 7.3
y = -184
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint4-3]
x = 7.3
y = -284
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropPoints4]
x = 7.3
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint4-3]
x = 7.3
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint4-2]
x = 7.3
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint4-1]
x = 7.3
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint5-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 = 4
[DropMidPoint5-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 = 4
[DropMidPoint5-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 = 4
[DropPoints5]
x = 15.3
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint5-3]
x = 15.3
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint5-2]
x = 15.3
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint5-1]
x = 15.3
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint6-1]
x = 5.7
y = -84
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint6-2]
x = 5.7
y = -184
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint6-3]
x = 5.7
y = -284
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropPoints6]
x = 5.7
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint6-3]
x = 5.7
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint6-2]
x = 5.7
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint6-1]
x = 5.7
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint7-1]
x = 7.2
y = -84
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
action =
[DropMidPoint7-2]
x = 7.2
y = -184
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint7-3]
x = 7.2
y = -284
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
[DropPoints7]
x = 7.2
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint7-3]
x = 7.2
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint7-2]
x = 7.2
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint7-1]
x = 7.2
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint8-1]
x = 7.3
y = -84
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint8-2]
x = 7.3
y = -184
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint8-3]
x = 7.3
y = -284
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropPoints8]
x = 7.3
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint8-3]
x = 7.3
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint8-2]
x = 7.3
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint8-1]
x = 7.3
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint9-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 = 4
[DropMidPoint9-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 = 4
[DropMidPoint9-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 = 4
[DropPoints9]
x = 15.3
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint9-3]
x = 15.3
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint9-2]
x = 15.3
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint9-1]
x = 15.3
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[DropMidPoint10-1]
x = 5.7
y = -84
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint10-2]
x = 5.7
y = -184
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint10-3]
x = 5.7
y = -284
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 7
linetype = 4
[DropPoints10]
x = 5.7
y = -384
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint10-3]
x = 5.7
y = -684
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint10-2]
x = 5.7
y = -584
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint10-1]
x = 5.7
y = -484
z = -91.0
u = 2.145
v = 0
w = 0
id = 2
order = 0
lineid = 1
status = 10
linetype = 4

508
CU/drop.ini
View File

@ -9,366 +9,456 @@ name = 35kg码垛路径
current_index = 0
[DropMidPoint1-1]
x = 15.3
y = -84
z = -91.0
u = 2.145
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropMidPoint1-2]
x = 15.3
y = -184
z = -91.0
u = 2.145
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropMidPoint1-3]
x = 15.3
y = -284
z = -91.0
u = 2.145
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropPoints1]
x = 15.3
y = -384
z = -91.0
u = 2.145
x = -92.53
y = -450
z = 118.729
u = -174.68
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 0
linetype = 4
[ResetPoint1-3]
x = 15.3
y = -684
z = -91.0
u = 2.145
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[ResetPoint1-2]
x = 15.3
y = -584
z = -91.0
u = 2.145
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[ResetPoint1-1]
x = 15.3
y = -484
z = -91.0
u = 2.145
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[DropMidPoint2-1]
x = 1379.783
y = -403.215
z = -200.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropMidPoint2-2]
x = 1379.783
y = -403.215
z = -250.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropMidPoint2-3]
x = 1379.783
y = -403.215
z = -300.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropPoints2]
x = 1379.783
y = -403.215
z = -334.279
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -20.149
y = -450
z = 105.28
u = -233.612
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 0
linetype = 4
[ResetPoint2-3]
x = 1379.783
y = -403.215
z = -366.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[ResetPoint2-2]
x = 1379.783
y = -403.215
z = -466.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[ResetPoint2-1]
x = 1379.783
y = -403.215
z = -566.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[DropMidPoint3-1]
x = 1379.783
y = -403.215
z = -500.0
u = 18.350
v = -0.985981
w = -125.710434
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
action =
linetype = 4
[DropMidPoint3-2]
x = 1379.783
y = -403.215
z = -400.0
u = 18.350
v = -0.985981
w = -125.710434
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropMidPoint3-3]
x = 1379.783
y = -403.215
z = -300.0
u = 18.350
v = -0.985981
w = -125.710434
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropPoints3]
x = 1379.783
y = -403.215
z = -334.279
u = 18.350
v = -0.985981
w = -125.710434
x = -61.092
y = -450
z = 123.624
u = -211.012
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 0
linetype = 4
[ResetPoint3-3]
x = 1379.783
y = -403.215
z = -200.0
u = 18.350
v = -0.985981
w = -125.710434
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[ResetPoint3-2]
x = 1379.783
y = -403.215
z = -100.0
u = 18.350
v = -0.985981
w = -125.710434
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[ResetPoint3-1]
x = 1379.783
y = -403.215
z = -50.0
u = 18.350
v = -0.985981
w = -125.710434
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[DropMidPoint4-1]
x = 1379.783
y = -403.215
z = -200.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropMidPoint4-2]
x = 1379.783
y = -403.215
z = -250.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropMidPoint4-3]
x = 1379.783
y = -403.215
z = -300.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 0
linetype = 4
[DropPoints4]
x = 1379.783
y = -403.215
z = -334.279
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -37.169
y = -450
z = 49.085
u = -70.397
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 0
linetype = 4
[ResetPoint4-3]
x = 1379.783
y = -403.215
z = -366.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[ResetPoint4-2]
x = 1379.783
y = -403.215
z = -466.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[ResetPoint4-1]
x = 1379.783
y = -403.215
z = -566.0
u = 18.350
v = -0.985981
w = -125.710434
id = 2
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 11
linetype = 0
status = 10
linetype = 4
[DropMidPoint5-1]
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint5-2]
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
[DropMidPoint5-3]
x = -21.648
y = -20
z = 98.003
u = -225.353
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 7
linetype = 4
[DropPoints5]
x = 2.48
y = -450
z = 33.293
u = -274.256
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint5-3]
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint5-2]
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4
[ResetPoint5-1]
x = -44.431
y = -189.009
z = -80.5
u = 51.386
v = 0
w = 0
id = 1
order = 0
lineid = 1
status = 10
linetype = 4

62
CU/drop.ini.bak
View File

@ -1,62 +0,0 @@
[DropLine1]
id = 1
name = 50kg码垛路径
current_index = 68
[DropLine2]
id = 2
name = 35kg码垛路径
current_index = 0
[DropPoints1]
x = 1067.078247
y = -919.529846
z = -189.15361
u = -97.082031
v = -0.985981
w = -125.710434
id = 3
order = 0
lineid = 1
status = 9
linetype = 0
[DropPoints2]
x = -187.124695
y = -1400.483765
z = -839.763611
u = -97.082031
v = -0.985981
w = -125.710434
id = 3
order = 1
lineid = 1
status = 9
linetype = 0
[DropPoints3]
x = 1069.078247
y = -919.529846
z = -500.15361
u = -97.082031
v = -0.985981
w = -125.710434
id = 3
order = 2
lineid = 1
status = 9
linetype = 0
[DropPoints4]
x = -187.124695
y = -1400.483765
z = -839.763611
u = -97.082031
v = -0.985981
w = -125.710434
id = 3
order = 3
lineid = 1
status = 9
linetype = 0

57
CU/drop.py
View File

@ -1,5 +1,4 @@
# File: drop_position_manager.py
from ast import mod
import configparser
import os
from typing import Optional
@ -19,11 +18,14 @@ class DropPositionManager:
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 get_next_drop_position(self, lineid: int, point: int) -> Optional[PositionModel]:
"""
获取指定 lineid 和 point 的下一个路径点。
@ -53,7 +55,8 @@ class DropPositionManager:
return None
def _load_point_path(self, lineid: int, point_id: int):
"""加载指定 lineid 和 point_id 的完整路径"""
"""加载指定 lineid 和 point_id 的完整路径
(点集合dropmidpoint、droppoint、resetpoint)"""
self._current_path = []
# 检查是否存在 DropPoints{point_id}
@ -99,31 +102,33 @@ class DropPositionManager:
# 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)
# model = PositionModel()
# model.init_position(pos)
# model.status = 7 # FMid
# model.lineType = 4 # WORLD
self._current_path.append(pos)
# 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)
# main_model = PositionModel()
# main_model.init_position(drop_pos)
# main_model.status = 9 # FDropBag
# main_model.lineType = 4
self._current_path.append(drop_pos)
# 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)
# model = PositionModel()
# model.init_position(pos)
# model.status = 10 # FReverse
# model.lineType = 4
self._current_path.append(pos)
print(f"✅ 已加载 DropLine{lineid} 中 DropPoints{point_id} 的路径，共 {len(self._current_path)} 个点")
def _read_position_from_section(self, section: str) -> Real_Position:
def _read_position_from_section(self, section: str) -> PositionModel:
"""从配置文件的 section 中读取位置信息"""
model = PositionModel()
pos = Real_Position()
pos.X = self.config.getfloat(section, "x")
pos.Y = self.config.getfloat(section, "y")
@ -131,7 +136,14 @@ class DropPositionManager:
pos.U = self.config.getfloat(section, "u")
pos.V = self.config.getfloat(section, "v")
pos.W = self.config.getfloat(section, "w")
return pos
model.init_position(pos)
model.lineType=self.config.getint(section, "linetype")
model.status=self.config.getint(section, "status")
return model
def _get_point_debug_info(manager, pos, model):
config = manager.config
@ -169,7 +181,8 @@ def _get_point_debug_info(manager, pos, model):
# 测试
if __name__ == "__main__":
manager = DropPositionManager("drop.ini")
# manager = DropPositionManager("drop.ini")
manager = DropPositionManager()
lineid = 1
print(f"\n🔁 测试：通过 point 参数切换路径集合\n")

8
CU/list.ini
View File

@ -1,5 +1,5 @@
[positions]
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
0 = -263.082245, -1305.589478, -1203.596436, -149.786407, 0.258, 0.258
1 = -263.082245, -1305.589478, -1203.596436, -149.786407, 0.258, 0.258
2 = -263.082245, -1305.589478, -1203.596436, -149.786407, 0.258, 0.258
3 = -263.082245, -1305.589478, -1203.596436, -149.786407, 0.258, 0.258