yanganjie/ailai_show
1
0
Fork 0
forked from huangxin/ailai
Code Pull Requests Activity

Compare commits

base: yanganjie:e7d6cb81fa0a0cfa0a2f1be7e4d0f6efa89bb7f5
Branches Tags
yanganjie:main huangxin:main
..
compare: yanganjie:main
Branches Tags
yanganjie:main huangxin:main

4 Commits
e7d6cb81fa ... main

Author SHA1 Message Date
yanganjie
0f94c88fc5 程序正常退出（界面关闭后（投料启动前后），实现所有线程有序退出） 2025-08-19 20:03:20 +08:00
yanganjie
c3e76a79b0 显示 "已投袋数" 2025-08-18 15:11:34 +08:00
huangxin
645ca8a4f7 Merge pull request '显示已投袋数' (#1) from yanganjie/ailai_show:main into main 
Reviewed-on: huangxin/ailai#1
 2025-08-18 14:29:49 +08:00
yanganjie
3c11bb6b2b 保留三个按钮（启动、停止、急停） 和 显示已投袋数 2025-08-18 10:16:44 +08:00
4 changed files with 1588 additions and 811 deletions
Expand all files Collapse all files

514
CU/Feeding.py
View File

@ -20,19 +20,23 @@ from enum import Enum, IntEnum
from COM.COM_Robot import RobotClient, DetectType from COM.COM_Robot import RobotClient, DetectType
from Model.RobotModel import CMDInstructRequest, MoveType from Model.RobotModel import CMDInstructRequest, MoveType
from Trace.handeye_calibration import getPosition from Trace.handeye_calibration import getPosition
from Trace.handeye_calibration import getxyz,getxyz1 from Trace.handeye_calibration import getxyz, getxyz1
from Util.util_math import get_distance from Util.util_math import get_distance
from Util.util_time import CRisOrFall from Util.util_time import CRisOrFall
#from Vision.camera_coordinate_dete import Detection
# from Vision.camera_coordinate_dete import Detection
from Util.util_log import log from Util.util_log import log
from Model.RobotModel import Instruction from Model.RobotModel import Instruction
from EMV.EMV import RelayController from EMV.EMV import RelayController
from CU.drop import DropPositionManager from CU.drop import DropPositionManager
class ResetStatus(Enum): class ResetStatus(Enum):
RNone = 0 RNone = 0
RStart = 1 RStart = 1
RRunging = 2 RRunging = 2
ROk =3 ROk = 3
class FeedStatus(IntEnum): class FeedStatus(IntEnum):
FNone = 0 FNone = 0
@ -42,13 +46,14 @@ class FeedStatus(IntEnum):
FPhoto = 4 FPhoto = 4
FTake = 5 FTake = 5
FBroken1 = 6 FBroken1 = 6
FBroken2 =7 FBroken2 = 7
FShake = 8 FShake = 8
FDropBag = 9 FDropBag = 9
FFinished = 10 FFinished = 10
FReverse = 11 FReverse = 11
FStartReverse = 12 FStartReverse = 12
class LineType(Enum): class LineType(Enum):
Straight = 0 Straight = 0
CureMid = 2 CureMid = 2
@ -59,15 +64,17 @@ class LineType(Enum):
class FeedMidStatus(Enum): class FeedMidStatus(Enum):
FMid_Start = 1 FMid_Start = 1
FMid_Take = 2 FMid_Take = 2
FMid_Feed= 3 FMid_Feed = 3
class FeedPosition: class FeedPosition:
def __init__(self,status:FeedStatus,position:Real_Position): def __init__(self, status: FeedStatus, position: Real_Position):
self.status = status self.status = status
self.position = position self.position = position
class FeedLine: class FeedLine:
def __init__(self, id, name, feed_positions:list): def __init__(self, id, name, feed_positions: list):
self.feed_positions = copy.deepcopy(feed_positions) self.feed_positions = copy.deepcopy(feed_positions)
self.feeding2end_pos_index = 0 self.feeding2end_pos_index = 0
self.origin2start_pos_index = 0 self.origin2start_pos_index = 0
@ -79,17 +86,37 @@ class FeedLine:
# 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...] # 这个列表将在加载 ini 时填充 [[x1,y1,z1,u1,v1,w1], [x2,y2,z2,u2,v2,w2], ...]
self.drop_point_list = [] self.drop_point_list = []
def get_current_feed_position(self,is_reverse): 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] pos = self.feeding_to_end[
return pos (
def get_current_take_position(self,is_reverse): (self.feeding2end_pos_index + 1) % len(self.feeding_to_end)
pos = self.start_to_take[ (self.start2take_pos_index+1)%len(self.start_to_take) if is_reverse else self.start2take_pos_index-1] if is_reverse
return pos else self.feeding2end_pos_index - 1
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 return pos
def get_next_feed_position(self,reverse:bool=False): 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] pos = self.feeding_to_end[self.feeding2end_pos_index]
if reverse: if reverse:
self.feeding2end_pos_index -= 1 self.feeding2end_pos_index -= 1
@ -101,7 +128,7 @@ class FeedLine:
self.feeding2end_pos_index = 0 self.feeding2end_pos_index = 0
return pos return pos
def get_next_start_position(self,reverse:bool=False): def get_next_start_position(self, reverse: bool = False):
pos = self.origin_to_start[self.origin2start_pos_index] pos = self.origin_to_start[self.origin2start_pos_index]
if reverse: if reverse:
self.origin2start_pos_index -= 1 self.origin2start_pos_index -= 1
@ -114,7 +141,7 @@ class FeedLine:
return pos return pos
def get_next_take_position(self,reverse:bool=False): def get_next_take_position(self, reverse: bool = False):
pos = self.start_to_take[self.start2take_pos_index] pos = self.start_to_take[self.start2take_pos_index]
if reverse: if reverse:
self.start2take_pos_index -= 1 self.start2take_pos_index -= 1
@ -125,6 +152,7 @@ class FeedLine:
if self.start2take_pos_index >= len(self.start_to_take): if self.start2take_pos_index >= len(self.start_to_take):
self.start2take_pos_index = 0 self.start2take_pos_index = 0
return pos return pos
def get_take_position(self): def get_take_position(self):
for i in range(len(self.feeding_to_end)): for i in range(len(self.feeding_to_end)):
if self.feeding_to_end[i].status == FeedStatus.FTake.value: if self.feeding_to_end[i].status == FeedStatus.FTake.value:
@ -140,20 +168,21 @@ class FeedLine:
if self.feeding_to_end[i].status == FeedStatus.FTake.value: if self.feeding_to_end[i].status == FeedStatus.FTake.value:
# 计算 XYZ 坐标 # 计算 XYZ 坐标
# xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c) # 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) xyz = getxyz1(
position.X,
position.Y,
position.Z,
position.a,
position.b,
position.c,
)
# 创建 before 和 after 位置 # 创建 before 和 after 位置
befor_take_position = Real_Position().init_position(xyz[0], befor_take_position = Real_Position().init_position(
xyz[1], xyz[0], xyz[1], xyz[2], position.U, position.V, position.W
xyz[2], )
position.U, after_take_position = Real_Position().init_position(
position.V, xyz[0], xyz[1], xyz[2], position.U, position.V, position.W
position.W) )
after_take_position = Real_Position().init_position(xyz[0],
xyz[1],
xyz[2],
position.U,
position.V,
position.W)
# 安全检查索引 # 安全检查索引
if i > 0: if i > 0:
self.feeding_to_end[i - 1].set_position(befor_take_position) self.feeding_to_end[i - 1].set_position(befor_take_position)
@ -176,7 +205,8 @@ class FeedLine:
# 直接设置当前点的位置 # 直接设置当前点的位置
self.feeding_to_end[i].set_position(position) self.feeding_to_end[i].set_position(position)
print( print(
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})") f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})"
)
break # 假设只有一个丢包点 break # 假设只有一个丢包点
def get_position_list(self): def get_position_list(self):
@ -189,11 +219,11 @@ class FeedLine:
if self.feed_positions[i].status == FeedStatus.FPhoto.value: if self.feed_positions[i].status == FeedStatus.FPhoto.value:
index_take = i index_take = i
self.origin_to_start = self.feed_positions[: index_start+1] self.origin_to_start = self.feed_positions[: index_start + 1]
self.start_to_take = self.feed_positions[index_start:index_take+1] self.start_to_take = self.feed_positions[index_start : index_take + 1]
self.feeding_to_end = self.feed_positions[index_take:] self.feeding_to_end = self.feed_positions[index_take:]
for i in range(len(self.feeding_to_end)): #插入动态中间点 for i in range(len(self.feeding_to_end)): # 插入动态中间点
if self.feeding_to_end[i].status == FeedStatus.FTake.value: if self.feeding_to_end[i].status == FeedStatus.FTake.value:
befor_position_model = PositionModel() befor_position_model = PositionModel()
befor_position_model.init_position(None) befor_position_model.init_position(None)
@ -201,19 +231,26 @@ class FeedLine:
after_position_model.init_position(None) after_position_model.init_position(None)
self.feeding_to_end.insert(i, befor_position_model) self.feeding_to_end.insert(i, befor_position_model)
self.feeding_to_end.insert(i+2, after_position_model) self.feeding_to_end.insert(i + 2, after_position_model)
break break
class FeedingConfig: class FeedingConfig:
def __init__(self, num: int, feedLine: FeedLine, photo_locs): def __init__(self, num: int, feedLine: FeedLine, photo_locs):
self.num = num self.num = num
self.feedLine = feedLine self.feedLine = feedLine
self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs] self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
def deal_photo_locs(self, photo_loc): def deal_photo_locs(self, photo_loc):
position_photo = Real_Position() 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]) 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 return position_photo
def get_line_info(self): def get_line_info(self):
@ -224,7 +261,8 @@ class Feeding(QObject):
need_origin_signal = Signal(str) need_origin_signal = Signal(str)
take_no_photo_sigal = Signal() take_no_photo_sigal = Signal()
update_detect_image = Signal(np.ndarray) update_detect_image = Signal(np.ndarray)
log_signal = Signal(int,str) log_signal = Signal(int, str)
def __init__(self, robotClient: RobotClient): def __init__(self, robotClient: RobotClient):
super().__init__() super().__init__()
self.feedConfig = None self.feedConfig = None
@ -246,7 +284,7 @@ class Feeding(QObject):
self.reset_status = ResetStatus.RNone self.reset_status = ResetStatus.RNone
self.reversed_positions = [] self.reversed_positions = []
self.current_position = None self.current_position = None
self.index=1 self.index = 1
self.pos_index = -1 self.pos_index = -1
self.pos_near_index = -1 self.pos_near_index = -1
self.catch = Catch(self.robotClient) self.catch = Catch(self.robotClient)
@ -257,7 +295,7 @@ class Feeding(QObject):
self.onekey = False self.onekey = False
self.debug_run_count = 0 # 初始化计数器 self.debug_run_count = 0 # 初始化计数器
self.mid_take_count = 0 self.mid_take_count = 0
#传感器判断抓包参数 # 传感器判断抓包参数
self.sensor2_ready = False # 传感器2是否检测到料包 self.sensor2_ready = False # 传感器2是否检测到料包
self.motor_stopped_by_sensor2 = False # 是否由传感器2触发停止电机 self.motor_stopped_by_sensor2 = False # 是否由传感器2触发停止电机
self.sensor_thread = None self.sensor_thread = None
@ -272,9 +310,11 @@ class Feeding(QObject):
pass pass
def close_feed(self): def close_feed(self):
self.need_origin_signal.disconnect()
self.relay_controller._running = False
self.is_detected = False self.is_detected = False
self.detect_thread.join() self.detect_thread.join()
self.detect.detection.release() # self.detect.detection.release()
def init_detection_image(self): def init_detection_image(self):
detection_image = cv2.imread(Constant.feed_sign_path) detection_image = cv2.imread(Constant.feed_sign_path)
@ -290,12 +330,14 @@ class Feeding(QObject):
# 获取事件坐标 # 获取事件坐标
real_position = Real_Position() real_position = Real_Position()
self.detect.position_index = 0 self.detect.position_index = 0
real_position.init_position(self.robotClient.status_model.world_0, real_position.init_position(
self.robotClient.status_model.world_0,
self.robotClient.status_model.world_1, self.robotClient.status_model.world_1,
self.robotClient.status_model.world_2, self.robotClient.status_model.world_2,
self.robotClient.status_model.world_3, self.robotClient.status_model.world_3,
self.robotClient.status_model.world_4, self.robotClient.status_model.world_4,
self.robotClient.status_model.world_5) self.robotClient.status_model.world_5,
)
# real_position.init_position(0, # real_position.init_position(0,
# 0, # 0,
# 0, # 0,
@ -312,7 +354,12 @@ class Feeding(QObject):
if self.feedConfig == None: if self.feedConfig == None:
self.feedStatus = FeedStatus.FNone 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.feedConfig.num == 0
and self.is_reverse
and self.robotClient.origin_position.compare(real_position, is_action=True)
):
self.feedStatus = FeedStatus.FNone self.feedStatus = FeedStatus.FNone
self.is_reverse = False self.is_reverse = False
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse) self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
@ -338,15 +385,24 @@ class Feeding(QObject):
else: else:
self.feed_Mid_Status = FeedMidStatus.FMid_Take self.feed_Mid_Status = FeedMidStatus.FMid_Take
if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position): if (
self.feedConfig.feedLine.start_to_take[0]
.get_position()
.compare(real_position)
):
self.next_position(self.is_reverse) self.next_position(self.is_reverse)
elif self.feedStatus == FeedStatus.FStart: elif self.feedStatus == FeedStatus.FStart:
self.log_signal.emit(logging.INFO, Constant.str_feed_start) self.log_signal.emit(logging.INFO, Constant.str_feed_start)
self.relay_controller.open(conveyor2=True)#开电机 self.relay_controller.open(conveyor2=True) # 开电机
#self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢 # self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢
#self.sensor2_thread.start() # self.sensor2_thread.start()
if not self.robotClient.origin_position.compare(real_position,is_action=True) and not self.is_reverse: 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 引起异常 # QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error) self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error)
self.need_origin_signal.emit(Constant.str_feed_start_error) self.need_origin_signal.emit(Constant.str_feed_start_error)
@ -373,19 +429,23 @@ class Feeding(QObject):
if Constant.Debug1: if Constant.Debug1:
self.log_signal.emit( self.log_signal.emit(
logging.INFO, logging.INFO,
f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次" f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次",
) )
#这里他写的中间点过完马上进行抓料，和整个流程不符合我就注释掉了，因为实际测试的时候还没到FTake的点他就进行了继电器关闭夹爪的抓取 # 这里他写的中间点过完马上进行抓料，和整个流程不符合我就注释掉了，因为实际测试的时候还没到FTake的点他就进行了继电器关闭夹爪的抓取
#if self.feedStatus == FeedStatus.FTake: # if self.feedStatus == FeedStatus.FTake:
#self.catch.catch_status = CatchStatus.CTake # self.catch.catch_status = CatchStatus.CTake
elif self.feedStatus == FeedStatus.FPhoto: elif self.feedStatus == FeedStatus.FPhoto:
if self.feedConfig.num == 0: if self.feedConfig.num == 0:
self.log_signal.emit(logging.INFO, Constant.str_feed_finish) self.log_signal.emit(logging.INFO, Constant.str_feed_finish)
self.is_reverse = True self.is_reverse = True
self.feed_Mid_Status = FeedMidStatus.FMid_Take self.feed_Mid_Status = FeedMidStatus.FMid_Take
self.feedConfig.feedLine.start2take_pos_index = len(self.feedConfig.feedLine.start_to_take) - 2 self.feedConfig.feedLine.start2take_pos_index = (
self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2 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.next_position(self.is_reverse)
self.log_signal.emit(logging.INFO, Constant.str_feed_photo) self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
self.init_detection_image() self.init_detection_image()
@ -395,15 +455,19 @@ class Feeding(QObject):
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto) self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto)
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.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(
#self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置 self.detect.detect_position, 0
'''real_position''' )
# self.feedConfig.feedLine.set_take_position(real_position, 0)#必须设置
"""real_position"""
# 一直等待传感器2信号，永不超时 # 一直等待传感器2信号，永不超时
while True: while True:
sensors = self.relay_controller.get_all_device_status('sensors') sensors = self.relay_controller.get_all_device_status("sensors")
sensor2_value = sensors.get(self.relay_controller.SENSOR2, False) sensor2_value = sensors.get(self.relay_controller.SENSOR2, False)
if sensor2_value: if sensor2_value:
self.log_signal.emit(logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取") self.log_signal.emit(
logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取"
)
break # ✅ 条件满足，跳出循环，继续执行下面的代码 break # ✅ 条件满足，跳出循环，继续执行下面的代码
else: else:
self.log_signal.emit(logging.INFO, "⏳ 等待传感器2料包信号...") self.log_signal.emit(logging.INFO, "⏳ 等待传感器2料包信号...")
@ -411,7 +475,7 @@ class Feeding(QObject):
self.next_position() self.next_position()
self.log_signal.emit(logging.INFO, Constant.str_sys_runing2) self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
#self.feedStatus = FeedStatus.FTake # self.feedStatus = FeedStatus.FTake
elif self.feedStatus == FeedStatus.FTake: elif self.feedStatus == FeedStatus.FTake:
self.log_signal.emit(logging.INFO, Constant.str_feed_take) self.log_signal.emit(logging.INFO, Constant.str_feed_take)
@ -424,31 +488,33 @@ class Feeding(QObject):
return return
self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位") self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位")
# 执行抓取动作 # 执行抓取动作
#self.catch.catch_status = CatchStatus.CTake # self.catch.catch_status = CatchStatus.CTake
#if self.catch.catch_status == CatchStatus.CNone: # if self.catch.catch_status == CatchStatus.CNone:
#self.catch.catch_status = CatchStatus.CTake # self.catch.catch_status = CatchStatus.CTake
#if self.catch.catch_status == CatchStatus.CTake: # if self.catch.catch_status == CatchStatus.CTake:
#self.log_signal.emit(logging.INFO, "正在执行抓料动作...") # self.log_signal.emit(logging.INFO, "正在执行抓料动作...")
#self.catch.catch_status = CatchStatus.COk # self.catch.catch_status = CatchStatus.COk
#if 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.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
#self.catch.catch_status = CatchStatus.CNone # self.catch.catch_status = CatchStatus.CNone
# 移动到下一个抓取点 # 移动到下一个抓取点
# 更新丢包点: 如果需要根据放置情况调整下次抓取 # 更新丢包点: 如果需要根据放置情况调整下次抓取
next_drop_pos = self.drop_manager.get_next_drop_position(lineid=1) next_drop_pos = self.drop_manager.get_next_drop_position(lineid=1)
print("next_drop_pos--------",next_drop_pos) print("next_drop_pos--------", next_drop_pos)
if next_drop_pos: if next_drop_pos:
self.feedConfig.feedLine.set_drop_position(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.log_signal.emit(
logging.INFO, f"已设置放置点: X={next_drop_pos.X:.2f}"
)
else: else:
self.log_signal.emit(logging.ERROR, "获取放置点失败") self.log_signal.emit(logging.ERROR, "获取放置点失败")
return return
#self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点 # self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
self.relay_controller.open(clamp=True) self.relay_controller.open(clamp=True)
self.next_position() self.next_position()
#return # return
#else: # else:
#self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail) # self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
elif self.feedStatus == FeedStatus.FBroken1: elif self.feedStatus == FeedStatus.FBroken1:
@ -456,7 +522,6 @@ class Feeding(QObject):
self.log_signal.emit(logging.INFO, Constant.str_feed_broken) self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
self.next_position() self.next_position()
elif self.feedStatus == FeedStatus.FBroken2: elif self.feedStatus == FeedStatus.FBroken2:
if self.get_current_position().get_position().compare(real_position): if self.get_current_position().get_position().compare(real_position):
@ -464,7 +529,11 @@ class Feeding(QObject):
self.next_position() self.next_position()
elif self.feedStatus == FeedStatus.FShake: elif self.feedStatus == FeedStatus.FShake:
if self.get_current_position().get_position().compare(real_position,is_action=True): if (
self.get_current_position()
.get_position()
.compare(real_position, is_action=True)
):
# TODO 震动方案 # TODO 震动方案
self.log_signal.emit(logging.INFO, Constant.str_feed_shake) self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
if self.catch.catch_status == CatchStatus.CNone: if self.catch.catch_status == CatchStatus.CNone:
@ -477,21 +546,25 @@ class Feeding(QObject):
# else: # else:
self.catch.shake_continue.SetReset() self.catch.shake_continue.SetReset()
self.next_position() self.next_position()
if self.feedStatus!=FeedStatus.FShake: if self.feedStatus != FeedStatus.FShake:
self.catch.catch_status = CatchStatus.CNone self.catch.catch_status = CatchStatus.CNone
return return
elif self.feedStatus == FeedStatus.FDropBag: elif self.feedStatus == FeedStatus.FDropBag:
#"""*** 处理投料点 (FDropBag) 的核心逻辑 ***""" # """*** 处理投料点 (FDropBag) 的核心逻辑 ***"""
# 1. 确认机械臂是否已精确到达当前目标投料点 # 1. 确认机械臂是否已精确到达当前目标投料点
# get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed) # get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed)
# 调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取 # 调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取
# 由 feeding2end_pos_index 指向的点。 # 由 feeding2end_pos_index 指向的点。
if self.get_current_position().get_position().compare(real_position, is_action=True): if (
self.get_current_position()
.get_position()
.compare(real_position, is_action=True)
):
# 2. 记录日志：已到达投料点 # 2. 记录日志：已到达投料点
#if not self.sensor2_ready: # if not self.sensor2_ready:
#self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2") # self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2")
#self.relay_controller.open(conveyor2=True) # self.relay_controller.open(conveyor2=True)
self.log_signal.emit(logging.INFO, Constant.str_feed_drop) self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
# 3. 与 Catch 模块进行状态交互来驱动投料动作 # 3. 与 Catch 模块进行状态交互来驱动投料动作
@ -519,7 +592,10 @@ class Feeding(QObject):
# self.feedConfig.feedLine.set_take_position(...) # self.feedConfig.feedLine.set_take_position(...)
# 4. 更新业务逻辑：减少剩余袋数 # 4. 更新业务逻辑：减少剩余袋数
self.feedConfig.num = self.feedConfig.num - 1 self.feedConfig.num = self.feedConfig.num - 1
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}') self.log_signal.emit(
logging.INFO,
f"{Constant.str_feed_feed_num}{self.feedConfig.num}",
)
# 5. *** 关键步骤 ***: 移动到路径中的下一个点 # 5. *** 关键步骤 ***: 移动到路径中的下一个点
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed) # next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
# 调用 next_Feed()。 # 调用 next_Feed()。
@ -527,17 +603,20 @@ class Feeding(QObject):
def run_reset(self): def run_reset(self):
real_position = Real_Position() real_position = Real_Position()
real_position.init_position(self.robotClient.status_model.world_0, real_position.init_position(
self.robotClient.status_model.world_0,
self.robotClient.status_model.world_1, self.robotClient.status_model.world_1,
self.robotClient.status_model.world_2, self.robotClient.status_model.world_2,
self.robotClient.status_model.world_3, self.robotClient.status_model.world_3,
self.robotClient.status_model.world_4, self.robotClient.status_model.world_4,
self.robotClient.status_model.world_5) self.robotClient.status_model.world_5,
)
if self.reset_status == ResetStatus.RNone: if self.reset_status == ResetStatus.RNone:
return return
if self.reset_status == ResetStatus.RStart: if self.reset_status == ResetStatus.RStart:
if self.feedConfig == None: return if self.feedConfig == None:
return
# for index in range(len(self.feedConfig.feedLine.positions)): # for index in range(len(self.feedConfig.feedLine.positions)):
# if self.feedConfig.feedLine.positions[index].status == 2: # if self.feedConfig.feedLine.positions[index].status == 2:
# start_index = index # start_index = index
@ -545,23 +624,36 @@ class Feeding(QObject):
self.pos_near_index = -1 self.pos_near_index = -1
self.reversed_positions = [] self.reversed_positions = []
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
if pos_model.get_position().compare(real_position,is_action=True): if pos_model.get_position().compare(real_position, is_action=True):
self.pos_index = index self.pos_index = index
break break
if self.pos_index == -1: if self.pos_index == -1:
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail) self.log_signal.emit(
logging.ERROR, Constant.str_feed_return_original_position_fail
)
min_distance = 99999999 min_distance = 99999999
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): for index, pos_model in enumerate(
if get_distance(pos_model.get_position(), real_position) < min_distance: self.feedConfig.feedLine.feed_positions
min_distance = get_distance(pos_model.get_position(), real_position) ):
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 self.pos_near_index = index
if self.pos_near_index != -1: if self.pos_near_index != -1:
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1] self.reversed_positions = self.feedConfig.feedLine.feed_positions[
: self.pos_near_index + 1
]
else: else:
return False return False
else: else:
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index+1] self.reversed_positions = self.feedConfig.feedLine.feed_positions[
: self.pos_index + 1
]
self.reversed_positions = list(reversed(self.reversed_positions)) self.reversed_positions = list(reversed(self.reversed_positions))
self.reverse_index = 0 self.reverse_index = 0
self.send_emergency_sound() self.send_emergency_sound()
@ -570,21 +662,30 @@ class Feeding(QObject):
self.reset_status = ResetStatus.RRunging self.reset_status = ResetStatus.RRunging
if self.reset_status == ResetStatus.RRunging: if self.reset_status == ResetStatus.RRunging:
if not real_position.compare(self.current_position.get_position(),is_action=True): if not real_position.compare(
self.current_position.get_position(), is_action=True
):
return return
pos_model = self.reversed_positions[self.reverse_index] pos_model = self.reversed_positions[self.reverse_index]
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点 if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
pos_model = self.reversed_positions[self.reverse_index + 1] pos_model = self.reversed_positions[self.reverse_index + 1]
self.reverse_index = self.reverse_index+1 self.reverse_index = self.reverse_index + 1
if pos_model.lineType == LineType.CureMid.value: if pos_model.lineType == LineType.CureMid.value:
pos_model1 = self.reversed_positions[self.reverse_index + 1] pos_model1 = self.reversed_positions[self.reverse_index + 1]
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure, self.sendTargPosition(
real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed) 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.current_position = pos_model1
self.reverse_index = self.reverse_index + 2 self.reverse_index = self.reverse_index + 2
else: else:
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed) self.sendTargPosition(
real_position=pos_model.get_position(),
speed=self.robotClient.reset_speed,
)
self.current_position = pos_model self.current_position = pos_model
self.reverse_index = self.reverse_index + 1 self.reverse_index = self.reverse_index + 1
@ -598,13 +699,16 @@ class Feeding(QObject):
self.run_reverse = True self.run_reverse = True
real_position = Real_Position() real_position = Real_Position()
real_position.init_position(self.robotClient.status_model.world_0, real_position.init_position(
self.robotClient.status_model.world_0,
self.robotClient.status_model.world_1, self.robotClient.status_model.world_1,
self.robotClient.status_model.world_2, self.robotClient.status_model.world_2,
self.robotClient.status_model.world_3, self.robotClient.status_model.world_3,
self.robotClient.status_model.world_4, self.robotClient.status_model.world_4,
self.robotClient.status_model.world_5) self.robotClient.status_model.world_5,
if self.feedConfig == None: return )
if self.feedConfig == None:
return
start_index = -1 start_index = -1
# for index in range(len(self.feedConfig.feedLine.positions)): # for index in range(len(self.feedConfig.feedLine.positions)):
# if self.feedConfig.feedLine.positions[index].status == 2: # if self.feedConfig.feedLine.positions[index].status == 2:
@ -618,14 +722,18 @@ class Feeding(QObject):
break break
if pos_index == -1: if pos_index == -1:
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail) self.log_signal.emit(
logging.ERROR, Constant.str_feed_return_original_position_fail
)
min_distance = 99999999 min_distance = 99999999
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions): for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
if get_distance(pos_model.get_position(), real_position)<min_distance: if get_distance(pos_model.get_position(), real_position) < min_distance:
min_distance = get_distance(pos_model.get_position(), real_position) min_distance = get_distance(pos_model.get_position(), real_position)
pos_near_index = index pos_near_index = index
if pos_near_index != -1: if pos_near_index != -1:
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_near_index+1] reversed_positions = self.feedConfig.feedLine.feed_positions[
: pos_near_index + 1
]
else: else:
return False return False
else: else:
@ -635,10 +743,12 @@ class Feeding(QObject):
self.send_emergency_sound() self.send_emergency_sound()
current_position = PositionModel() current_position = PositionModel()
current_position.init_position(real_position) current_position.init_position(real_position)
while self.run_reverse and self.reverse_index!=len(reversed_positions): 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()): if self.reverse_index != 0 and not real_position.compare(
current_position.get_position()
):
continue continue
#todo 缺少比对 # todo 缺少比对
pos_model = reversed_positions[self.reverse_index] pos_model = reversed_positions[self.reverse_index]
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点 if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
@ -647,16 +757,24 @@ class Feeding(QObject):
if pos_model.lineType == LineType.CureMid.value: if pos_model.lineType == LineType.CureMid.value:
pos_model1 = reversed_positions[self.reverse_index + 1] pos_model1 = reversed_positions[self.reverse_index + 1]
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure, self.sendTargPosition(
real_position1=pos_model1.get_position(),speed= self.robotClient.reset_speed) 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 current_position = pos_model1
self.reverse_index = self.reverse_index+2 self.reverse_index = self.reverse_index + 2
else: else:
self.sendTargPosition(real_position=pos_model.get_position(),speed=self.robotClient.reset_speed) self.sendTargPosition(
real_position=pos_model.get_position(),
speed=self.robotClient.reset_speed,
)
current_position = pos_model current_position = pos_model
self.reverse_index = self.reverse_index + 1 self.reverse_index = self.reverse_index + 1
self.send_emergency_stop() self.send_emergency_stop()
return True return True
def send_emergency_sound(self): def send_emergency_sound(self):
self.sendIOControl(Constant.IO_EmergencyPoint, 1) self.sendIOControl(Constant.IO_EmergencyPoint, 1)
@ -670,17 +788,30 @@ class Feeding(QObject):
io_instruction.IO = True io_instruction.IO = True
io_instruction.io_status = IO_Status 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_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.dsID = "HCRemoteCommand"
IO_command.instructions.append(io_instruction) IO_command.instructions.append(io_instruction)
self.robotClient.add_sendQuene(IO_command.toString()) self.robotClient.add_sendQuene(IO_command.toString())
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}') self.log_signal.emit(
logging.INFO, f"{Constant.str_feed_io_control}{IO_bit}，{IO_Status}"
)
pass pass
def sendTargPosition1(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None): def sendTargPosition1(
self,
real_position,
move_type: MoveType = MoveType.WORLD,
speed=5,
real_position1=None,
):
pass pass
def sendTargPosition(
def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None): self,
real_position,
move_type: MoveType = MoveType.WORLD,
speed=5,
real_position1=None,
):
position_instruction = Instruction() position_instruction = Instruction()
position_instruction.speed = speed position_instruction.speed = speed
position_instruction.m0 = real_position.X position_instruction.m0 = real_position.X
@ -703,24 +834,26 @@ class Feeding(QObject):
instruction_command.instructions.append(position_instruction) instruction_command.instructions.append(position_instruction)
request_command = instruction_command.toString() request_command = instruction_command.toString()
log_str = f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：' \ log_str = (
f'X:{position_instruction.m0}-' \ f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：'
f'Y:{position_instruction.m1}-' \ f"X:{position_instruction.m0}-"
f'Z:{position_instruction.m2}-' \ f"Y:{position_instruction.m1}-"
f'U:{position_instruction.m3}-' \ f"Z:{position_instruction.m2}-"
f'V:{position_instruction.m4}-' \ f"U:{position_instruction.m3}-"
f'W:{position_instruction.m5}' f"V:{position_instruction.m4}-"
f"W:{position_instruction.m5}"
)
try: try:
print('fuck1',log_str) print("fuck1", log_str)
self.log_signal.emit(logging.INFO, log_str) self.log_signal.emit(logging.INFO, log_str)
print('fuck2',log_str) print("fuck2", log_str)
except: except:
return return
print(request_command) print(request_command)
self.robotClient.add_sendQuene(request_command) self.robotClient.add_sendQuene(request_command)
pass pass
# def get_take_position(self): # def get_take_position(self):
# if Constant.Debug: # if Constant.Debug:
# return self.robotClient.status_model.getRealPosition() # return self.robotClient.status_model.getRealPosition()
@ -733,59 +866,117 @@ class Feeding(QObject):
# position = Real_Position().init_position(*target_position[:3],*noraml_base[:3]) # position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
# position.Z = position.Z+200 # position.Z = position.Z+200
# return position # return position
def next_start(self,reverse=False): def next_start(self, reverse=False):
start_pos = self.feedConfig.feedLine.get_next_start_position(reverse) start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone self.feedStatus = (
FeedStatus(start_pos.status)
if self.feedStatus != FeedStatus.FNone
else FeedStatus.FNone
)
if start_pos.lineType == LineType.CureMid.value: if start_pos.lineType == LineType.CureMid.value:
start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse) 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.feedStatus = (
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed, move_type=MoveType.Cure, real_position1=start_pos1.get_position()) 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: 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): if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
start_pos.get_position(), self.robotClient.max_angle_interval
):
self.feedStatus = None self.feedStatus = None
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error) self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
else: else:
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed,move_type=MoveType.AXIS) self.sendTargPosition(
real_position=start_pos.get_position(),
speed=self.robotClient.reset_speed,
move_type=MoveType.AXIS,
)
else: else:
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed) self.sendTargPosition(
real_position=start_pos.get_position(),
speed=self.robotClient.reset_speed,
)
pass pass
def next_take(self,reverse=False): def next_take(self, reverse=False):
take_pos = self.feedConfig.feedLine.get_next_take_position(reverse) take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone self.feedStatus = (
FeedStatus(take_pos.status)
if self.feedStatus != FeedStatus.FNone
else FeedStatus.FNone
)
if take_pos.lineType == LineType.CureMid.value: if take_pos.lineType == LineType.CureMid.value:
take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse) 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.feedStatus = (
self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position(),speed=self.robotClient.feed_speed) 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: elif take_pos.lineType == LineType.WORLD.value:
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(take_pos.get_position(), if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
self.robotClient.max_angle_interval): take_pos.get_position(), self.robotClient.max_angle_interval
):
self.feedStatus = None self.feedStatus = None
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error) self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
else: else:
self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed, self.sendTargPosition(
move_type=MoveType.AXIS) real_position=take_pos.get_position(),
speed=self.robotClient.feed_speed,
move_type=MoveType.AXIS,
)
else: else:
self.sendTargPosition(real_position=take_pos.get_position(),speed=self.robotClient.feed_speed) self.sendTargPosition(
real_position=take_pos.get_position(), speed=self.robotClient.feed_speed
)
pass pass
def next_Feed(self,reverse=False): def next_Feed(self, reverse=False):
feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse) feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone self.feedStatus = (
FeedStatus(feed_pos.status)
if self.feedStatus != FeedStatus.FNone
else FeedStatus.FNone
)
if feed_pos.lineType == LineType.CureMid.value: if feed_pos.lineType == LineType.CureMid.value:
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse) 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.feedStatus = (
self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position(),speed=self.robotClient.feed_speed) 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: elif feed_pos.lineType == LineType.WORLD.value:
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(), if self.robotClient.status_model.getAnglePosition().is_error_angel_move(
self.robotClient.max_angle_interval): feed_pos.get_position(), self.robotClient.max_angle_interval
):
self.feedStatus = None self.feedStatus = None
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error) self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
else: else:
self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed, self.sendTargPosition(
move_type=MoveType.AXIS) real_position=feed_pos.get_position(),
speed=self.robotClient.feed_speed,
move_type=MoveType.AXIS,
)
else: else:
# if not reverse and self.feedStatus == FeedStatus.FShake: # if not reverse and self.feedStatus == FeedStatus.FShake:
@ -797,29 +988,31 @@ class Feeding(QObject):
# self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed) # self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
# return # return
self.sendTargPosition(
real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed
)
self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed) def get_current_position(self, is_reverse=False):
def get_current_position(self,is_reverse=False):
if self.feed_Mid_Status == FeedMidStatus.FMid_Start: if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
return self.feedConfig.feedLine.get_current_start_position(self.is_reverse) return self.feedConfig.feedLine.get_current_start_position(self.is_reverse)
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take: elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
return self.feedConfig.feedLine.get_current_take_position(self.is_reverse) return self.feedConfig.feedLine.get_current_take_position(self.is_reverse)
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed: elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
return self.feedConfig.feedLine.get_current_feed_position(self.is_reverse) return self.feedConfig.feedLine.get_current_feed_position(self.is_reverse)
def next_position(self,reverse=False):
def next_position(self, reverse=False):
if self.feed_Mid_Status == FeedMidStatus.FMid_Start: if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
self.next_start(reverse) self.next_start(reverse)
if Constant.Debug1: if Constant.Debug1:
print('next_start') print("next_start")
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take: elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
self.next_take(reverse) self.next_take(reverse)
if Constant.Debug1: if Constant.Debug1:
print('next_take') print("next_take")
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed: elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
self.next_Feed(reverse) self.next_Feed(reverse)
if Constant.Debug1: if Constant.Debug1:
print('next_feed') print("next_feed")
def safe_check_columns(self): def safe_check_columns(self):
return True return True
@ -828,4 +1021,3 @@ class Feeding(QObject):
def safe_check_person(self): def safe_check_person(self):
return True return True
pass pass

