料袋末尾检测
This commit is contained in:
134
CU/Feeding.py
134
CU/Feeding.py
@ -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 模块进行状态交互来驱动投料动作
|
||||
|
||||
Reference in New Issue
Block a user