源码
This commit is contained in:
琉璃月光
329
CU/Feeding.py
329
CU/Feeding.py
@ -27,6 +27,7 @@ from Util.util_time import CRisOrFall
|
||||
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
|
||||
@ -66,17 +67,16 @@ class FeedPosition:
|
||||
self.position = position
|
||||
|
||||
class FeedLine:
|
||||
def __init__(self, id, name, feed_positions:list,drop_positons:list):
|
||||
def __init__(self, id, name, feed_positions:list):
|
||||
self.feed_positions = copy.deepcopy(feed_positions)
|
||||
self.drop_positions = copy.deepcopy(drop_positons)
|
||||
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], ...]
|
||||
# --- 新增：用于存储从 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):
|
||||
@ -130,50 +130,41 @@ class FeedLine:
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
return self.feeding_to_end[i]
|
||||
|
||||
def get_drop_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):
|
||||
print("[调试] 开始设置抓取位置")
|
||||
print(f"传入的 position 坐标: X={position.X}, Y={position.Y}, Z={position.Z}, "
|
||||
f"a={position.a}, b={position.b}, c={position.c}, "
|
||||
f"U={position.U}, V={position.V}, W={position.W}")
|
||||
|
||||
"""
|
||||
设置 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)
|
||||
|
||||
print(f"[调试] getxyz1 返回值: X={xyz[0]}, Y={xyz[1]}, Z={xyz[2]}")
|
||||
|
||||
# 创建 before 和 after 位置
|
||||
befor_take_position = Real_Position().init_position(xyz[0],
|
||||
xyz[1],
|
||||
xyz[2],
|
||||
position.U,
|
||||
position.V,
|
||||
position.W)
|
||||
print(
|
||||
f"[调试] 抓取前位置: X={befor_take_position.X}, Y={befor_take_position.Y}, Z={befor_take_position.Z}, "
|
||||
f"U={befor_take_position.U}, V={befor_take_position.V}, W={befor_take_position.W}")
|
||||
|
||||
after_take_position = Real_Position().init_position(xyz[0],
|
||||
xyz[1],
|
||||
xyz[2],
|
||||
position.U,
|
||||
position.V,
|
||||
position.W)
|
||||
print(
|
||||
f"[调试] 抓取后位置: X={after_take_position.X}, Y={after_take_position.Y}, Z={after_take_position.Z}, "
|
||||
f"U={after_take_position.U}, V={after_take_position.V}, W={after_take_position.W}")
|
||||
|
||||
self.feeding_to_end[i - 1].set_position(befor_take_position)
|
||||
self.feeding_to_end[i + 1].set_position(after_take_position)
|
||||
# 安全检查索引
|
||||
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)
|
||||
|
||||
print(f"[调试] 当前抓取点已设置: X={position.X}, Y={position.Y}, Z={position.Z}, "
|
||||
f"U={position.U}, V={position.V}, W={position.W}")
|
||||
print("[调试] 抓取前后位置已设置完成")
|
||||
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):
|
||||
"""
|
||||
@ -214,14 +205,11 @@ class FeedLine:
|
||||
break
|
||||
|
||||
class FeedingConfig:
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs, drop_positions=None):
|
||||
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]
|
||||
|
||||
# --- 新增: 存储投料点列表 ---
|
||||
# 从UI界面读取 or 从txt文本读取
|
||||
self.drop_positions = [self.deal_photo_locs(p) for p in drop_positions] if drop_positions else []
|
||||
|
||||
def deal_photo_locs(self, photo_loc):
|
||||
position_photo = Real_Position()
|
||||
@ -246,7 +234,7 @@ class Feeding(QObject):
|
||||
self.relay_controller = RelayController()
|
||||
self.sensor_thread = None
|
||||
self.detection_image = None
|
||||
# self.init_detection_image()
|
||||
self.init_detection_image()
|
||||
self.pause = False
|
||||
self.cRis_photo = CRisOrFall()
|
||||
self.cRis_shake = CRisOrFall()
|
||||
@ -262,12 +250,12 @@ class Feeding(QObject):
|
||||
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.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.debug_run_count = 0 # 初始化计数器
|
||||
self.mid_take_count = 0
|
||||
#传感器判断抓包参数
|
||||
self.sensor2_ready = False # 传感器2是否检测到料包
|
||||
@ -280,28 +268,28 @@ class Feeding(QObject):
|
||||
|
||||
# --- 新增: 用于码垛模式的投料点索引 ---
|
||||
self.current_drop_index = 0
|
||||
|
||||
self.drop_manager = DropPositionManager("CU/drop.ini")
|
||||
pass
|
||||
|
||||
def close_feed(self):
|
||||
pass
|
||||
# self.is_detected = False
|
||||
# self.detect_thread.join()
|
||||
# self.detect.detection.release()
|
||||
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 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_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
|
||||
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,
|
||||
@ -324,8 +312,7 @@ class Feeding(QObject):
|
||||
if self.feedConfig == None:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
|
||||
if self.feedConfig != None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(
|
||||
real_position, is_action=True):
|
||||
if self.feedConfig !=None and self.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)
|
||||
@ -335,6 +322,16 @@ class Feeding(QObject):
|
||||
|
||||
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
|
||||
@ -361,10 +358,8 @@ class Feeding(QObject):
|
||||
return
|
||||
|
||||
self.feedConfig.feedLine.get_position_list()
|
||||
# self.detect.detect_status = DetectStatus.DNone
|
||||
self.detect.detect_status = DetectStatus.DNone
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
# --- 修改: 初始化投料点索引 ---
|
||||
self.current_drop_index = 0
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FMid:
|
||||
@ -380,22 +375,42 @@ class Feeding(QObject):
|
||||
logging.INFO,
|
||||
f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次"
|
||||
)
|
||||
if self.feedStatus == FeedStatus.FTake:
|
||||
self.catch.catch_status = CatchStatus.CTake
|
||||
#if self.feedStatus == FeedStatus.FTake:
|
||||
#self.catch.catch_status = CatchStatus.CTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
self.log_signal.emit(logging.INFO, "🟡 [码垛模式] 跳过拍照定位步骤")
|
||||
# 直接进入抓取状态或准备投料
|
||||
# 所以在 FPhoto 后，应该移动到抓取点
|
||||
take_position_model = self.feedConfig.feedLine.get_take_position()
|
||||
if take_position_model:
|
||||
self.log_signal.emit(logging.INFO, "[码垛模式] 准备移动到抓取点")
|
||||
self.sendTargPosition(take_position_model.get_position(), speed=self.robotClient.feed_speed)
|
||||
self.feedStatus = FeedStatus.FTake
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, "[码垛模式] 无法获取抓取点位置！")
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return # 退出当前循环
|
||||
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检测到料包到位，开始执行抓取")
|
||||
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)
|
||||
@ -407,49 +422,32 @@ class Feeding(QObject):
|
||||
self.log_signal.emit(logging.INFO, "🟡 机器人尚未到达抓料点位")
|
||||
return
|
||||
self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位")
|
||||
'''real_position'''
|
||||
# 一直等待传感器2信号，永不超时
|
||||
wait_start = time.time()
|
||||
timeout = 30 # 30秒超时，可根据需要调整
|
||||
sensor2_detected = False
|
||||
while not sensor2_detected:
|
||||
if time.time() - wait_start > timeout:
|
||||
self.log_signal.emit(logging.ERROR, "⏰ FTake: 等待传感器2超时!")
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return
|
||||
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检测到料包到位，开始执行抓取")
|
||||
sensor2_detected = True
|
||||
else:
|
||||
self.log_signal.emit(logging.INFO, "⏳ FTake: 等待传感器2料包信号...")
|
||||
time.sleep(0.2) # 每0.5秒检查一次
|
||||
# 执行抓取动作
|
||||
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)
|
||||
if not self.sensor2_ready:
|
||||
self.log_signal.emit(logging.INFO, "抓取完成，重新启动 conveyor2")
|
||||
self.relay_controller.open(conveyor2=True)
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
self.log_signal.emit(logging.INFO, "[码垛模式] 跳过视觉检测，准备投料")
|
||||
# 移动到第一个（或当前）投料点
|
||||
if self.feedConfig.drop_positions and len(self.feedConfig.drop_positions) > self.current_drop_index:
|
||||
first_drop_pos = self.feedConfig.drop_positions[self.current_drop_index]
|
||||
self.log_signal.emit(logging.INFO, f"[码垛模式] 移动到投料点 {self.current_drop_index}")
|
||||
self.sendTargPosition(first_drop_pos, speed=self.robotClient.feed_speed)
|
||||
self.feedStatus = FeedStatus.FDropBag
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, "[码垛模式] 投料点列表为空或索引错误！")
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return # 退出当前循环
|
||||
#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)
|
||||
print("next_drop_pos--------",next_drop_pos)
|
||||
if next_drop_pos:
|
||||
self.feedConfig.feedLine.set_drop_position(next_drop_pos)
|
||||
self.log_signal.emit(logging.INFO, f"已设置放置点: X={next_drop_pos.X:.2f}")
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
self.log_signal.emit(logging.ERROR, "获取放置点失败")
|
||||
return
|
||||
#self.feedConfig.feedLine.set_drop_position(real_position)#我想在这里读取我的一个ini文件值里面有很多个drop点，每一次索引递增的点
|
||||
self.relay_controller.open(clamp=True)
|
||||
self.next_position()
|
||||
#return
|
||||
#else:
|
||||
#self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
|
||||
@ -483,70 +481,48 @@ class Feeding(QObject):
|
||||
return
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
# 1. 确保配置了投料点
|
||||
if not self.feedConfig or not self.feedConfig.drop_positions:
|
||||
self.log_signal.emit(logging.ERROR, "[码垛模式] 错误：未配置投料点列表！")
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return
|
||||
#"""*** 处理投料点 (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)
|
||||
|
||||
# 2. 获取当前目标投料点 (基于索引)
|
||||
if self.current_drop_index >= len(self.feedConfig.drop_positions):
|
||||
self.log_signal.emit(logging.ERROR, f"[码垛模式] 错误：投料点索引 {self.current_drop_index} 超出范围！")
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
|
||||
# 3. 与 Catch 模块进行状态交互来驱动投料动作
|
||||
# a. 初始状态 (CNone): 触发投料动作
|
||||
|
||||
target_drop_position = self.feedConfig.drop_positions[self.current_drop_index]
|
||||
self.log_signal.emit(logging.INFO, f"[码垛模式] 当前目标投料点索引: {self.current_drop_index}")
|
||||
|
||||
# 3. 检查是否到达目标投料点
|
||||
if target_drop_position.compare(real_position, is_action=True):
|
||||
self.log_signal.emit(logging.INFO, f"[码垛模式] 到达投料点 {self.current_drop_index}")
|
||||
|
||||
# 4. 执行投料动作
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
# 将 Catch 状态设置为 CDrop，通知 Catch 模块开始执行物理投料动作
|
||||
self.catch.catch_status = CatchStatus.CDrop
|
||||
self.log_signal.emit(logging.INFO, "[码垛模式] 开始执行投料动作...")
|
||||
return # 等待下一周期检查 COk
|
||||
# 立即返回，等待 Catch 模块处理
|
||||
return
|
||||
|
||||
# b. 投料进行中 (CDrop): 等待完成
|
||||
|
||||
if self.catch.catch_status == CatchStatus.CDrop:
|
||||
self.log_signal.emit(logging.INFO, "[码垛模式] 投料动作进行中...")
|
||||
return # 等待抓取模块完成
|
||||
# 什么都不做，等待 Catch 模块完成动作并更新状态
|
||||
return
|
||||
# c. 投料完成 (COk): 处理后续逻辑并移动到下一个点
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
# 重置 Catch 状态，为下一次操作做准备
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
self.log_signal.emit(logging.INFO, f"[码垛模式] 在投料点 {self.current_drop_index} 投料完成")
|
||||
|
||||
# 5. 更新投料计数
|
||||
self.feedConfig.num -= 1
|
||||
# (后续增加) 延时: 让物料稳定
|
||||
# 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}')
|
||||
|
||||
# 6. 检查是否所有投料任务完成
|
||||
if self.feedConfig.num > 0:
|
||||
# 7. 更新投料点索引 (循环)
|
||||
self.current_drop_index = (self.current_drop_index + 1) % len(self.feedConfig.drop_positions)
|
||||
self.log_signal.emit(logging.INFO, f"[码垛模式] 更新投料点索引为: {self.current_drop_index}")
|
||||
|
||||
# 8. 移动到固定的抓取点
|
||||
take_position_model = self.feedConfig.feedLine.get_take_position()
|
||||
if take_position_model:
|
||||
self.log_signal.emit(logging.INFO, f"[码垛模式] 移动到固定抓取点...")
|
||||
self.sendTargPosition(take_position_model.get_position(), speed=self.robotClient.feed_speed)
|
||||
self.feedStatus = FeedStatus.FTake # 设置状态为 FTake
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, "[码垛模式] 无法获取抓取点位置！")
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
|
||||
else:
|
||||
# 9. 所有投料完成
|
||||
self.log_signal.emit(logging.INFO, "[码垛模式] 所有投料任务完成")
|
||||
# 可以选择返回原点或进入空闲状态
|
||||
# 例如，移动到原点
|
||||
# self.sendTargPosition(self.robotClient.origin_position, speed=self.robotClient.reset_speed)
|
||||
# self.feedStatus = FeedStatus.FStartReverse # 或者自定义一个结束状态
|
||||
self.feedStatus = FeedStatus.FNone # 简单地结束
|
||||
|
||||
else:
|
||||
# 如果还没到达目标投料点，可以打印日志或等待
|
||||
# (通常机器人移动指令发出后，会自动移动到位，下次循环再检查)
|
||||
self.log_signal.emit(logging.INFO, f"[码垛模式] 正在移动到投料点 {self.current_drop_index}...")
|
||||
# 5. *** 关键步骤 ***: 移动到路径中的下一个点
|
||||
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
|
||||
# 调用 next_Feed()。
|
||||
self.next_position()
|
||||
|
||||
def run_reset(self):
|
||||
real_position = Real_Position()
|
||||
@ -744,7 +720,18 @@ class Feeding(QObject):
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user