152
EMV/EMV.py
View File

@ -15,7 +15,7 @@ class RelayController:
update_detect_image = Signal(np.ndarray) update_detect_image = Signal(np.ndarray)
log_signal = Signal(int, str) log_signal = Signal(int, str)
def __init__(self, host='192.168.0.18', port=50000): def __init__(self, host="192.168.0.18", port=50000):
# ===================== 全局线程延时参数 ===================== # ===================== 全局线程延时参数 =====================
self.sensor1_loop_delay = 0.2 # SENSOR1 线程轮询间隔（秒） self.sensor1_loop_delay = 0.2 # SENSOR1 线程轮询间隔（秒）
self.sensor1_error_delay = 1.0 # SENSOR1 出错时延时（秒） self.sensor1_error_delay = 1.0 # SENSOR1 出错时延时（秒）
@ -25,7 +25,9 @@ class RelayController:
self.sensor2_error_delay = 0.5 # SENSOR2 出错时延时（秒） self.sensor2_error_delay = 0.5 # SENSOR2 出错时延时（秒）
self.sensor2_post_action_delay = 0.2 # SENSOR2 每次循环后延时（秒） self.sensor2_post_action_delay = 0.2 # SENSOR2 每次循环后延时（秒）
# ===================== 全局动作延时参数 ===================== # ===================== 全局动作延时参数 =====================
self.delay_conveyor = 0.5 # 传送带开/关动作延时（一半时间，我在控制程序和线程都加了一样的延时） self.delay_conveyor = (
0.5 # 传送带开/关动作延时（一半时间，我在控制程序和线程都加了一样的延时）
)
self.delay_pusher = 0.05 # 推板开/关动作延时 self.delay_pusher = 0.05 # 推板开/关动作延时
self.delay_clamp = 0.5 # 夹爪动作延时 self.delay_clamp = 0.5 # 夹爪动作延时
self.delay_after_pusher = 3.0 # 推板推出后到重启传动带时间 self.delay_after_pusher = 3.0 # 推板推出后到重启传动带时间
@ -39,25 +41,40 @@ class RelayController:
self.host = host self.host = host
self.port = port self.port = port
self.CONVEYOR1 = 'conveyor1' self.CONVEYOR1 = "conveyor1"
self.PUSHER = 'pusher' self.PUSHER = "pusher"
self.CONVEYOR2 = 'conveyor2' self.CONVEYOR2 = "conveyor2"
self.CLAMP = 'clamp' self.CLAMP = "clamp"
self.PUSHER1 = 'pusher1' self.PUSHER1 = "pusher1"
self.SENSOR1 = 'sensor1' self.SENSOR1 = "sensor1"
self.SENSOR2 = 'sensor2' self.SENSOR2 = "sensor2"
self.valve_commands = { self.valve_commands = {
self.CONVEYOR1: {'open': '00000000000601050000FF00', 'close': '000000000006010500000000'}, self.CONVEYOR1: {
self.PUSHER: {'open': '00000000000601050001FF00', 'close': '000000000006010500010000'}, "open": "00000000000601050000FF00",
self.CONVEYOR2: {'open': '000100000006020620000012', 'close': '000100000006020620000001'}, "close": "000000000006010500000000",
self.CLAMP: {'open': '00000000000601050003FF00', 'close': '000000000006010500030000'}, },
self.PUSHER1: {'open': '00000000000601050004FF00', 'close': '000000000006010500040000'} self.PUSHER: {
"open": "00000000000601050001FF00",
"close": "000000000006010500010000",
},
self.CONVEYOR2: {
"open": "000100000006020620000012",
"close": "000100000006020620000001",
},
self.CLAMP: {
"open": "00000000000601050003FF00",
"close": "000000000006010500030000",
},
self.PUSHER1: {
"open": "00000000000601050004FF00",
"close": "000000000006010500040000",
},
} }
self.read_status_command = { self.read_status_command = {
'devices': '000000000006010100000008', "devices": "000000000006010100000008",
'sensors': '000000000006010200000008' "sensors": "000000000006010200000008",
} }
self.device_bit_map = { self.device_bit_map = {
@ -82,8 +99,8 @@ class RelayController:
} }
self.sensor_name_map = { self.sensor_name_map = {
self.SENSOR1: '位置传感器1', self.SENSOR1: "位置传感器1",
self.SENSOR2: '位置传感器2', self.SENSOR2: "位置传感器2",
} }
# ===================== 状态控制变量 ===================== # ===================== 状态控制变量 =====================
@ -91,8 +108,8 @@ class RelayController:
self._sensor1_thread = None self._sensor1_thread = None
self._sensor2_thread = None self._sensor2_thread = None
self.required_codes = {'0101', '0103'} # 有效状态码（传感器1） self.required_codes = {"0101", "0103"} # 有效状态码（传感器1）
self.required_codes_1 = {'0102', '0103'} # 有效状态码（传感器2） self.required_codes_1 = {"0102", "0103"} # 有效状态码（传感器2）
self.sensor1_triggered = False self.sensor1_triggered = False
self.sensor1_last_time = 0 self.sensor1_last_time = 0
@ -101,25 +118,33 @@ class RelayController:
self.motor_stopped_by_sensor2 = False self.motor_stopped_by_sensor2 = False
# ===================== 基础通信方法 ===================== # ===================== 基础通信方法 =====================
def send_command(self, command_hex, retry_count=2, source='unknown'): def send_command(self, command_hex, retry_count=2, source="unknown"):
if not self._running:
return
byte_data = binascii.unhexlify(command_hex) byte_data = binascii.unhexlify(command_hex)
for attempt in range(retry_count): for attempt in range(retry_count):
if not self._running:
return
try: try:
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
sock.settimeout(10) sock.settimeout(10)
sock.connect((self.host, self.port)) sock.connect((self.host, self.port))
sock.send(byte_data) sock.send(byte_data)
response = sock.recv(1024) response = sock.recv(1024)
hex_response = binascii.hexlify(response).decode('utf-8') hex_response = binascii.hexlify(response).decode("utf-8")
#if source == 'sensor': # if source == 'sensor':
#print(f"[传感器响应] {hex_response}") # print(f"[传感器响应] {hex_response}")
#elif source == 'device': # elif source == 'device':
#print(f"[设备控制响应] {hex_response}") # print(f"[设备控制响应] {hex_response}")
#else: # else:
#print(f"[通信响应] {hex_response}") # print(f"[通信响应] {hex_response}")
return response return response
except Exception as e: except Exception as e:
print(f"通信错误 ({source}): {e}, 尝试重连... ({attempt + 1}/{retry_count})") print(
f"通信错误 ({source}): {e}, 尝试重连... ({attempt + 1}/{retry_count})"
)
time.sleep(5) time.sleep(5)
self.trigger_alarm() self.trigger_alarm()
return None return None
@ -128,30 +153,38 @@ class RelayController:
print("警告：连续多次通信失败，请检查设备连接！") print("警告：连续多次通信失败，请检查设备连接！")
# ===================== 状态读取方法 ===================== # ===================== 状态读取方法 =====================
def get_all_device_status(self, command_type='devices'): def get_all_device_status(self, command_type="devices"):
command = self.read_status_command.get(command_type) command = self.read_status_command.get(command_type)
if not command: if not command:
print(f"未知的读取类型: {command_type}") print(f"未知的读取类型: {command_type}")
return {} return {}
source = 'sensor' if command_type == 'sensors' else 'device' source = "sensor" if command_type == "sensors" else "device"
response = self.send_command(command, source=source) response = self.send_command(command, source=source)
status_dict = {} status_dict = {}
if response and len(response) >= 10: if response and len(response) >= 10:
status_byte = response[9] status_byte = response[9]
status_bin = f"{status_byte:08b}"[::-1] status_bin = f"{status_byte:08b}"[::-1]
bit_map = self.device_bit_map if command_type == 'devices' else self.sensor_bit_map bit_map = (
name_map = self.device_name_map if command_type == 'devices' else self.sensor_name_map self.device_bit_map
if command_type == "devices"
else self.sensor_bit_map
)
name_map = (
self.device_name_map
if command_type == "devices"
else self.sensor_name_map
)
for key, bit_index in bit_map.items(): for key, bit_index in bit_map.items():
status_dict[key] = status_bin[bit_index] == '1' status_dict[key] = status_bin[bit_index] == "1"
else: else:
print(f"[{command_type}] 读取状态失败或响应无效") print(f"[{command_type}] 读取状态失败或响应无效")
return status_dict return status_dict
def get_all_sensor_responses(self, command_type='sensors'): def get_all_sensor_responses(self, command_type="sensors"):
""" """
获取所有传感器的原始 Modbus 响应字符串 获取所有传感器的原始 Modbus 响应字符串
示例：{'sensor1': '00000000000401020101', 'sensor2': '00000000000401020100'} 示例：{'sensor1': '00000000000401020101', 'sensor2': '00000000000401020100'}
@ -161,12 +194,12 @@ class RelayController:
print(f"未知的读取类型: {command_type}") print(f"未知的读取类型: {command_type}")
return {} return {}
source = 'sensor' if command_type == 'sensors' else 'device' source = "sensor" if command_type == "sensors" else "device"
response = self.send_command(command, source=source) response = self.send_command(command, source=source)
responses = {} responses = {}
if response and len(response) >= 10: if response and len(response) >= 10:
hex_response = binascii.hexlify(response).decode('utf-8') hex_response = binascii.hexlify(response).decode("utf-8")
print(f"[原始响应][{command_type}] {hex_response}") print(f"[原始响应][{command_type}] {hex_response}")
# 假设传感器数据从第 9 字节开始，长度为 2 字节 # 假设传感器数据从第 9 字节开始，长度为 2 字节
@ -192,7 +225,7 @@ class RelayController:
def is_valid_sensor_status(self, sensor_name): def is_valid_sensor_status(self, sensor_name):
stable_count = 0 stable_count = 0
for _ in range(int(self.sensor_stable_duration / self.sensor1_loop_delay)): for _ in range(int(self.sensor_stable_duration / self.sensor1_loop_delay)):
responses = self.get_all_sensor_responses('sensors') responses = self.get_all_sensor_responses("sensors")
response = responses.get(sensor_name) response = responses.get(sensor_name)
if not response: if not response:
@ -211,7 +244,7 @@ class RelayController:
def is_valid_sensor_status_1(self, sensor_name): def is_valid_sensor_status_1(self, sensor_name):
stable_count = 0 stable_count = 0
for _ in range(int(self.sensor_stable_duration / self.sensor2_loop_delay)): for _ in range(int(self.sensor_stable_duration / self.sensor2_loop_delay)):
responses = self.get_all_sensor_responses('sensors') responses = self.get_all_sensor_responses("sensors")
response = responses.get(sensor_name) response = responses.get(sensor_name)
if not response: if not response:
@ -229,49 +262,57 @@ class RelayController:
return False return False
# ===================== 动作控制方法 ===================== # ===================== 动作控制方法 =====================
def open(self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False): def open(
self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False
):
status = self.get_all_device_status() status = self.get_all_device_status()
if conveyor1 and not status.get(self.CONVEYOR1, False): if conveyor1 and not status.get(self.CONVEYOR1, False):
self.send_command(self.valve_commands[self.CONVEYOR1]['open']) self.send_command(self.valve_commands[self.CONVEYOR1]["open"])
time.sleep(self.delay_conveyor) time.sleep(self.delay_conveyor)
if pusher and not status.get(self.PUSHER, False): if pusher and not status.get(self.PUSHER, False):
self.send_command(self.valve_commands[self.PUSHER]['open']) self.send_command(self.valve_commands[self.PUSHER]["open"])
time.sleep(self.delay_pusher) time.sleep(self.delay_pusher)
if conveyor2 and not status.get(self.CONVEYOR2, False): if conveyor2 and not status.get(self.CONVEYOR2, False):
self.send_command(self.valve_commands[self.CONVEYOR2]['open']) self.send_command(self.valve_commands[self.CONVEYOR2]["open"])
time.sleep(self.delay_conveyor) time.sleep(self.delay_conveyor)
if clamp and not status.get(self.CLAMP, False): if clamp and not status.get(self.CLAMP, False):
self.send_command(self.valve_commands[self.CLAMP]['open']) self.send_command(self.valve_commands[self.CLAMP]["open"])
time.sleep(self.delay_clamp) time.sleep(self.delay_clamp)
if pusher1 and not status.get(self.PUSHER1, False): if pusher1 and not status.get(self.PUSHER1, False):
self.send_command(self.valve_commands[self.PUSHER1]['open']) self.send_command(self.valve_commands[self.PUSHER1]["open"])
time.sleep(self.delay_pusher) time.sleep(self.delay_pusher)
def close(self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False): def close(
self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False
):
if conveyor1: if conveyor1:
self.send_command(self.valve_commands[self.CONVEYOR1]['close']) self.send_command(self.valve_commands[self.CONVEYOR1]["close"])
time.sleep(self.delay_conveyor) time.sleep(self.delay_conveyor)
if pusher: if pusher:
self.send_command(self.valve_commands[self.PUSHER]['close']) self.send_command(self.valve_commands[self.PUSHER]["close"])
time.sleep(self.delay_pusher) time.sleep(self.delay_pusher)
if conveyor2: if conveyor2:
self.send_command(self.valve_commands[self.CONVEYOR2]['close']) self.send_command(self.valve_commands[self.CONVEYOR2]["close"])
time.sleep(self.delay_conveyor) time.sleep(self.delay_conveyor)
if clamp: if clamp:
self.send_command(self.valve_commands[self.CLAMP]['close']) self.send_command(self.valve_commands[self.CLAMP]["close"])
time.sleep(self.delay_clamp) time.sleep(self.delay_clamp)
if pusher1: if pusher1:
self.send_command(self.valve_commands[self.PUSHER1]['close']) self.send_command(self.valve_commands[self.PUSHER1]["close"])
time.sleep(self.delay_pusher) time.sleep(self.delay_pusher)
# ===================== 传感器处理线程 ===================== # ===================== 传感器处理线程 =====================
def handle_sensor1(self): def handle_sensor1(self):
print("传感器线程1启动！！！")
while self._running: while self._running:
try: try:
if self.is_valid_sensor_status(self.SENSOR1): if self.is_valid_sensor_status(self.SENSOR1):
current_time = time.time() current_time = time.time()
if not self.sensor1_triggered and ( if (
current_time - self.sensor1_last_time) > self.sensor1_debounce_time: not self.sensor1_triggered
and (current_time - self.sensor1_last_time)
> self.sensor1_debounce_time
):
self.sensor1_triggered = True self.sensor1_triggered = True
self.sensor1_last_time = current_time self.sensor1_last_time = current_time
# 1.停止包装机皮带电机：关闭 conveyor1 # 1.停止包装机皮带电机：关闭 conveyor1
@ -293,7 +334,7 @@ class RelayController:
time.sleep(1) time.sleep(1)
# 5. 状态检查（可选） # 5. 状态检查（可选）
status = self.get_all_device_status() status = self.get_all_device_status()
if status.get('conveyor1') and not status.get('pusher'): if status.get("conveyor1") and not status.get("pusher"):
print("流程完成1：皮带运行中，推板已收回") print("流程完成1：皮带运行中，推板已收回")
else: else:
print("状态异常，请检查设备") print("状态异常，请检查设备")
@ -306,6 +347,7 @@ class RelayController:
time.sleep(self.sensor1_error_delay) time.sleep(self.sensor1_error_delay)
def handle_sensor2(self): def handle_sensor2(self):
print("传感器线程12启动！！！")
while self._running: while self._running:
try: try:
if self.is_valid_sensor_status_1(self.SENSOR2): if self.is_valid_sensor_status_1(self.SENSOR2):
@ -357,7 +399,7 @@ class RelayController:
self._sensor1_thread.start() self._sensor1_thread.start()
if __name__ == '__main__': if __name__ == "__main__":
controller = RelayController() controller = RelayController()
controller.start() controller.start()
try: try:

