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-28 09:48:12 +08:00
parent 61e6d39dbf
commit 1916fb0bbc
62 changed files with 29808 additions and 21103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
CU/CsvToIni.py
View File

@ -1,58 +0,0 @@
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)

2
CU/Feed_save.py
View File

@ -268,7 +268,7 @@ class Feeding(QObject):
# --- 新增: 用于码垛模式的投料点索引 ---
self.current_drop_index = 0
self.drop_manager = DropPositionManager("CU/drop.ini")
self.drop_manager = DropPositionManager()
pass
def close_feed(self):

134
CU/Feeding.py
View File

@ -29,6 +29,7 @@ from Util.util_log import log
from Model.RobotModel import Instruction
from EMV.EMV import RelayController
from CU.drop import DropPositionManager
# from Mv3D.CameraImg import CameraImg
class ResetStatus(Enum):
RNone = 0
RStart = 1
@ -45,8 +46,8 @@ class FeedStatus(IntEnum):
FBroken1 = 6
FDropMid = 7 # 暂时替换为扔包中间点
FShake = 8
FDropBag = 9
FDropReset = 10
FDropBag = 9 #码垛点
FDropReset = 10 #码垛后点
FFinished = 11
FReverse = 12
FStartReverse = 13
@ -82,7 +83,7 @@ class FeedLine:
self.start2take_pos_index = 0
self.name = name
self.id = id
self.drop_manager = DropPositionManager("CU/drop.ini")
self.drop_manager = DropPositionManager()
# 初始化各个阶段的位置列表
self.feeding_to_end = []
@ -93,6 +94,8 @@ class FeedLine:
self.current_index = remain_count+1
#记录feed_positions当前扔包点索引
self.current_dropbag_index=0
#记录当前抓包点索引
self.current_take_index = 0
def get_current_index(self):
return self.current_index
@ -153,7 +156,7 @@ class FeedLine:
def set_take_position(self, position: Real_Position, dynamic_height=0):
"""
设置 FTake 位置，并更新其前后动态点的位置。
设置 FTake 位置（），并更新其前后动态点的位置。
:param position: 新的抓取位置
:param dynamic_height: 动态高度调整 (如果需要)
"""
@ -231,40 +234,16 @@ class FeedLine:
index_take = i
#开始插入动态扔包点，按照 动态扔包中间点，扔包点，动态复位点的顺序
#记录feed_positions扔包点初始位置,后续动态增加路径仍从此获取
#记录feed_positions扔包点初始位置,注意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
# 开始插入动态扔包中间点
# 开始插入动态扔包点
# 开始插入复位中间点
# 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]
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:
# 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
def set_feeding_to_end(self):
@ -272,11 +251,6 @@ class FeedLine:
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
@ -307,19 +281,18 @@ class FeedingConfig:
class Feeding(QObject):
need_origin_signal = Signal(str)
take_no_photo_sigal = Signal()
take_photo_sigal = Signal()
update_detect_image = Signal(np.ndarray)
log_signal = Signal(int,str)
#码垛完成通知
stack_finish_signal=Signal()
def __init__(self, robotClient: RobotClient):
def __init__(self, robotClient: RobotClient,relay_controller:RelayController):
super().__init__()
self.feedConfig = None
self.feedStatus = FeedStatus.FNone
self.robotClient = robotClient
# 添加 RelayController 实例
self.relay_controller = RelayController()
self.sensor_thread = None
# self.sensor_thread = None
self.detection_image = None
self.pause = False
self.cRis_photo = CRisOrFall()
@ -338,19 +311,25 @@ class Feeding(QObject):
self.catch = Catch(self.robotClient)
self.detect = Detect()
self.is_detected = True
self.detect_thread = threading.Thread(target=self.run_detect)
self.detect_thread.start()
self.detect_thread = threading.Thread(target=self.run_detect,name="run_detect")
# self.detect_thread.start()
self.onekey = False
self.debug_run_count = 0 # 初始化计数器
self.mid_take_count = 0
#记录抓取传感器状态通知
self.take_sensor_signal=False
#传感器判断抓包参数
self.sensor2_ready = False # 传感器2是否检测到料包
self.motor_stopped_by_sensor2 = False # 是否由传感器2触发停止电机
self.sensor_thread = None
self.relay_controller = RelayController()
# self.sensor2_ready = False # 传感器2是否检测到料包
# self.motor_stopped_by_sensor2 = False # 是否由传感器2触发停止电机
# self.sensor_thread = None
# self.relay_controller = RelayController()
#用于同步控制EMV相关
self.relay_controller = relay_controller
self.relay_controller.take_robot_signal.connect(self.take_feed_notice)
# self.camera_img=CameraImg()
# 启动传感器2线程
self.relay_controller._running = True
self.sensor2_thread = None
# self.relay_controller._running = True
# self.sensor2_thread = None
# --- 新增: 用于码垛模式的投料点索引 ---
self.current_drop_index = 1
@ -358,15 +337,22 @@ class Feeding(QObject):
def close_feed(self):
self.is_detected = False
self.detect_thread.join()
# self.detect_thread.join()
if self.detect.detection:
self.detect.detection.release()
# if self.camera_img:
# self.camera_img.close_camera()
def run_detect(self):
#图片相关线程
while self.is_detected:
self.detect.run()
time.sleep(0.02)
def take_feed_notice(self):
"""接收机器人信号通知"""
self.take_sensor_signal = True
def run(self):
self.catch.run()
# 获取事件坐标
@ -432,9 +418,8 @@ class Feeding(QObject):
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()
# self.relay_controller.open(conveyor2=True)#开电机
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)
@ -449,6 +434,7 @@ class Feeding(QObject):
self.feedConfig.feedLine.get_position_list()
self.detect.detect_status = DetectStatus.DNone
self.feed_Mid_Status = FeedMidStatus.FMid_Start
self.next_position(self.is_reverse)
elif self.feedStatus == FeedStatus.FMid:
@ -487,25 +473,38 @@ class Feeding(QObject):
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)
if(self.feed_Mid_Status != FeedMidStatus.FMid_Feed):
self.feed_Mid_Status = FeedMidStatus.FMid_Feed
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_success)
self.next_position()
return
#初始点无论如何先打开夹爪
self.relay_controller.close(clamp=True)
#重新抓去信号料带
self.take_sensor_signal=False
self.relay_controller.sensor2_ready=True
#去除list.ini读取抓取点20250915
#self.feedConfig.feedLine.set_take_position(self.detect.detect_position, 0)
#self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置
'''real_position'''
# 一直等待传感器2信号，永不超时
# TODO:逻辑需改变，不能用while循环
if Constant.DebugPosition:
self.take_sensor_signal=True
while True:
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检测到料包到位，开始执行抓取")
# sensors = self.relay_controller.get_all_device_status('sensors')
# sensor2_value = sensors.get(self.relay_controller.SENSOR2, False)
if self.take_sensor_signal:
self.log_signal.emit(logging.INFO, "传感器2检测到料包到位开始执行抓取")
break # ✅ 条件满足，跳出循环，继续执行下面的代码
else:
self.log_signal.emit(logging.INFO, "⏳ 等待传感器2料包信号...")
if self.feedStatus == FeedStatus.FNone:
return
time.sleep(1) # 每秒检查一次
#第二次执行FeedStatus.FPhoto时，改变码垛点
# self.camera_img.save_frame_path()
self.feedConfig.feedLine.set_feeding_to_end()
# self.take_photo_sigal.emit()
self.next_position()
self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
# self.feedStatus = FeedStatus.FTake
@ -537,8 +536,7 @@ class Feeding(QObject):
self.relay_controller.open(clamp=True)
self.next_position(self.is_reverse)
#self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
@ -584,11 +582,7 @@ class Feeding(QObject):
# 调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取
# 由 feeding2end_pos_index 指向的点。
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)
# 2. 记录日志：已到达投料点
self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
# 3. 与 Catch 模块进行状态交互来驱动投料动作