1641
main.py
View File

File diff suppressed because it is too large Load Diff

12
ui_MainWin.py
View File

@ -451,7 +451,11 @@ class Ui_MainWindow(object):
sizePolicy5 = QSizePolicy( sizePolicy5 = QSizePolicy(
QSizePolicy.Policy.Minimum, QSizePolicy.Policy.Expanding QSizePolicy.Policy.Minimum, QSizePolicy.Policy.Expanding
) )
sizePolicy5.setHorizontalStretch(0) # sizePolicy5 = QSizePolicy(
# QSizePolicy.Policy.Expanding, # 水平方向
# QSizePolicy.Policy.Expanding, # 垂直方向
# )
sizePolicy5.setHorizontalStretch(0) # 设置水平扩展
sizePolicy5.setVerticalStretch(0) sizePolicy5.setVerticalStretch(0)
sizePolicy5.setHeightForWidth( sizePolicy5.setHeightForWidth(
self.pushButton_leftmenu_produce.sizePolicy().hasHeightForWidth() self.pushButton_leftmenu_produce.sizePolicy().hasHeightForWidth()
@ -4084,6 +4088,7 @@ class Ui_MainWindow(object):
self.gridLayout_2.setObjectName("gridLayout_2") self.gridLayout_2.setObjectName("gridLayout_2")
self.pushButton_reset = QPushButton(self.frame_21) self.pushButton_reset = QPushButton(self.frame_21)
self.pushButton_reset.setObjectName("pushButton_reset") self.pushButton_reset.setObjectName("pushButton_reset")
# self.pushButton_reset.setVisible(False) # 8/18 隐藏复位
sizePolicy5.setHeightForWidth( sizePolicy5.setHeightForWidth(
self.pushButton_reset.sizePolicy().hasHeightForWidth() self.pushButton_reset.sizePolicy().hasHeightForWidth()
) )
@ -4157,6 +4162,7 @@ class Ui_MainWindow(object):
sizePolicy5.setHeightForWidth( sizePolicy5.setHeightForWidth(
self.pushButton_pauseFeed.sizePolicy().hasHeightForWidth() self.pushButton_pauseFeed.sizePolicy().hasHeightForWidth()
) )
# self.pushButton_pauseFeed.setVisible(False) # 8/18 隐藏暂停
self.pushButton_pauseFeed.setSizePolicy(sizePolicy5) self.pushButton_pauseFeed.setSizePolicy(sizePolicy5)
self.pushButton_pauseFeed.setStyleSheet( self.pushButton_pauseFeed.setStyleSheet(
"*{\n" "*{\n"
@ -4221,6 +4227,8 @@ class Ui_MainWindow(object):
icon13 = QIcon(QIcon.fromTheme(QIcon.ThemeIcon.SystemReboot)) icon13 = QIcon(QIcon.fromTheme(QIcon.ThemeIcon.SystemReboot))
self.pushButton_clearAlarm.setIcon(icon13) self.pushButton_clearAlarm.setIcon(icon13)
# self.pushButton_clearAlarm.setVisible(False) # 8/18 隐藏 清除警报
self.gridLayout_2.addWidget(self.pushButton_clearAlarm, 3, 1, 1, 2) self.gridLayout_2.addWidget(self.pushButton_clearAlarm, 3, 1, 1, 2)
self.pushButton_onekeyfeed = QPushButton(self.frame_21) self.pushButton_onekeyfeed = QPushButton(self.frame_21)
@ -4254,6 +4262,8 @@ class Ui_MainWindow(object):
self.gridLayout_2.setRowStretch(2, 2) self.gridLayout_2.setRowStretch(2, 2)
self.gridLayout_2.setRowStretch(3, 2) self.gridLayout_2.setRowStretch(3, 2)
self.gridLayout_2.setColumnStretch(0, 1) self.gridLayout_2.setColumnStretch(0, 1)
# 8/18 第一列不分配空间（将第一列隐藏之后设置， 只保留启动，停止，急停）
# self.gridLayout_2.setColumnStretch(1, 0)
self.gridLayout_2.setColumnStretch(1, 1) self.gridLayout_2.setColumnStretch(1, 1)
self.gridLayout_2.setRowMinimumHeight(0, 2) self.gridLayout_2.setRowMinimumHeight(0, 2)
self.gridLayout_2.setRowMinimumHeight(1, 2) self.gridLayout_2.setRowMinimumHeight(1, 2)