860
CU/Feeding.py.bak
View File

@ -1,860 +0,0 @@
import copy
import logging
import random
import threading
import time
import cv2
import numpy as np
from PyQt5.QtWidgets import QMessageBox
from PySide6.QtCore import Signal, QObject
from PySide6.QtGui import QPixmap
import Constant
import Expection
from CU.Catch import Catch, CatchStatus
from CU.Detect import Detect, DetectStatus
from Model.FeedModel import PositionModel
from Model.Position import Real_Position, Detection_Position
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 Util.util_math import get_distance
from Util.util_time import CRisOrFall
#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
class FeedStatus(IntEnum):
FNone = 0
FStart = 1
FCheck = 2
FMid = 3
FPhoto = 4
FTake = 5
FBroken1 = 6
FBroken2 =7
FShake = 8
FDropBag = 9
FFinished = 10
FReverse = 11
FStartReverse = 12
class LineType(Enum):
Straight = 0
CureMid = 2
CureEnd = 3
WORLD = 4
class FeedMidStatus(Enum):
FMid_Start = 1
FMid_Take = 2
FMid_Feed= 3
class FeedPosition:
def __init__(self,status:FeedStatus,position:Real_Position):
self.status = status
self.position = position
class FeedLine:
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
self.start2take_pos_index = 0
self.name = name
self.id = id
# --- 新增：用于存储从 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]
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
if self.feeding2end_pos_index < 0:
self.feeding2end_pos_index = len(self.feeding_to_end) - 1
else:
self.feeding2end_pos_index += 1
if self.feeding2end_pos_index >= len(self.feeding_to_end):
self.feeding2end_pos_index = 0
return pos
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
if self.origin2start_pos_index < 0:
self.origin2start_pos_index = len(self.origin_to_start) - 1
else:
self.origin2start_pos_index += 1
if self.origin2start_pos_index >= len(self.origin_to_start):
self.origin2start_pos_index = 0
return pos
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
if self.start2take_pos_index < 0:
self.start2take_pos_index = len(self.start_to_take) - 1
else:
self.start2take_pos_index += 1
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:
return self.feeding_to_end[i]
def set_take_position(self, position: Real_Position, dynamic_height=0):
"""
设置 FTake 位置，并更新其前后动态点的位置。
:param position: 新的抓取位置
:param dynamic_height: 动态高度调整 (如果需要)
"""
for i in range(len(self.feeding_to_end)):
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)
# 创建 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)
# 安全检查索引
if i > 0:
self.feeding_to_end[i - 1].set_position(befor_take_position)
else:
print("Warning: No position before FTake to update.")
self.feeding_to_end[i].set_position(position)
if i + 1 < len(self.feeding_to_end):
self.feeding_to_end[i + 1].set_position(after_take_position)
else:
print("Warning: No position after FTake to update.")
break # 抓料点暂时就一个
def set_drop_position(self, position: Real_Position):
"""
设置 FDropBag 位置，只设置当前点，不处理前后点。
:param position: 新的丢包位置
"""
for i in range(len(self.feeding_to_end)):
if self.feeding_to_end[i].status == FeedStatus.FDropBag.value:
# 直接设置当前点的位置
self.feeding_to_end[i].set_position(position)
print(
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})")
break # 假设只有一个丢包点
def get_position_list(self):
index_start = -1
for i in range(len(self.feed_positions)):
if self.feed_positions[i].status == FeedStatus.FCheck.value:
index_start = i
break
for i in range(len(self.feed_positions)):
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.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)
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):
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])
return position_photo
def get_line_info(self):
pass
class Feeding(QObject):
need_origin_signal = Signal(str)
take_no_photo_sigal = Signal()
update_detect_image = Signal(np.ndarray)
log_signal = Signal(int,str)
def __init__(self, robotClient: RobotClient):
super().__init__()
self.feedConfig = None
self.feedStatus = FeedStatus.FNone
self.robotClient = robotClient
# 添加 RelayController 实例
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()
self.feed_Mid_Status = FeedMidStatus.FMid_Start
self.is_reverse = False
# 复位集合
self.run_reverse = False
self.take_no_photo = False
self.reset_status = ResetStatus.RNone
self.reversed_positions = []
self.current_position = None
self.index=1
self.pos_index = -1
self.pos_near_index = -1
self.catch = Catch(self.robotClient)
self.detect = Detect()
self.is_detected = True
self.detect_thread = threading.Thread(target=self.run_detect)
self.detect_thread.start()
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
self.relay_controller = RelayController()
# 启动传感器2线程
self.relay_controller._running = True
self.sensor2_thread = None
# --- 新增: 用于码垛模式的投料点索引 ---
self.current_drop_index = 0
self.drop_manager = DropPositionManager("CU/drop.ini")
pass
def close_feed(self):
self.is_detected = False
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()
time.sleep(0.02)
def run(self):
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,
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,
# 0,
# 0,
# 0);
# img_path = f'Image/{self.index}.png'
# img = cv2.imread(img_path)
# self.index += 1
# self.index = self.index % 4
# self.detection_image = img
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):
self.feedStatus = FeedStatus.FNone
self.is_reverse = False
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
if self.feedStatus == FeedStatus.FNone or self.pause:
return
elif self.feedStatus == FeedStatus.FCheck:
self.log_signal.emit(logging.INFO, Constant.str_feed_check)
# 1, 检查是否是三列
# 2, 检查是否有人
# if self.safe_check_columns() and self.safe_check_person():
# pass
# else:
# if self.feedConfig.num != 0:
# self.next_target()
# if == 原点 继续判断
# else:
# QMessageBox.information(None, "提示", Constant.str_feed_sae_error_msgbox)
if self.is_reverse:
self.feed_Mid_Status = FeedMidStatus.FMid_Start
else:
self.feed_Mid_Status = FeedMidStatus.FMid_Take
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:
# 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)
self.feedStatus = FeedStatus.FNone
return
if self.is_reverse:
self.feedStatus = FeedStatus.FNone
self.is_reverse = False
return
self.feedConfig.feedLine.get_position_list()
self.detect.detect_status = DetectStatus.DNone
self.feed_Mid_Status = FeedMidStatus.FMid_Start
self.next_position(self.is_reverse)
elif self.feedStatus == FeedStatus.FMid:
feed_pos = self.get_current_position(self.is_reverse)
if feed_pos.get_position().compare(real_position):
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
self.next_position(self.is_reverse)
# 增加计数器逻辑
self.mid_take_count += 1
# 可选：在 Debug1 模式下输出日志
if Constant.Debug1:
self.log_signal.emit(
logging.INFO,
f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次"
)
#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.next_position(self.is_reverse)
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
self.init_detection_image()
return
if not Constant.Debug:
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'''
# 一直等待传感器2信号，永不超时
while True:
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.relay_controller.close(conveyor2=True)
print("---------传感器2检测到料包到位，开始执行抓取,------------")
break # ✅ 条件满足，跳出循环，继续执行下面的代码
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:
self.log_signal.emit(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.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, "🟡 机器人尚未到达抓料点位")
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
# 移动到下一个抓取点
# 更新丢包点: 如果需要根据放置情况调整下次抓取
next_drop_pos = self.drop_manager.get_next_drop_position(lineid=1)
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.relay_controller.open(conveyor2=True)
print(f"--------已设置放置点: X={next_drop_pos.X:.2f}")
else:
self.log_signal.emit(logging.ERROR, "获取放置点失败")
print("--------获取放置点失败")
return
#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)
elif self.feedStatus == FeedStatus.FBroken1:
if self.get_current_position().get_position().compare(real_position):
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):
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
self.next_position()
elif self.feedStatus == FeedStatus.FShake:
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:
self.catch.catch_status = CatchStatus.CShake
return
if self.catch.catch_status == CatchStatus.CShake:
# if self.feedConfig.feedLine.feeding_to_end[
# self.feedConfig.feedLine.feeding2end_pos_index + 1].status != FeedStatus.FShake:
# self.catch.catch_status = CatchStatus.COk
# else:
self.catch.shake_continue.SetReset()
self.next_position()
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):
# 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): 触发投料动作
if self.catch.catch_status == CatchStatus.CNone:
# 将 Catch 状态设置为 CDrop，通知 Catch 模块开始执行物理投料动作
self.catch.catch_status = CatchStatus.CDrop
# 立即返回，等待 Catch 模块处理
return
# b. 投料进行中 (CDrop): 等待完成
if self.catch.catch_status == CatchStatus.CDrop:
# 什么都不做，等待 Catch 模块完成动作并更新状态
return
# c. 投料完成 (COk): 处理后续逻辑并移动到下一个点
if self.catch.catch_status == CatchStatus.COk:
# 重置 Catch 状态，为下一次操作做准备
self.catch.catch_status = CatchStatus.CNone
# (后续增加) 延时: 让物料稳定
# time.sleep(self.robotClient.time_delay_put)
# (后续增加) 视觉确认: 拍照确认袋子已放置
# 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}')
# 5. *** 关键步骤 ***: 移动到路径中的下一个点
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
# 调用 next_Feed()。
self.next_position()
def run_reset(self):
"""
复位流程主函数：让机器人沿喂料路径反向移动，回到起始安全位置。
该函数通常在设备启动、错误恢复或手动干预后调用。
"""
# 1. 获取机器人当前的实际物理位置
# 从 robotClient 的状态模型中读取6个关节的世界坐标（world_0 ~ world_5）
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)
# 2. 状态检查：如果复位状态为 RNone（无任务），直接返回，不执行任何操作
if self.reset_status == ResetStatus.RNone:
return
# 3. 状态一：RStart - 复位启动阶段：确定当前位置在喂料路径中的索引
if self.reset_status == ResetStatus.RStart:
# a. 安全检查：确保 feedConfig 配置已加载
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
# b. 初始化关键变量
self.pos_index = -1 # 用于存储与当前位置精确匹配的路径点索引
self.pos_near_index = -1 # 用于存储距离当前位置最近的路径点索引
self.reversed_positions = [] # 存储从当前位置到起始点的反向路径点列表
# c. 第一步：尝试在 feed_positions 路径点中寻找与当前实际位置完全匹配的点
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
if pos_model.get_position().compare(real_position,is_action=True):
self.pos_index = index
break
# d. 如果没有找到精确匹配的点，则寻找距离最近的点
if self.pos_index == -1:
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)
self.pos_near_index = index
# 根据最近点构建回退路径（从起点到最近点）
if 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]
# e. 关键步骤：反转路径，使其成为“从当前位置退回起点”的顺序
self.reversed_positions = list(reversed(self.reversed_positions))
self.reverse_index = 0
# f. 触发复位警报音，提示用户设备正在复位
self.send_emergency_sound()
# g. 记录当前实际位置作为移动的初始参考点
self.current_position = PositionModel()
self.current_position.init_position(real_position)
# h. 更新状态机：进入“正在复位”阶段
self.reset_status = ResetStatus.RRunging
# 4. 状态二：RRunging - 正在复位中：逐步移动到反向路径的每一个点
if self.reset_status == ResetStatus.RRunging:
# a. 安全检查：只有当机器人到达上一个目标点后，才允许执行下一步
# compare 判断当前实际位置是否与目标位置（current_position）一致
if not real_position.compare(self.current_position.get_position(),is_action=True):
return# 未到达目标，等待下一次调用
# b. 获取当前要移动到的反向路径点
pos_model = self.reversed_positions[self.reverse_index]
# c. 特殊处理：跳过“取袋”状态的节点（FTake）
# 在回退过程中，不需要在取袋点停留或执行动作
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
pos_model = self.reversed_positions[self.reverse_index + 1]
self.reverse_index = self.reverse_index+1
# d. 根据路径点类型执行不同的移动指令
if pos_model.lineType == LineType.CureMid.value:
#如果是圆弧中间点（CureMid），需要同时指定当前点和下一个点来规划圆弧
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.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.current_position = pos_model
self.reverse_index = self.reverse_index + 1
# e. 检查是否已完成所有反向路径点的移动
if self.reverse_index == len(self.reversed_positions):
self.reset_status = ResetStatus.ROk
# 5. 状态三：ROk - 复位完成
if self.reset_status == ResetStatus.ROk:
# a. 重置状态为 RNone，表示复位任务结束
self.reset_status = ResetStatus.RNone
# b.发送紧急停止信号（用于关闭警报音后续可扩展复位其他外设）
self.send_emergency_stop()
def return_original_position(self):
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
start_index = -1
# for index in range(len(self.feedConfig.feedLine.positions)):
# if self.feedConfig.feedLine.positions[index].status == 2:
# start_index = index
pos_index = -1
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):
pos_index = index
break
if pos_index == -1:
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)
pos_near_index = index
if pos_near_index != -1:
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_near_index+1]
else:
return False
else:
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_index]
reversed_positions = list(reversed(reversed_positions))
self.reverse_index = 0
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()):
continue
#todo 缺少比对
pos_model = reversed_positions[self.reverse_index]
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
pos_model = reversed_positions[self.reverse_index + 1]
# TODO take节点判断
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)
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)
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)
def send_emergency_stop(self):
self.sendIOControl(Constant.IO_EmergencyPoint, 0)
def sendIOControl(self, IO_bit, IO_Status: int):
IO_command = CMDInstructRequest()
io_instruction = Instruction()
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.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}')
pass
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):
position_instruction = Instruction()
position_instruction.speed = speed
position_instruction.m0 = real_position.X
position_instruction.m1 = real_position.Y
position_instruction.m2 = real_position.Z
position_instruction.m3 = real_position.U
position_instruction.m4 = real_position.V
position_instruction.m5 = real_position.W
position_instruction.smooth = self.robotClient.smooth
position_instruction.action = move_type.value
if position_instruction.action == 17:
position_instruction.m0_p = real_position1.X
position_instruction.m1_p = real_position1.Y
position_instruction.m2_p = real_position1.Z
position_instruction.m3_p = real_position1.U
position_instruction.m4_p = real_position1.V
position_instruction.m5_p = real_position1.W
instruction_command = CMDInstructRequest()
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}'
try:
print('fuck1',log_str)
self.log_signal.emit(logging.INFO, 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()
# _, img, xyz, uvw, points = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,save_img_point=0, Height_reduce=30, width_reduce=30)
# self.detection_image = img.copy()
# if xyz ==None or uvw==None or points==None:
# return None
# target_position,noraml_base = getPosition(*xyz,*uvw,None,points)
#
# position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
# position.Z = position.Z+200
# return position
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
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())
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)
else:
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed)
pass
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
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)
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)
else:
self.sendTargPosition(real_position=take_pos.get_position(),speed=self.robotClient.feed_speed)
pass
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
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)
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)
else:
# if not reverse and self.feedStatus == FeedStatus.FShake:
# if not reverse:
# if self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, True):
# pass
# elif self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, False):
# self.feedStatus = FeedStatus.FShake
# 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)
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):
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
self.next_start(reverse)
if Constant.Debug1:
print('next_start')
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
self.next_take(reverse)
if Constant.Debug1:
print('next_take')
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
self.next_Feed(reverse)
if Constant.Debug1:
print('next_feed')
def safe_check_columns(self):
return True
pass
def safe_check_person(self):
return True
pass

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
View File

@ -1,922 +0,0 @@
[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

464
CU/drop.ini
View File

@ -1,464 +0,0 @@
[DropLine1]
id = 1
name = 50kg码垛路径
current_index = 22
[DropLine2]
id = 2
name = 35kg码垛路径
current_index = 0
[DropMidPoint1-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
[DropMidPoint1-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
[DropMidPoint1-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
[DropPoints1]
x = -92.53
y = -450
z = 118.729
u = -174.68
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint1-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
[ResetPoint1-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
[ResetPoint1-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
[DropMidPoint2-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
[DropMidPoint2-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
[DropMidPoint2-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
[DropPoints2]
x = -20.149
y = -450
z = 105.28
u = -233.612
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint2-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
[ResetPoint2-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
[ResetPoint2-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
[DropMidPoint3-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
[DropMidPoint3-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
[DropMidPoint3-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
[DropPoints3]
x = -61.092
y = -450
z = 123.624
u = -211.012
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint3-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
[ResetPoint3-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
[ResetPoint3-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
[DropMidPoint4-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
[DropMidPoint4-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
[DropMidPoint4-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
[DropPoints4]
x = -37.169
y = -450
z = 49.085
u = -70.397
v = 0
w = 0
id = 1
order = 1
lineid = 1
status = 9
linetype = 4
[ResetPoint4-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
[ResetPoint4-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
[ResetPoint4-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
[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

223
CU/drop.py
View File

@ -3,11 +3,12 @@ import configparser
import os
from typing import Optional
from Model.Position import Real_Position
from Model.FeedModel import PositionModel
from Model.FeedModel import LineModel, PositionModel
import Constant
class DropPositionManager:
def __init__(self, config_path="CU/drop.ini"):
def __init__(self, config_path=Constant.dropLine_set_file):
self.config_path = config_path
self.config = configparser.ConfigParser()
self._load_config()
@ -17,8 +18,7 @@ class DropPositionManager:
self._current_point_id: Optional[int] = None
self._current_path: list = [] # 当前路径点列表
self._current_index: int = 0 # 当前路径中的索引
self._current_drop_section:dict = {} # 使用字典存储每个main_section对应的最大drop-section
def _load_config(self):
"""加载配置文件"""
@ -77,24 +77,31 @@ class DropPositionManager:
# 1. 加载 DropMidPoint{point_id}-*（按 level 升序）
mid_points = []
# _mid_section_start=f"DropMidPoint{point_id}-"
_mid_section_start="DropMidPoint1-"
for sec in self.config.sections():
if sec.startswith(f"DropMidPoint{point_id}-") and self.config.getint(sec, "lineid") == lineid:
if sec.startswith(_mid_section_start) and self.config.getint(sec, "lineid") == lineid:
try:
level = int(sec.split('-')[1])
pos = self._read_position_from_section(sec)
mid_points.append((level, pos))
break
except Exception as e:
print(f"❌ 解析 {sec} 失败: {e}")
mid_points.sort(key=lambda x: x[0])
# 2. 加载 ResetPoint{point_id}-*（按 level 升序）
reset_points = []
# _reset_section_start=f"ResetPoint{point_id}-"
_reset_section_start="ResetPoint1-"
for sec in self.config.sections():
if sec.startswith(f"ResetPoint{point_id}-") and self.config.getint(sec, "lineid") == lineid:
if sec.startswith(_reset_section_start) 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))
break
except Exception as e:
print(f"❌ 解析 {sec} 失败: {e}")
reset_points.sort(key=lambda x: x[0])
@ -140,10 +147,167 @@ class DropPositionManager:
model.lineType=self.config.getint(section, "linetype")
model.status=self.config.getint(section, "status")
model.id=self.config.getint(section, "id")
model.order=self.config.getint(section, "order")
model.lineId=self.config.getint(section, "lineid")
# 保存section名称，用于排序
model.section = section
return model
def _read_position_return_leveL(self,config_reader,section,lineid)->Optional[tuple]:
try:
if config_reader.getint(section, "lineid") == lineid:
# 提取 - 后的数字
level = int(section.split('-')[1])
position_model = self._read_position_from_section(section)
return (level, position_model)
except Exception as e:
print(f"{__name__}:_read_position_return_leveL:{e}")
return None
def load_path_points(self,lineid: int) ->Optional[LineModel]:
"""根据lineid加载所有码垛的路径信息"""
#默认码垛的lineid从10开始
_lineid=lineid+10
line_model = LineModel(_lineid)
line_model.line_category = 2
line_model.id = _lineid
# 查找主表 DropLineX
main_section = f"{Constant.dropLine_set_section}{lineid}"
if self.config.has_section(main_section):
line_model.name = self.config.get(main_section, "name", fallback=f"路径{lineid}")
else:
return None
# 先收集所有 DropPoints 开头的 section
drop_points_sections = []
for sec in self.config.sections():
if sec.startswith("DropPoints"):
try:
if self.config.getint(sec, "lineid") == lineid:
# 提取数字部分
num_part = sec.replace("DropPoints", "")
if num_part.isdigit():
drop_points_sections.append((sec, int(num_part)))
except Exception as e:
print(f"{__name__}:_load_path_points异常1:{e}")
continue
# 按数字排序 DropPoints sections
drop_points_sections.sort(key=lambda x: x[1])
# 使用main_section作为键存储当前最大的drop-section编号
self._current_drop_section[main_section] = drop_points_sections[-1][1] if drop_points_sections else 1
mid_points_other=[] #最后点没有匹配的droppoints
# 遍历每个 DropPoints，按照_load_point_path的逻辑加载对应的中间点和复位点
for sec, point_num in drop_points_sections:
try:
# 1. 加载 DropMidPoint{point_num}-*（按 level 升序）
mid_points = []
for s in self.config.sections():
if s.startswith(f"DropMidPoint{point_num}-"):
_mid=self._read_position_return_leveL(self.config,s,lineid)
if _mid:
mid_points.append(_mid)
if point_num==(len(drop_points_sections)):
if s.startswith(f"DropMidPoint{point_num+1}-"):
_mid=self._read_position_return_leveL(self.config,s,lineid)
if _mid:
mid_points_other.append(_mid)
# 按level升序排序
mid_points.sort(key=lambda x: x[0])
# 添加中间点到路径
for _, pos in mid_points:
line_model.positions.append(pos)
# 加载 ResetPoint{point_num}-*（按 level 升序）
reset_points = []
for s in self.config.sections():
if s.startswith(f"ResetPoint{point_num}-"):
_reset=self._read_position_return_leveL(self.config,s,lineid)
if _reset:
reset_points.append(_reset)
# 按level升序排序
reset_points.sort(key=lambda x: x[0])
# 添加复位点到路径
for _, pos in reset_points:
line_model.positions.append(pos)
#添加当前 DropPoints
try:
position_model = self._read_position_from_section(sec)
line_model.positions.append(position_model)
except Exception as e:
print(f"{__name__}:_load_path_points异常3:{e}")
except Exception as e:
print(f"{__name__}:_load_path_points异常4:{e}")
if mid_points_other:
mid_points_other.sort(key=lambda x: x[0])
for _, pos in mid_points_other:
line_model.positions.append(pos)
return line_model
def save_path_points(self, line_model: LineModel):
"""根据lineid保存所有码垛的路径信息"""
#默认码垛的lineid从10开始,保存的时候减一
_lineid=line_model.id-10
if _lineid<=0:
return
self.config.read(Constant.dropLine_set_file, encoding='utf-8')
# 查找主表 DropLineX
main_section = f"{Constant.dropLine_set_section}{_lineid}"
if not self.config.has_section(main_section):
self.config.add_section(main_section)
self.config.set(main_section, "name", line_model.name)
self.config.set(main_section, "id", str(_lineid))
_current_reset_index=1
_current_mid_index=1
_current_drop_section_val=self._current_drop_section[main_section]
# 保存每个DropPoints
for i, pos in enumerate(line_model.positions):
if pos.lineId == _lineid or pos.lineId == line_model.id:
#最后一个扔包点
if pos.section.startswith(f"DropMidPoint{_current_drop_section_val+1}"):
_current_mid_index=int(pos.section.split('-')[-1])+1
if pos.section.startswith(f"ResetPoint{_current_drop_section_val}"):
_current_reset_index=int(pos.section.split('-')[-1])+1
#新增的数据，如果是前点，需要获取后点的数据
if pos.section.startswith("Position"):
pos.lineId = _lineid
if pos.status is None:
continue
# FDropMid = 7
elif pos.status==7:
#只有一个
pos.section = f"DropMidPoint{_current_drop_section_val+1}-{_current_mid_index}"
_current_mid_index+=1
elif pos.status==9:
pos.section = f"DropPoints{_current_drop_section_val+1}"
_current_drop_section_val+=1
_current_mid_index=1
_current_reset_index=1
elif pos.status==10:
pos.section = f"ResetPoint{_current_drop_section_val}-{_current_reset_index}"
_current_reset_index+=1
#保存数据
pos.save_position_model(self.config)
with open(Constant.dropLine_set_file, 'w', encoding='utf-8') as f:
self.config.write(f)
def del_drop_point(self,section):
self.config.read(Constant.dropLine_set_file, encoding = 'utf-8')
self.config.remove_section(section)
with open(Constant.dropLine_set_file, 'w', encoding='utf-8') as f:
self.config.write(f)
def _get_point_debug_info(manager, pos, model):
config = manager.config
@ -184,47 +348,4 @@ if __name__ == "__main__":
# manager = DropPositionManager("drop.ini")
manager = DropPositionManager()
lineid = 1
print(f"\n🔁 测试：通过 point 参数切换路径集合\n")
# 先走 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}")
manager.load_path_points(1)

8
CU/list.ini
View File

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