first commit
This commit is contained in:
98
CU/Catch.py
Normal file
98
CU/Catch.py
Normal file
@ -0,0 +1,98 @@
|
||||
#!/usr/bin/python3
|
||||
import time
|
||||
from enum import Enum
|
||||
|
||||
import Constant
|
||||
from COM.COM_Robot import RobotClient
|
||||
from Util.util_time import CClockPulse, CTon
|
||||
from .EMV import *
|
||||
|
||||
class CatchStatus(Enum):
|
||||
CNone = 0
|
||||
CTake = 1
|
||||
CDrop = 2
|
||||
CShake = 3
|
||||
COk = 4
|
||||
|
||||
class Catch:
|
||||
def __init__(self, robotClient: RobotClient):
|
||||
self.robotClient = robotClient
|
||||
self.catch_status = CatchStatus.CNone
|
||||
self.shake_continue = CTon()
|
||||
self.drop_continue = CTon()
|
||||
self.take_continue = CTon()
|
||||
self.shakePulse = CClockPulse()
|
||||
self.shake_count = 5
|
||||
self.drop_count = 2
|
||||
self.is_send_command = False#这里到时候可以改成True，然后加一个获取继电器状态变成False
|
||||
self.is_send_take_command = False
|
||||
self.shake_Q = False
|
||||
def run(self):
|
||||
if self.catch_status == CatchStatus.CNone:
|
||||
return
|
||||
|
||||
if self.catch_status == CatchStatus.CTake:
|
||||
# 这里可以添加继电器状态获取，把is_send_command = False
|
||||
if not self.is_send_take_command:
|
||||
# 本身IO
|
||||
#self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
|
||||
# 网络继电器
|
||||
close(1, 0, 0) #逻辑关闭电磁阀1
|
||||
self.is_send_take_command = True
|
||||
|
||||
if self.catch_status == CatchStatus.CDrop:
|
||||
if not self.is_send_command:
|
||||
# 本身IO
|
||||
# self.robotClient.sendIOControl(self.robotClient.con_ios[0], 0)
|
||||
# 网络继电器
|
||||
open(1, 0, 0)#逻辑
|
||||
#time.sleep(1)
|
||||
#for _ in range(self.drop_count):
|
||||
# self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1, delay=self.robotClient.time_delay_put)
|
||||
#open(0, 0, 1)
|
||||
#time.sleep(self.robotClient.time_delay_put) # 会造成这个时间点 其他命令插入不进去 需要另开线程
|
||||
#close(0, 0, 1)
|
||||
# self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0)
|
||||
|
||||
# self.robotClient.sendIOControl(self.robotClient.con_ios[1], 1)
|
||||
#close(0, 0, 1)
|
||||
self.is_send_command = True
|
||||
if self.drop_continue.Q(True,self.robotClient.time_delay_put*1000*self.drop_count):
|
||||
# if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
|
||||
self.is_send_command = False
|
||||
self.catch_status = CatchStatus.COk
|
||||
self.drop_continue.SetReset()
|
||||
|
||||
if self.catch_status == CatchStatus.CShake: # 1500
|
||||
self.shake_Q = not self.shake_Q # 10
|
||||
if not self.shake_continue.Q(True, 6000):
|
||||
#if self.shake_Q:
|
||||
#open(0, 1, 0)
|
||||
#else:
|
||||
#close(0, 1, 0)
|
||||
#else:
|
||||
self.shake_continue.SetReset()
|
||||
#self.catch_status = CatchStatus.COk
|
||||
#if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
|
||||
# self.robotClient.sendIOControl(self.robotClient.con_ios[2], 0)
|
||||
#close(0, 1, 0)
|
||||
print("震动结束")
|
||||
|
||||
if self.catch_status == CatchStatus.COk:
|
||||
self.shake_continue.SetReset()
|
||||
# self.robotClient.sendIOControl(self.robotClient.con_ios[1], 0,emptyList='1')
|
||||
open(1,0,0)#逻辑
|
||||
#close(0, 1, 0)
|
||||
#close(0, 0, 1)
|
||||
self.is_send_take_command = False
|
||||
pass
|
||||
|
||||
|
||||
def take_bag(self):
|
||||
return True
|
||||
|
||||
def press_bag(self):
|
||||
return True
|
||||
|
||||
def shake_bag(self):
|
||||
return True
|
||||
16
CU/Command.py
Normal file
16
CU/Command.py
Normal file
@ -0,0 +1,16 @@
|
||||
from enum import Enum
|
||||
class Status(Enum):
|
||||
Prepareing = 0
|
||||
Runing = 1
|
||||
End = 2
|
||||
|
||||
|
||||
class Command:
|
||||
def __init__(self):
|
||||
self.status = Status.Prepareing
|
||||
pass
|
||||
|
||||
class FeedCommand(Command):
|
||||
def __init__(self, feedingConfig):
|
||||
super().__init__()
|
||||
self.feed_config = feedingConfig
|
||||
86
CU/Detect.py
Normal file
86
CU/Detect.py
Normal file
@ -0,0 +1,86 @@
|
||||
from enum import Enum
|
||||
|
||||
import numpy as np
|
||||
from PySide6.QtCore import Signal
|
||||
import Constant
|
||||
from Model.Position import Real_Position
|
||||
from Trace.handeye_calibration import getPosition
|
||||
#from Vision.camera_coordinate_dete import Detection
|
||||
import configparser
|
||||
import os
|
||||
|
||||
class DetectStatus(Enum):
|
||||
DNone = 0
|
||||
DDetect = 1
|
||||
DOk = 2
|
||||
|
||||
class miDetect:
|
||||
update_detect_image = Signal(np.ndarray)
|
||||
def __init__(self):
|
||||
self.detection = Detection(alignmentType='depth2color')
|
||||
self.detect_status = DetectStatus.DNone
|
||||
self.detect_position = None
|
||||
def run(self):
|
||||
if self.detect_status == DetectStatus.DNone:
|
||||
return
|
||||
if self.detect_status == DetectStatus.DDetect:
|
||||
if Constant.Debug:
|
||||
self.detect_status = DetectStatus.DOk
|
||||
return
|
||||
_, img, xyz, uvw, points = self.detection.get_position(Point_isVision=False, Box_isPoint=True,
|
||||
First_Depth=True, Iter_Max_Pixel=30,
|
||||
save_img_point=1, Height_reduce=30, width_reduce=30)
|
||||
self.detection_image = img.copy()
|
||||
if xyz == None or uvw == None or points == None:
|
||||
self.detect_position = None
|
||||
self.detect_status = DetectStatus.DOk
|
||||
return
|
||||
target_position, noraml_base = getPosition(*xyz, *uvw, None, points)
|
||||
position = Real_Position().init_position(*target_position[:3], *noraml_base[:3])
|
||||
# position.Z = position.Z
|
||||
position.a = uvw[0]
|
||||
position.b = uvw[1]
|
||||
position.c = uvw[2]
|
||||
self.detect_position = position
|
||||
self.detect_status = DetectStatus.DOk
|
||||
if self.detect_status == DetectStatus.DOk:
|
||||
return
|
||||
|
||||
class Detect:
|
||||
update_detect_image = Signal(np.ndarray)
|
||||
|
||||
def __init__(self):
|
||||
self.detection = ""
|
||||
self.detect_status = DetectStatus.DOk
|
||||
self.detect_position = None
|
||||
self.position_index = 0 # 默认读取索引为 0 的点位
|
||||
|
||||
def run(self):
|
||||
if self.detect_status == DetectStatus.DOk:
|
||||
self.detect_position = None
|
||||
config = configparser.ConfigParser()
|
||||
config_file = os.path.join(os.path.dirname(__file__), 'list.ini') #配置文件地址
|
||||
if not os.path.exists(config_file):
|
||||
print("配置文件 list.ini 不存在")
|
||||
return
|
||||
config.read(config_file)
|
||||
if not config.has_section('positions'):
|
||||
print("配置文件中没有 [positions] 段")
|
||||
return
|
||||
if not config.has_option('positions', str(self.position_index)):
|
||||
print(f"没有索引为 {self.position_index} 的点位")
|
||||
return
|
||||
try:
|
||||
# 读取配置项
|
||||
data = config.get('positions', str(self.position_index)).strip().split(',')
|
||||
if len(data) != 6:
|
||||
raise ValueError(f"点位数据格式错误（应为6个值）: {data}")
|
||||
|
||||
x, y, z, a, b, c = map(float, data)
|
||||
# 初始化坐标
|
||||
self.detect_position = Real_Position()
|
||||
self.detect_position.init_position(x, y, z, a, b, c)
|
||||
#print(f"成功加载点位索引 {self.position_index}: ({x}, {y}, {z}, {a}, {b}, {c})")
|
||||
|
||||
except Exception as e:
|
||||
print(f"读取点位时出错: {e}")
|
||||
82
CU/EMV.py
Normal file
82
CU/EMV.py
Normal file
@ -0,0 +1,82 @@
|
||||
import socket
|
||||
import binascii
|
||||
import time
|
||||
|
||||
# 网络继电器的 IP 和端
|
||||
HOST = '192.168.0.18'
|
||||
PORT = 50000
|
||||
|
||||
# 电磁阀控制报文
|
||||
valve_commands = {
|
||||
1: {
|
||||
'open': '00000000000601050003FF00',#钢针
|
||||
'close': '000000000006010500030000',
|
||||
},
|
||||
2: {
|
||||
'open': '00000000000601050007FF00',
|
||||
'close': '000000000006010500070000',
|
||||
},
|
||||
3: {
|
||||
'open': '00000000000601050008FF00',
|
||||
'close': '000000000006010500080000',
|
||||
}
|
||||
}
|
||||
|
||||
# 将十六进制字符串转换为字节数据并发送
|
||||
def send_command(command):
|
||||
byte_data = binascii.unhexlify(command)
|
||||
# 创建套接字并连接到继电器
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
|
||||
try:
|
||||
sock.connect((HOST, PORT))
|
||||
sock.send(byte_data)
|
||||
# 接收响应
|
||||
response = sock.recv(1024)
|
||||
print(f"收到响应: {binascii.hexlify(response)}")
|
||||
# 校验响应
|
||||
if response == byte_data:
|
||||
# print("命令成功下发，继电器已执行操作。")
|
||||
return True
|
||||
else:
|
||||
print("命令下发失败，响应与请求不符。")
|
||||
return False
|
||||
except Exception as e:
|
||||
print(f"通信错误: {e}")
|
||||
return False
|
||||
|
||||
# 控制电磁阀打开
|
||||
def open(grasp, shake, throw):
|
||||
if grasp:
|
||||
print("打开电磁阀 1") # 钢针关闭
|
||||
if send_command(valve_commands[1]['open']):
|
||||
time.sleep(1)
|
||||
if shake:
|
||||
print("打开电磁阀 2") # 震动
|
||||
if send_command(valve_commands[2]['open']):
|
||||
time.sleep(0.05)
|
||||
if throw:
|
||||
print("打开电磁阀 3") # 扔袋
|
||||
if send_command(valve_commands[3]['open']):
|
||||
time.sleep(0.5)
|
||||
|
||||
# 控制电磁阀关闭
|
||||
def close(grasp, shake, throw):
|
||||
if grasp:
|
||||
print("关闭电磁阀 1")#钢针打开
|
||||
if send_command(valve_commands[1]['close']):
|
||||
time.sleep(1)
|
||||
if shake:
|
||||
# print("关闭电磁阀 2")
|
||||
if send_command(valve_commands[2]['close']):
|
||||
time.sleep(0.05)
|
||||
if throw:
|
||||
print("关闭电磁阀 3")
|
||||
if send_command(valve_commands[3]['close']):
|
||||
time.sleep(0.5)
|
||||
|
||||
# 关闭电磁阀
|
||||
# open(False, False, True) # 参数传True和False
|
||||
# close(True,False,True)
|
||||
# for i in range(10):
|
||||
# open(False,True,True)
|
||||
# close(True,True,True)
|
||||
843
CU/Feeding.py
Normal file
843
CU/Feeding.py
Normal file
@ -0,0 +1,843 @@
|
||||
import copy
|
||||
import logging
|
||||
import random
|
||||
import threading
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from PyQt5.QtWidgets import QMessageBox
|
||||
from PySide6.QtCore import Signal, QObject
|
||||
from PySide6.QtGui import QPixmap
|
||||
|
||||
import Constant
|
||||
import Expection
|
||||
from CU.Catch import Catch, CatchStatus
|
||||
from CU.Detect import Detect, DetectStatus
|
||||
from Model.FeedModel import PositionModel
|
||||
from Model.Position import Real_Position, Detection_Position
|
||||
from enum import Enum, IntEnum
|
||||
from COM.COM_Robot import RobotClient, DetectType
|
||||
from Model.RobotModel import CMDInstructRequest, MoveType
|
||||
from Trace.handeye_calibration import getPosition
|
||||
from Trace.handeye_calibration import getxyz,getxyz1
|
||||
from Util.util_math import get_distance
|
||||
from Util.util_time import CRisOrFall
|
||||
#from Vision.camera_coordinate_dete import Detection
|
||||
from Util.util_log import log
|
||||
from Model.RobotModel import Instruction
|
||||
from EMV.EMV import RelayController
|
||||
class ResetStatus(Enum):
|
||||
RNone = 0
|
||||
RStart = 1
|
||||
RRunging = 2
|
||||
ROk =3
|
||||
|
||||
class FeedStatus(IntEnum):
|
||||
FNone = 0
|
||||
FStart = 1
|
||||
FCheck = 2
|
||||
FMid = 3
|
||||
FPhoto = 4
|
||||
FTake = 5
|
||||
FBroken1 = 6
|
||||
FBroken2 =7
|
||||
FShake = 8
|
||||
FDropBag = 9
|
||||
FFinished = 10
|
||||
FReverse = 11
|
||||
FStartReverse = 12
|
||||
|
||||
class LineType(Enum):
|
||||
Straight = 0
|
||||
CureMid = 2
|
||||
CureEnd = 3
|
||||
WORLD = 4
|
||||
|
||||
|
||||
class FeedMidStatus(Enum):
|
||||
FMid_Start = 1
|
||||
FMid_Take = 2
|
||||
FMid_Feed= 3
|
||||
|
||||
class FeedPosition:
|
||||
def __init__(self,status:FeedStatus,position:Real_Position):
|
||||
self.status = status
|
||||
self.position = position
|
||||
|
||||
class FeedLine:
|
||||
def __init__(self, id, name, feed_positions:list,drop_positons: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], ...]
|
||||
self.drop_point_list = []
|
||||
|
||||
def get_current_feed_position(self,is_reverse):
|
||||
pos = self.feeding_to_end[ (self.feeding2end_pos_index+1)%len(self.feeding_to_end) if is_reverse else self.feeding2end_pos_index-1]
|
||||
return pos
|
||||
def get_current_take_position(self,is_reverse):
|
||||
pos = self.start_to_take[ (self.start2take_pos_index+1)%len(self.start_to_take) if is_reverse else self.start2take_pos_index-1]
|
||||
return pos
|
||||
def get_current_start_position(self,is_reverse):
|
||||
pos = self.origin_to_start[(self.origin2start_pos_index+1)%len(self.origin_to_start) if is_reverse else self.origin2start_pos_index-1]
|
||||
return pos
|
||||
|
||||
def get_next_feed_position(self,reverse:bool=False):
|
||||
pos = self.feeding_to_end[self.feeding2end_pos_index]
|
||||
if reverse:
|
||||
self.feeding2end_pos_index -= 1
|
||||
if self.feeding2end_pos_index < 0:
|
||||
self.feeding2end_pos_index = len(self.feeding_to_end) - 1
|
||||
else:
|
||||
self.feeding2end_pos_index += 1
|
||||
if self.feeding2end_pos_index >= len(self.feeding_to_end):
|
||||
self.feeding2end_pos_index = 0
|
||||
return pos
|
||||
|
||||
def get_next_start_position(self,reverse:bool=False):
|
||||
pos = self.origin_to_start[self.origin2start_pos_index]
|
||||
if reverse:
|
||||
self.origin2start_pos_index -= 1
|
||||
if self.origin2start_pos_index < 0:
|
||||
self.origin2start_pos_index = len(self.origin_to_start) - 1
|
||||
else:
|
||||
self.origin2start_pos_index += 1
|
||||
if self.origin2start_pos_index >= len(self.origin_to_start):
|
||||
self.origin2start_pos_index = 0
|
||||
|
||||
return pos
|
||||
|
||||
def get_next_take_position(self,reverse:bool=False):
|
||||
pos = self.start_to_take[self.start2take_pos_index]
|
||||
if reverse:
|
||||
self.start2take_pos_index -= 1
|
||||
if self.start2take_pos_index < 0:
|
||||
self.start2take_pos_index = len(self.start_to_take) - 1
|
||||
else:
|
||||
self.start2take_pos_index += 1
|
||||
if self.start2take_pos_index >= len(self.start_to_take):
|
||||
self.start2take_pos_index = 0
|
||||
return pos
|
||||
def get_take_position(self):
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
return self.feeding_to_end[i]
|
||||
|
||||
def 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}")
|
||||
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
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]}")
|
||||
|
||||
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)
|
||||
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("[调试] 抓取前后位置已设置完成")
|
||||
|
||||
def set_drop_position(self, position: Real_Position):
|
||||
"""
|
||||
设置 FDropBag 位置，只设置当前点，不处理前后点。
|
||||
:param position: 新的丢包位置
|
||||
"""
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FDropBag.value:
|
||||
# 直接设置当前点的位置
|
||||
self.feeding_to_end[i].set_position(position)
|
||||
print(
|
||||
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})")
|
||||
break # 假设只有一个丢包点
|
||||
|
||||
def get_position_list(self):
|
||||
index_start = -1
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FCheck.value:
|
||||
index_start = i
|
||||
break
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
|
||||
index_take = i
|
||||
|
||||
self.origin_to_start = self.feed_positions[: index_start+1]
|
||||
self.start_to_take = self.feed_positions[index_start:index_take+1]
|
||||
self.feeding_to_end = self.feed_positions[index_take:]
|
||||
|
||||
for i in range(len(self.feeding_to_end)): #插入动态中间点
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
befor_position_model = PositionModel()
|
||||
befor_position_model.init_position(None)
|
||||
after_position_model = PositionModel()
|
||||
after_position_model.init_position(None)
|
||||
|
||||
self.feeding_to_end.insert(i, befor_position_model)
|
||||
self.feeding_to_end.insert(i+2, after_position_model)
|
||||
break
|
||||
|
||||
class FeedingConfig:
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs, drop_positions=None):
|
||||
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()
|
||||
position_photo.init_position(photo_loc[0], photo_loc[1], photo_loc[2], photo_loc[3], photo_loc[4], photo_loc[5])
|
||||
return position_photo
|
||||
|
||||
def get_line_info(self):
|
||||
pass
|
||||
|
||||
|
||||
class Feeding(QObject):
|
||||
need_origin_signal = Signal(str)
|
||||
take_no_photo_sigal = Signal()
|
||||
update_detect_image = Signal(np.ndarray)
|
||||
log_signal = Signal(int,str)
|
||||
def __init__(self, robotClient: RobotClient):
|
||||
super().__init__()
|
||||
self.feedConfig = None
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.robotClient = robotClient
|
||||
# 添加 RelayController 实例
|
||||
self.relay_controller = RelayController()
|
||||
self.sensor_thread = None
|
||||
self.detection_image = None
|
||||
# self.init_detection_image()
|
||||
self.pause = False
|
||||
self.cRis_photo = CRisOrFall()
|
||||
self.cRis_shake = CRisOrFall()
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
self.is_reverse = False
|
||||
# 复位集合
|
||||
self.run_reverse = False
|
||||
self.take_no_photo = False
|
||||
self.reset_status = ResetStatus.RNone
|
||||
self.reversed_positions = []
|
||||
self.current_position = None
|
||||
self.index=1
|
||||
self.pos_index = -1
|
||||
self.pos_near_index = -1
|
||||
self.catch = Catch(self.robotClient)
|
||||
# self.detect = Detect()
|
||||
# self.is_detected = True
|
||||
# self.detect_thread = threading.Thread(target=self.run_detect)
|
||||
# self.detect_thread.start()
|
||||
self.onekey = False
|
||||
# self.debug_run_count = 0 # 初始化计数器
|
||||
self.mid_take_count = 0
|
||||
#传感器判断抓包参数
|
||||
self.sensor2_ready = False # 传感器2是否检测到料包
|
||||
self.motor_stopped_by_sensor2 = False # 是否由传感器2触发停止电机
|
||||
self.sensor_thread = None
|
||||
self.relay_controller = RelayController()
|
||||
# 启动传感器2线程
|
||||
self.relay_controller._running = True
|
||||
self.sensor2_thread = None
|
||||
|
||||
# --- 新增: 用于码垛模式的投料点索引 ---
|
||||
self.current_drop_index = 0
|
||||
|
||||
|
||||
def close_feed(self):
|
||||
pass
|
||||
# self.is_detected = False
|
||||
# self.detect_thread.join()
|
||||
# self.detect.detection.release()
|
||||
|
||||
# def init_detection_image(self):
|
||||
# detection_image = cv2.imread(Constant.feed_sign_path)
|
||||
# self.update_detect_image.emit(detection_image)
|
||||
|
||||
# def run_detect(self):
|
||||
# while self.is_detected:
|
||||
# self.detect.run()
|
||||
# time.sleep(0.02)
|
||||
|
||||
def run(self):
|
||||
self.catch.run()
|
||||
# 获取事件坐标
|
||||
real_position = Real_Position()
|
||||
# self.detect.position_index = 0
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
# real_position.init_position(0,
|
||||
# 0,
|
||||
# 0,
|
||||
# 0,
|
||||
# 0,
|
||||
# 0);
|
||||
|
||||
# img_path = f'Image/{self.index}.png'
|
||||
# img = cv2.imread(img_path)
|
||||
# self.index += 1
|
||||
# self.index = self.index % 4
|
||||
# self.detection_image = img
|
||||
|
||||
if self.feedConfig == None:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
|
||||
if self.feedConfig != None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(
|
||||
real_position, is_action=True):
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.is_reverse = False
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
|
||||
|
||||
if self.feedStatus == FeedStatus.FNone or self.pause:
|
||||
return
|
||||
|
||||
elif self.feedStatus == FeedStatus.FCheck:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_check)
|
||||
|
||||
if self.is_reverse:
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
else:
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
|
||||
if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position):
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FStart:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_start)
|
||||
self.relay_controller.open(conveyor2=True)#开电机
|
||||
#self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢
|
||||
#self.sensor2_thread.start()
|
||||
if not self.robotClient.origin_position.compare(real_position,is_action=True) and not self.is_reverse:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error)
|
||||
self.need_origin_signal.emit(Constant.str_feed_start_error)
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return
|
||||
if self.is_reverse:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.is_reverse = False
|
||||
return
|
||||
|
||||
self.feedConfig.feedLine.get_position_list()
|
||||
# self.detect.detect_status = DetectStatus.DNone
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
# --- 修改: 初始化投料点索引 ---
|
||||
self.current_drop_index = 0
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FMid:
|
||||
feed_pos = self.get_current_position(self.is_reverse)
|
||||
if feed_pos.get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
|
||||
self.next_position(self.is_reverse)
|
||||
# 增加计数器逻辑
|
||||
self.mid_take_count += 1
|
||||
# 可选：在 Debug1 模式下输出日志
|
||||
if Constant.Debug1:
|
||||
self.log_signal.emit(
|
||||
logging.INFO,
|
||||
f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次"
|
||||
)
|
||||
if self.feedStatus == FeedStatus.FTake:
|
||||
self.catch.catch_status = CatchStatus.CTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
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 # 退出当前循环
|
||||
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_take)
|
||||
take_position = self.feedConfig.feedLine.get_take_position()
|
||||
if not take_position or not take_position.get_position():
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
return
|
||||
if not take_position.get_position().compare(real_position, is_action=True):
|
||||
self.log_signal.emit(logging.INFO, "🟡 机器人尚未到达抓料点位")
|
||||
return
|
||||
self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位")
|
||||
'''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 # 退出当前循环
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken2:
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FShake:
|
||||
if self.get_current_position().get_position().compare(real_position,is_action=True):
|
||||
# TODO 震动方案
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
self.catch.catch_status = CatchStatus.CShake
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.CShake:
|
||||
# if self.feedConfig.feedLine.feeding_to_end[
|
||||
# self.feedConfig.feedLine.feeding2end_pos_index + 1].status != FeedStatus.FShake:
|
||||
# self.catch.catch_status = CatchStatus.COk
|
||||
# else:
|
||||
self.catch.shake_continue.SetReset()
|
||||
self.next_position()
|
||||
if self.feedStatus!=FeedStatus.FShake:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
return
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
# 1. 确保配置了投料点
|
||||
if not self.feedConfig or not self.feedConfig.drop_positions:
|
||||
self.log_signal.emit(logging.ERROR, "[码垛模式] 错误：未配置投料点列表！")
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return
|
||||
|
||||
# 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
|
||||
|
||||
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:
|
||||
self.catch.catch_status = CatchStatus.CDrop
|
||||
self.log_signal.emit(logging.INFO, "[码垛模式] 开始执行投料动作...")
|
||||
return # 等待下一周期检查 COk
|
||||
if self.catch.catch_status == CatchStatus.CDrop:
|
||||
self.log_signal.emit(logging.INFO, "[码垛模式] 投料动作进行中...")
|
||||
return # 等待抓取模块完成
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
self.log_signal.emit(logging.INFO, f"[码垛模式] 在投料点 {self.current_drop_index} 投料完成")
|
||||
|
||||
# 5. 更新投料计数
|
||||
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}...")
|
||||
|
||||
def run_reset(self):
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
if self.reset_status == ResetStatus.RNone:
|
||||
return
|
||||
|
||||
if self.reset_status == ResetStatus.RStart:
|
||||
if self.feedConfig == None: return
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
# if self.feedConfig.feedLine.positions[index].status == 2:
|
||||
# start_index = index
|
||||
self.pos_index = -1
|
||||
self.pos_near_index = -1
|
||||
self.reversed_positions = []
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position,is_action=True):
|
||||
self.pos_index = index
|
||||
break
|
||||
|
||||
if self.pos_index == -1:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
|
||||
min_distance = 99999999
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if get_distance(pos_model.get_position(), real_position) < min_distance:
|
||||
min_distance = get_distance(pos_model.get_position(), real_position)
|
||||
self.pos_near_index = index
|
||||
if self.pos_near_index != -1:
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1]
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index+1]
|
||||
self.reversed_positions = list(reversed(self.reversed_positions))
|
||||
self.reverse_index = 0
|
||||
self.send_emergency_sound()
|
||||
self.current_position = PositionModel()
|
||||
self.current_position.init_position(real_position)
|
||||
self.reset_status = ResetStatus.RRunging
|
||||
|
||||
if self.reset_status == ResetStatus.RRunging:
|
||||
if not real_position.compare(self.current_position.get_position(),is_action=True):
|
||||
return
|
||||
pos_model = self.reversed_positions[self.reverse_index]
|
||||
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
|
||||
pos_model = self.reversed_positions[self.reverse_index + 1]
|
||||
self.reverse_index = self.reverse_index+1
|
||||
|
||||
if pos_model.lineType == LineType.CureMid.value:
|
||||
pos_model1 = self.reversed_positions[self.reverse_index + 1]
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index + 2
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
|
||||
if self.reverse_index == len(self.reversed_positions):
|
||||
self.reset_status = ResetStatus.ROk
|
||||
if self.reset_status == ResetStatus.ROk:
|
||||
self.reset_status = ResetStatus.RNone
|
||||
self.send_emergency_stop()
|
||||
|
||||
def return_original_position(self):
|
||||
|
||||
self.run_reverse = True
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
if self.feedConfig == None: return
|
||||
start_index = -1
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
# if self.feedConfig.feedLine.positions[index].status == 2:
|
||||
# start_index = index
|
||||
pos_index = -1
|
||||
pos_near_index = -1
|
||||
reversed_positions = []
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position):
|
||||
pos_index = index
|
||||
break
|
||||
|
||||
if pos_index == -1:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
|
||||
min_distance = 99999999
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if get_distance(pos_model.get_position(), real_position)<min_distance:
|
||||
min_distance = get_distance(pos_model.get_position(), real_position)
|
||||
pos_near_index = index
|
||||
if pos_near_index != -1:
|
||||
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_near_index+1]
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_index]
|
||||
reversed_positions = list(reversed(reversed_positions))
|
||||
self.reverse_index = 0
|
||||
self.send_emergency_sound()
|
||||
current_position = PositionModel()
|
||||
current_position.init_position(real_position)
|
||||
while self.run_reverse and self.reverse_index!=len(reversed_positions):
|
||||
if self.reverse_index!=0 and not real_position.compare(current_position.get_position()):
|
||||
continue
|
||||
#todo 缺少比对
|
||||
pos_model = reversed_positions[self.reverse_index]
|
||||
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
|
||||
|
||||
pos_model = reversed_positions[self.reverse_index + 1]
|
||||
# TODO take节点判断
|
||||
|
||||
if pos_model.lineType == LineType.CureMid.value:
|
||||
pos_model1 = reversed_positions[self.reverse_index + 1]
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(),speed= self.robotClient.reset_speed)
|
||||
current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index+2
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(),speed=self.robotClient.reset_speed)
|
||||
current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
self.send_emergency_stop()
|
||||
return True
|
||||
def send_emergency_sound(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 1)
|
||||
|
||||
def send_emergency_stop(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 0)
|
||||
|
||||
def sendIOControl(self, IO_bit, IO_Status: int):
|
||||
|
||||
IO_command = CMDInstructRequest()
|
||||
io_instruction = Instruction()
|
||||
io_instruction.IO = True
|
||||
io_instruction.io_status = IO_Status
|
||||
io_instruction.point = IO_bit # {"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
|
||||
IO_command.dsID = 'HCRemoteCommand'
|
||||
IO_command.instructions.append(io_instruction)
|
||||
self.robotClient.add_sendQuene(IO_command.toString())
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
|
||||
pass
|
||||
|
||||
def sendTargPosition1(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
|
||||
pass
|
||||
|
||||
|
||||
def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
|
||||
position_instruction = Instruction()
|
||||
position_instruction.speed = speed
|
||||
position_instruction.m0 = real_position.X
|
||||
position_instruction.m1 = real_position.Y
|
||||
position_instruction.m2 = real_position.Z
|
||||
position_instruction.m3 = real_position.U
|
||||
position_instruction.m4 = real_position.V
|
||||
position_instruction.m5 = real_position.W
|
||||
position_instruction.smooth = self.robotClient.smooth
|
||||
|
||||
position_instruction.action = move_type.value
|
||||
if position_instruction.action == 17:
|
||||
position_instruction.m0_p = real_position1.X
|
||||
position_instruction.m1_p = real_position1.Y
|
||||
position_instruction.m2_p = real_position1.Z
|
||||
position_instruction.m3_p = real_position1.U
|
||||
position_instruction.m4_p = real_position1.V
|
||||
position_instruction.m5_p = real_position1.W
|
||||
instruction_command = CMDInstructRequest()
|
||||
instruction_command.instructions.append(position_instruction)
|
||||
request_command = instruction_command.toString()
|
||||
|
||||
log_str = f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：' \
|
||||
f'X:{position_instruction.m0}-' \
|
||||
f'Y:{position_instruction.m1}-' \
|
||||
f'Z:{position_instruction.m2}-' \
|
||||
f'U:{position_instruction.m3}-' \
|
||||
f'V:{position_instruction.m4}-' \
|
||||
f'W:{position_instruction.m5}'
|
||||
|
||||
|
||||
try:
|
||||
print('fuck1',log_str)
|
||||
self.log_signal.emit(logging.INFO, log_str)
|
||||
print('fuck2',log_str)
|
||||
except:
|
||||
return
|
||||
print(request_command)
|
||||
self.robotClient.add_sendQuene(request_command)
|
||||
pass
|
||||
|
||||
def next_start(self,reverse=False):
|
||||
start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
if start_pos.lineType == LineType.CureMid.value:
|
||||
start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
self.feedStatus = FeedStatus(start_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed, move_type=MoveType.Cure, real_position1=start_pos1.get_position())
|
||||
elif start_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(start_pos.get_position(),self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed,move_type=MoveType.AXIS)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed)
|
||||
pass
|
||||
|
||||
def next_take(self,reverse=False):
|
||||
|
||||
take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
if take_pos.lineType == LineType.CureMid.value:
|
||||
take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position(),speed=self.robotClient.feed_speed)
|
||||
elif take_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(take_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
pass
|
||||
|
||||
def next_Feed(self,reverse=False):
|
||||
feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
if feed_pos.lineType == LineType.CureMid.value:
|
||||
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position(),speed=self.robotClient.feed_speed)
|
||||
elif feed_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
# if not reverse and self.feedStatus == FeedStatus.FShake:
|
||||
# if not reverse:
|
||||
# if self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, True):
|
||||
# pass
|
||||
# elif self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, False):
|
||||
# self.feedStatus = FeedStatus.FShake
|
||||
# self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
# return
|
||||
|
||||
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
|
||||
def get_current_position(self,is_reverse=False):
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
return self.feedConfig.feedLine.get_current_start_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
return self.feedConfig.feedLine.get_current_take_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
return self.feedConfig.feedLine.get_current_feed_position(self.is_reverse)
|
||||
def next_position(self,reverse=False):
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
self.next_start(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_start')
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
self.next_take(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_take')
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
self.next_Feed(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_feed')
|
||||
|
||||
def safe_check_columns(self):
|
||||
return True
|
||||
pass
|
||||
|
||||
def safe_check_person(self):
|
||||
return True
|
||||
pass
|
||||
|
||||
474
CU/Feeding_C.py
Normal file
474
CU/Feeding_C.py
Normal file
@ -0,0 +1,474 @@
|
||||
# import logging
|
||||
# import time
|
||||
#
|
||||
# import cv2
|
||||
# from PyQt5.QtWidgets import QMessageBox
|
||||
#
|
||||
# import Constant
|
||||
# import Expection
|
||||
# from Model.Position import Real_Position, Detection_Position
|
||||
# from enum import Enum, IntEnum
|
||||
# from COM.COM_Robot import RobotClient, DetectType
|
||||
# from Model.RobotModel import CMDInstructRequest, MoveType
|
||||
# from Util.util_time import CRisOrFall
|
||||
# from Vision.camera_coordinate_dete import Detection
|
||||
# from Util.util_log import log
|
||||
# from Model.RobotModel import Instruction
|
||||
#
|
||||
#
|
||||
#
|
||||
#
|
||||
# class FeedStatus(IntEnum):
|
||||
# FNone = 0
|
||||
# FStart = 1
|
||||
# FCheck = 2
|
||||
# FMid = 3
|
||||
# FPhoto = 4
|
||||
# FTake = 5
|
||||
# FBroken1 = 6
|
||||
# FBroken2 =7
|
||||
# FShake = 8
|
||||
# FDropBag = 9
|
||||
# FFinished = 10
|
||||
# FReverse = 11
|
||||
# FStartReverse = 12
|
||||
#
|
||||
# class LineType(Enum):
|
||||
# Straight = 0
|
||||
# CureStart = 1
|
||||
# CureMid = 2
|
||||
# CureEnd = 3
|
||||
#
|
||||
#
|
||||
# class FeedMidStatus(Enum):
|
||||
# FMid_Start = 1
|
||||
# FMid_Take = 2
|
||||
# FMid_Feed= 3
|
||||
#
|
||||
# class FeedPosition:
|
||||
# def __init__(self,status:FeedStatus,position:Real_Position):
|
||||
# self.status = status
|
||||
# self.position = position
|
||||
#
|
||||
# class FeedLine:
|
||||
# def __init__(self, id, name, feed_positions:list):
|
||||
# self.feed_positions = feed_positions
|
||||
# self.feeding2end_pos_index = 0
|
||||
# self.origin2start_pos_index = 0
|
||||
# self.start2take_pos_index = 0
|
||||
# self.name = name
|
||||
# self.id = id
|
||||
# self.get_position_list()
|
||||
#
|
||||
# def get_current_feed_position(self):
|
||||
# pos = self.feeding_to_end[self.feeding2end_pos_index]
|
||||
# return pos
|
||||
# def get_current_take_position(self):
|
||||
# pos = self.start2take_pos_index[self.start2take_pos_index]
|
||||
# return pos
|
||||
# def get_current_start_position(self):
|
||||
# pos = self.origin2start_pos_index[self.origin2start_pos_index]
|
||||
# return pos
|
||||
#
|
||||
# def get_next_feed_position(self,reverse:bool=False):
|
||||
# pos = self.feeding_to_end[self.feeding2end_pos_index]
|
||||
# if reverse:
|
||||
# self.feeding2end_pos_index -= 1
|
||||
# if self.feeding2end_pos_index < 0:
|
||||
# self.feeding2end_pos_index = len(self.feeding_to_end) - 1
|
||||
# else:
|
||||
# self.feeding2end_pos_index += 1
|
||||
# if self.feeding2end_pos_index >= len(self.feeding_to_end):
|
||||
# self.feeding2end_pos_index = 0
|
||||
# return pos
|
||||
#
|
||||
#
|
||||
# def get_next_start_position(self,reverse:bool=False):
|
||||
# pos = self.origin2start_pos_index[self.origin2start_pos_index]
|
||||
# if reverse:
|
||||
# self.origin2start_pos_index -= 1
|
||||
# if self.origin2start_pos_index < 0:
|
||||
# self.origin2start_pos_index = len(self.origin2start_pos) - 1
|
||||
# else:
|
||||
# self.origin2start_pos_index += 1
|
||||
# if self.origin2start_pos_index >= len(self.origin2start_pos):
|
||||
# self.origin2start_pos_index = 0
|
||||
#
|
||||
# return pos
|
||||
#
|
||||
# def get_next_take_position(self,reverse:bool=False):
|
||||
# pos = self.start2take_pos_index[self.start2take_pos_index]
|
||||
# if reverse:
|
||||
# self.start2take_pos_index -= 1
|
||||
# if self.start2take_pos_index < 0:
|
||||
# self.start2take_pos_index = len(self.start2take_pos) - 1
|
||||
# else:
|
||||
# self.start2take_pos_index += 1
|
||||
# if self.start2take_pos_index >= len(self.start2take_pos):
|
||||
# self.start2take_pos_index = 0
|
||||
# return pos
|
||||
#
|
||||
# def get_take_position(self):
|
||||
# for i in range(len(self.feeding_to_end)):
|
||||
# if self.feeding_to_end[i].status == FeedStatus.FTake:
|
||||
# return self.feeding_to_end[i]
|
||||
#
|
||||
# def set_take_position(self,position:Real_Position):
|
||||
# for i in range(len(self.feeding_to_end)):
|
||||
# if self.feeding_to_end[i].status == FeedStatus.FTake:
|
||||
# self.feeding_to_end[i].position = position
|
||||
#
|
||||
# def get_position_list(self):
|
||||
# index_start = -1
|
||||
# for i in range(len(self.feed_positions)):
|
||||
# if self.feed_positions[i].status == FeedStatus.FStart:
|
||||
# index_start = i
|
||||
# break
|
||||
# for i in range(len(self.feed_positions)):
|
||||
# if self.feed_positions[i].status == FeedStatus.FPhoto:
|
||||
# index_take = i
|
||||
#
|
||||
# self.origin_to_start = self.feed_positions[: index_start+1]
|
||||
# self.start_to_take = self.feed_positions[index_start:index_take+1]
|
||||
# self.feeding_to_end = self.feed_positions[index_take:]
|
||||
#
|
||||
#
|
||||
#
|
||||
#
|
||||
# class FeedingConfig:
|
||||
# def __init__(self, num: int, feedLine: FeedLine, photo_locs):
|
||||
# self.num = num
|
||||
# self.feedLine = feedLine
|
||||
# self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
|
||||
#
|
||||
# def deal_photo_locs(self, photo_loc):
|
||||
# position_photo = Real_Position()
|
||||
# position_photo.init_position(photo_loc[0], photo_loc[1], photo_loc[2], photo_loc[3], photo_loc[4], photo_loc[5])
|
||||
# return position_photo
|
||||
#
|
||||
# def get_line_info(self):
|
||||
# pass
|
||||
#
|
||||
#
|
||||
# class Feeding:
|
||||
# def __init__(self, robotClient: RobotClient, detection: Detection):
|
||||
# self.feedConfig = None
|
||||
# self.feedStatus = FeedStatus.FNone
|
||||
# self.robotClient = robotClient
|
||||
# self.detection = detection
|
||||
# self.detection_image = None
|
||||
# self.init_detection_image()
|
||||
# self.pause = False
|
||||
# self.cRis_photo = CRisOrFall()
|
||||
# self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
# self.is_reverse = False
|
||||
# pass
|
||||
#
|
||||
# def init_detection_image(self):
|
||||
# self.detection_image = cv2.imread(Constant.feed_sign_path)
|
||||
#
|
||||
# def run(self):
|
||||
# # 获取事件坐标
|
||||
# real_position = Real_Position()
|
||||
# real_position.init_position(self.robotClient.status_model.world_0,
|
||||
# self.robotClient.status_model.world_1,
|
||||
# self.robotClient.status_model.world_2,
|
||||
# self.robotClient.status_model.world_3,
|
||||
# self.robotClient.status_model.world_4,
|
||||
# self.robotClient.status_model.world_5)
|
||||
# # real_position.init_position(0,
|
||||
# # 0,
|
||||
# # 0,
|
||||
# # 0,
|
||||
# # 0,
|
||||
# # 0);
|
||||
#
|
||||
# if self.feedConfig == None:
|
||||
# self.feedStatus = FeedStatus.FNone
|
||||
# elif self.feedConfig.num == 0 and self.feedStatus!=FeedStatus.FReverse:
|
||||
# self.feedStatus = FeedStatus.FNone
|
||||
#
|
||||
# if self.feedStatus == FeedStatus.FNone or self.pause:
|
||||
# return
|
||||
# elif self.feedStatus == FeedStatus.FCheck:
|
||||
# log.log_message(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_safe_error_msgbox)
|
||||
# if self.is_reverse:
|
||||
# self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
# else:
|
||||
# self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
# self.next_position(self.is_reverse)
|
||||
#
|
||||
# elif self.feedStatus == FeedStatus.FStart:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_start)
|
||||
# if not self.robotClient.get_origin_position().compare(real_position) and not self.is_reverse:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_start_error)
|
||||
# self.feedStatus = FeedStatus.FNone
|
||||
# if self.is_reverse and self.robotClient.get_origin_position().compare(real_position):
|
||||
# self.feedStatus = FeedStatus.FNone
|
||||
# self.is_reverse = False
|
||||
#
|
||||
# self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
# self.next_position(self.is_reverse)
|
||||
#
|
||||
#
|
||||
# elif self.feedStatus == FeedStatus.FMid:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_mid)
|
||||
# feed_pos = self.get_current_position()
|
||||
# if feed_pos.position.compare(real_position):
|
||||
# self.next_position(self.is_reverse)
|
||||
#
|
||||
#
|
||||
#
|
||||
# elif self.feedStatus == FeedStatus.FPhoto:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_photo)
|
||||
# if self.feedConfig.num == 0:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_finish)
|
||||
# self.is_reverse = True
|
||||
# self.FeedMid_Status = FeedMidStatus.FMid_Take
|
||||
# self.next_position(self.is_reverse)
|
||||
# return
|
||||
#
|
||||
# if self.robotClient.type_detection == DetectType.EyeOutHand:
|
||||
# self.feed_Mid_Status = FeedMidStatus.FMid_Feed
|
||||
# self.next_position()
|
||||
# return
|
||||
# detect_pos_list = []
|
||||
# if not Constant.Debug:
|
||||
# try:
|
||||
# from Util.util_time import CRisOrFall
|
||||
# if self.cRis_photo.Q(self.error_photo_count >= 5, True):
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_photo_error_msgbox)
|
||||
# self.error_photo_count = 0
|
||||
# log.log_message(logging.INFO, Constant.str_feed_photo_confirm)
|
||||
#
|
||||
# # TODO 返回初始状态
|
||||
# for pos in self.feedConfig.photo_locs:
|
||||
# self.sendTargPosition(pos)
|
||||
# while not pos.compare(real_position): # 可以优化 TODO
|
||||
# if self.feedStatus == FeedStatus.FNone or not self.pause:
|
||||
# return
|
||||
# time.sleep(0.1)
|
||||
# code, img, xyz, uvw, mng = self.detection.get_position() # 检测结果
|
||||
# self.detection_image = img
|
||||
# if xyz != None:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
|
||||
# # dp = Detection_Position().init_position(*xyz, *uvw)
|
||||
# from Trace.handeye_calibration import R_matrix, getPosition
|
||||
# rotation = R_matrix(self.robotClient.status_model.world_0,
|
||||
# self.robotClient.status_model.world_1,
|
||||
# self.robotClient.status_model.world_2,
|
||||
# self.robotClient.status_model.world_3,
|
||||
# self.robotClient.status_model.world_4,
|
||||
# self.robotClient.status_model.world_5)
|
||||
#
|
||||
# # 黄老师给我的xyz和法向量
|
||||
# target_position, noraml_base = getPosition(*xyz, *uvw, rotation, *mng)
|
||||
# detect_pos_list.append(Real_Position().init_position(*target_position[:3], *noraml_base))
|
||||
# log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
|
||||
# else:
|
||||
# log.log_message(logging.ERROR, Constanstr_feed_takePhoto_fail + real_position.to_string())
|
||||
# z_diff, max_z_index = (lambda pts: (
|
||||
# max(pts, key=lambda p: p.Z).Z - min(pts, key=lambda p: p.Z).Z,
|
||||
# pts.index(max(pts, key=lambda p: p.Z))
|
||||
# ))(detect_pos_list)
|
||||
# if len(self.feedConfig.photo_locs) == 5:
|
||||
# if z_diff < Constant.bag_height and len(
|
||||
# detect_pos_list) == 3: # 第一次检测到没有高度差距开始三列拍照 TODO 保留全部的开关
|
||||
# # 拍照位置从五个变为三个
|
||||
# self.feedConfig.photo_locs = [detect_pos_list[0], detect_pos_list[2], detect_pos_list[4]]
|
||||
# take_position = detect_pos_list[0]
|
||||
# log.log_message(logging.INFO, Constant.str_feed_takePhoto_front_finish)
|
||||
# else:
|
||||
# take_position = detect_pos_list[max_z_index]
|
||||
# log.log_message(logging.INFO, Constant.str_feed_takePhoto_front)
|
||||
# else:
|
||||
# if z_diff < Constant.bag_height:
|
||||
# take_position = detect_pos_list[0]
|
||||
# log.log_message(logging.INFO, Constant.str_feed_takePhoto_new_line)
|
||||
# else:
|
||||
# take_position = detect_pos_list[max_z_index]
|
||||
# log.log_message(logging.INFO, Constant.str_feed_takePhoto_line)
|
||||
#
|
||||
# self.feedConfig.feedLine.set_take_position(take_position) ##TODO 检查有没有异常
|
||||
# self.next_position()
|
||||
# except:
|
||||
# log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
# self.error_photo_count += 1
|
||||
# else:
|
||||
# self.feedConfig.feedLine.set_take_position(real_position) ##TODO 检查有没有异常
|
||||
# self.next_position()
|
||||
# log.log_message(logging.INFO, Constant.str_feed_takePhoto_move)
|
||||
#
|
||||
# elif self.feedStatus == FeedStatus.FTake:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_take)
|
||||
# if self.feedConfig.feedLine.get_take_position() != None:
|
||||
# if self.feedConfig.feedLine.get_take_position().compare(real_position):
|
||||
# # 打开吸嘴并返回
|
||||
# self.sendIOControl(self.robotClient.con_ios[0], 1)
|
||||
# self.sendIOControl(self.robotClient.con_ios[1], 1)
|
||||
# self.sendIOControl(self.robotClient.con_ios[2], 1)
|
||||
#
|
||||
# # TODO 检测是否通 不然报警
|
||||
# self.feedConfig.feedLine.set_take_position(None)
|
||||
# time.sleep(self.robotClient.time_delay_take)
|
||||
# log.log_message(logging.INFO, Constant.str_feed_take_success)
|
||||
#
|
||||
#
|
||||
#
|
||||
# elif self.feedStatus == FeedStatus.FBroken1:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_broken)
|
||||
# if self.get_current_position().compare(real_position):
|
||||
# self.next_position()
|
||||
#
|
||||
#
|
||||
# elif self.feedStatus == FeedStatus.FBroken2:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_broken)
|
||||
# if self.get_current_position().compare(real_position):
|
||||
# self.next_position()
|
||||
#
|
||||
# elif self.feedStatus == FeedStatus.FShake:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_shake)
|
||||
# if self.get_current_position().compare(real_position):
|
||||
# # TODO 震动方案
|
||||
# time.sleep(self.robotClient.time_delay_shake)
|
||||
# self.next_position()
|
||||
#
|
||||
# elif self.feedStatus == FeedStatus.FDropBag:
|
||||
# log.log_message(logging.INFO, Constant.str_feed_drop)
|
||||
#
|
||||
# if self.get_current_position().compare(real_position):
|
||||
# self.sendIOControl(self.robotClient.con_ios[0], 0)
|
||||
# self.sendIOControl(self.robotClient.con_ios[1], 0)
|
||||
# self.sendIOControl(self.robotClient.con_ios[2], 0)
|
||||
# # TODO 检测是否断 不然报警
|
||||
# time.sleep(self.robotClient.time_delay_put)
|
||||
# # TODO 获取目标位置
|
||||
# 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.feedConfig.num = self.feedConfig.num - 1
|
||||
# log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
|
||||
# self.next_position()
|
||||
#
|
||||
#
|
||||
#
|
||||
#
|
||||
# def send_emergency_sound(self):
|
||||
# self.sendIOControl(Constant.IO_EmergencyPoint, 1)
|
||||
#
|
||||
# def send_emergency_stop(self):
|
||||
# self.sendIOControl(Constant.IO_EmergencyPoint, 0)
|
||||
#
|
||||
# def sendIOControl(self, IO_bit, IO_Status: int):
|
||||
#
|
||||
# IO_command = CMDInstructRequest()
|
||||
# io_instruction = Instruction()
|
||||
# io_instruction.IO = True
|
||||
# io_instruction.io_status = IO_Status
|
||||
# io_instruction.point = IO_bit # {"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
|
||||
# IO_command.dsID = 'HCRemoteCommand'
|
||||
# IO_command.instructions.append(io_instruction)
|
||||
# self.robotClient.add_sendQuene(IO_command.toString())
|
||||
# log.log_message(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
|
||||
# pass
|
||||
#
|
||||
# def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=Constant.speed,real_position1=None):
|
||||
# position_instruction = Instruction()
|
||||
# position_instruction.speed = speed
|
||||
# position_instruction.m0 = real_position.X
|
||||
# position_instruction.m1 = real_position.Y
|
||||
# position_instruction.m2 = real_position.Z
|
||||
# position_instruction.m3 = real_position.U
|
||||
# position_instruction.m4 = real_position.V
|
||||
# position_instruction.m5 = real_position.W
|
||||
#
|
||||
# position_instruction.action = move_type.value
|
||||
# if position_instruction.action == 17:
|
||||
# position_instruction.m0_p = real_position1.X
|
||||
# position_instruction.m1_p = real_position1.Y
|
||||
# position_instruction.m2_p = real_position1.Z
|
||||
# position_instruction.m3_p = real_position1.U
|
||||
# position_instruction.m4_p = real_position1.V
|
||||
# position_instruction.m5_p = real_position1.W
|
||||
# instruction_command = CMDInstructRequest()
|
||||
# instruction_command.instructions.append(position_instruction)
|
||||
# request_command = instruction_command.toString()
|
||||
#
|
||||
# log_str = f'移动到位置:{"姿势直线"}：' \
|
||||
# f'X:{position_instruction.m0}-' \
|
||||
# f'Y:{position_instruction.m1}-' \
|
||||
# f'Z:{position_instruction.m2}-' \
|
||||
# f'U:{position_instruction.m3}-' \
|
||||
# f'V:{position_instruction.m4}-' \
|
||||
# f'W:{position_instruction.m5}'
|
||||
#
|
||||
# try:
|
||||
# log.log_message(logging.INFO, log_str)
|
||||
# except:
|
||||
# print("error")
|
||||
#
|
||||
# self.robotClient.add_sendQuene(request_command)
|
||||
# pass
|
||||
#
|
||||
#
|
||||
# def next_start(self,reverse=False):
|
||||
# start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
# self.feedStatus = start_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
# if start_pos.lineType == LineType.CureMid:
|
||||
# start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
# self.sendTargPosition(real_position=start_pos.position, move_type=MoveType.Cure, real_position1=start_pos1.position)
|
||||
# else:
|
||||
# self.sendTargPosition(start_pos.position)
|
||||
# pass
|
||||
#
|
||||
# def next_take(self,reverse=False):
|
||||
# take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
# self.feedStatus = take_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
# if take_pos.lineType == LineType.CureMid:
|
||||
# take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
# self.sendTargPosition(real_position=take_pos.position, move_type=MoveType.Cure, real_position1=take_pos1.position)
|
||||
# else:
|
||||
# self.sendTargPosition(take_pos.position)
|
||||
# pass
|
||||
#
|
||||
# def next_Feed(self,reverse=False):
|
||||
# feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
# self.feedStatus = feed_pos.status if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
# if feed_pos.lineType == LineType.CureMid:
|
||||
# feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
# self.sendTargPosition(real_position=feed_pos.position, move_type=MoveType.Cure, real_position1=feed_pos1.position)
|
||||
# else:
|
||||
# self.sendTargPosition(feed_pos.position)
|
||||
#
|
||||
# def get_current_position(self):
|
||||
# if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
# return self.feedConfig.feedLine.get_current_start_position()
|
||||
# elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
# return self.feedConfig.feedLine.get_current_take_position()
|
||||
# elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
# return self.feedConfig.feedLine.get_current_feed_position()
|
||||
# def next_position(self,reverse=False):
|
||||
# if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
# self.next_start(reverse)
|
||||
# elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
# self.next_take(reverse)
|
||||
# elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
# self.next_Feed(reverse)
|
||||
#
|
||||
# def safe_check_columns(self):
|
||||
# return True
|
||||
# pass
|
||||
#
|
||||
# def safe_check_person(self):
|
||||
# return True
|
||||
# pass
|
||||
#
|
||||
809
CU/Feeding_old.py
Normal file
809
CU/Feeding_old.py
Normal file
@ -0,0 +1,809 @@
|
||||
import copy
|
||||
import logging
|
||||
import random
|
||||
import threading
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from PyQt5.QtWidgets import QMessageBox
|
||||
from PySide6.QtCore import Signal, QObject
|
||||
from PySide6.QtGui import QPixmap
|
||||
|
||||
import Constant
|
||||
import Expection
|
||||
from CU.Catch import Catch, CatchStatus
|
||||
from CU.Detect import Detect, DetectStatus
|
||||
from Model.FeedModel import PositionModel
|
||||
from Model.Position import Real_Position, Detection_Position
|
||||
from enum import Enum, IntEnum
|
||||
from COM.COM_Robot import RobotClient, DetectType
|
||||
from Model.RobotModel import CMDInstructRequest, MoveType
|
||||
from Trace.handeye_calibration import getPosition
|
||||
from Trace.handeye_calibration import getxyz,getxyz1
|
||||
from Util.util_math import get_distance
|
||||
from Util.util_time import CRisOrFall
|
||||
#from Vision.camera_coordinate_dete import Detection
|
||||
from Util.util_log import log
|
||||
from Model.RobotModel import Instruction
|
||||
from EMV.EMV import RelayController
|
||||
class ResetStatus(Enum):
|
||||
RNone = 0
|
||||
RStart = 1
|
||||
RRunging = 2
|
||||
ROk =3
|
||||
|
||||
class FeedStatus(IntEnum):
|
||||
FNone = 0
|
||||
FStart = 1
|
||||
FCheck = 2
|
||||
FMid = 3
|
||||
FPhoto = 4
|
||||
FTake = 5
|
||||
FBroken1 = 6
|
||||
FBroken2 =7
|
||||
FShake = 8
|
||||
FDropBag = 9
|
||||
FFinished = 10
|
||||
FReverse = 11
|
||||
FStartReverse = 12
|
||||
|
||||
class LineType(Enum):
|
||||
Straight = 0
|
||||
CureMid = 2
|
||||
CureEnd = 3
|
||||
WORLD = 4
|
||||
|
||||
|
||||
class FeedMidStatus(Enum):
|
||||
FMid_Start = 1
|
||||
FMid_Take = 2
|
||||
FMid_Feed= 3
|
||||
|
||||
class FeedPosition:
|
||||
def __init__(self,status:FeedStatus,position:Real_Position):
|
||||
self.status = status
|
||||
self.position = position
|
||||
|
||||
class FeedLine:
|
||||
def __init__(self, id, name, feed_positions:list):
|
||||
self.feed_positions = copy.deepcopy(feed_positions)
|
||||
self.feeding2end_pos_index = 0
|
||||
self.origin2start_pos_index = 0
|
||||
self.start2take_pos_index = 0
|
||||
self.name = name
|
||||
self.id = id
|
||||
|
||||
def get_current_feed_position(self,is_reverse):
|
||||
pos = self.feeding_to_end[ (self.feeding2end_pos_index+1)%len(self.feeding_to_end) if is_reverse else self.feeding2end_pos_index-1]
|
||||
return pos
|
||||
def get_current_take_position(self,is_reverse):
|
||||
pos = self.start_to_take[ (self.start2take_pos_index+1)%len(self.start_to_take) if is_reverse else self.start2take_pos_index-1]
|
||||
return pos
|
||||
def get_current_start_position(self,is_reverse):
|
||||
pos = self.origin_to_start[(self.origin2start_pos_index+1)%len(self.origin_to_start) if is_reverse else self.origin2start_pos_index-1]
|
||||
return pos
|
||||
|
||||
def get_next_feed_position(self,reverse:bool=False):
|
||||
pos = self.feeding_to_end[self.feeding2end_pos_index]
|
||||
if reverse:
|
||||
self.feeding2end_pos_index -= 1
|
||||
if self.feeding2end_pos_index < 0:
|
||||
self.feeding2end_pos_index = len(self.feeding_to_end) - 1
|
||||
else:
|
||||
self.feeding2end_pos_index += 1
|
||||
if self.feeding2end_pos_index >= len(self.feeding_to_end):
|
||||
self.feeding2end_pos_index = 0
|
||||
return pos
|
||||
|
||||
def get_next_start_position(self,reverse:bool=False):
|
||||
pos = self.origin_to_start[self.origin2start_pos_index]
|
||||
if reverse:
|
||||
self.origin2start_pos_index -= 1
|
||||
if self.origin2start_pos_index < 0:
|
||||
self.origin2start_pos_index = len(self.origin_to_start) - 1
|
||||
else:
|
||||
self.origin2start_pos_index += 1
|
||||
if self.origin2start_pos_index >= len(self.origin_to_start):
|
||||
self.origin2start_pos_index = 0
|
||||
|
||||
return pos
|
||||
|
||||
def get_next_take_position(self,reverse:bool=False):
|
||||
pos = self.start_to_take[self.start2take_pos_index]
|
||||
if reverse:
|
||||
self.start2take_pos_index -= 1
|
||||
if self.start2take_pos_index < 0:
|
||||
self.start2take_pos_index = len(self.start_to_take) - 1
|
||||
else:
|
||||
self.start2take_pos_index += 1
|
||||
if self.start2take_pos_index >= len(self.start_to_take):
|
||||
self.start2take_pos_index = 0
|
||||
return pos
|
||||
def get_take_position(self):
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
return self.feeding_to_end[i]
|
||||
|
||||
def set_take_position(self, position: Real_Position, dynamic_height=0):
|
||||
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}")
|
||||
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
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]}")
|
||||
|
||||
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)
|
||||
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("[调试] 抓取前后位置已设置完成")
|
||||
|
||||
def set_drop_position(self, position: Real_Position):
|
||||
"""
|
||||
设置 FDropBag 位置，只设置当前点，不处理前后点。
|
||||
:param position: 新的丢包位置
|
||||
"""
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FDropBag.value:
|
||||
# 直接设置当前点的位置
|
||||
self.feeding_to_end[i].set_position(position)
|
||||
print(
|
||||
f"✅ FDropBag 位置已更新: ({position.X}, {position.Y}, {position.Z}, {position.U}, {position.V}, {position.W})")
|
||||
break # 假设只有一个丢包点
|
||||
|
||||
def get_position_list(self):
|
||||
index_start = -1
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FCheck.value:
|
||||
index_start = i
|
||||
break
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
|
||||
index_take = i
|
||||
|
||||
self.origin_to_start = self.feed_positions[: index_start+1]
|
||||
self.start_to_take = self.feed_positions[index_start:index_take+1]
|
||||
self.feeding_to_end = self.feed_positions[index_take:]
|
||||
|
||||
for i in range(len(self.feeding_to_end)): #插入动态中间点
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
befor_position_model = PositionModel()
|
||||
befor_position_model.init_position(None)
|
||||
after_position_model = PositionModel()
|
||||
after_position_model.init_position(None)
|
||||
|
||||
self.feeding_to_end.insert(i, befor_position_model)
|
||||
self.feeding_to_end.insert(i+2, after_position_model)
|
||||
break
|
||||
|
||||
class FeedingConfig:
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs):
|
||||
self.num = num
|
||||
self.feedLine = feedLine
|
||||
self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
|
||||
|
||||
def deal_photo_locs(self, photo_loc):
|
||||
position_photo = Real_Position()
|
||||
position_photo.init_position(photo_loc[0], photo_loc[1], photo_loc[2], photo_loc[3], photo_loc[4], photo_loc[5])
|
||||
return position_photo
|
||||
|
||||
def get_line_info(self):
|
||||
pass
|
||||
|
||||
|
||||
class Feeding(QObject):
|
||||
need_origin_signal = Signal(str)
|
||||
take_no_photo_sigal = Signal()
|
||||
update_detect_image = Signal(np.ndarray)
|
||||
log_signal = Signal(int,str)
|
||||
def __init__(self, robotClient: RobotClient):
|
||||
super().__init__()
|
||||
self.feedConfig = None
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.robotClient = robotClient
|
||||
# 添加 RelayController 实例
|
||||
self.relay_controller = RelayController()
|
||||
self.sensor_thread = None
|
||||
self.detection_image = None
|
||||
self.init_detection_image()
|
||||
self.pause = False
|
||||
self.cRis_photo = CRisOrFall()
|
||||
self.cRis_shake = CRisOrFall()
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
self.is_reverse = False
|
||||
# 复位集合
|
||||
self.run_reverse = False
|
||||
self.take_no_photo = False
|
||||
self.reset_status = ResetStatus.RNone
|
||||
self.reversed_positions = []
|
||||
self.current_position = None
|
||||
self.index=1
|
||||
self.pos_index = -1
|
||||
self.pos_near_index = -1
|
||||
self.catch = Catch(self.robotClient)
|
||||
self.detect = Detect()
|
||||
self.is_detected = True
|
||||
self.detect_thread = threading.Thread(target=self.run_detect)
|
||||
self.detect_thread.start()
|
||||
self.onekey = False
|
||||
self.debug_run_count = 0 # 初始化计数器
|
||||
self.mid_take_count = 0
|
||||
#传感器判断抓包参数
|
||||
self.sensor2_ready = False # 传感器2是否检测到料包
|
||||
self.motor_stopped_by_sensor2 = False # 是否由传感器2触发停止电机
|
||||
self.sensor_thread = None
|
||||
self.relay_controller = RelayController()
|
||||
# 启动传感器2线程
|
||||
self.relay_controller._running = True
|
||||
self.sensor2_thread = None
|
||||
pass
|
||||
|
||||
def close_feed(self):
|
||||
self.is_detected = False
|
||||
self.detect_thread.join()
|
||||
self.detect.detection.release()
|
||||
|
||||
def init_detection_image(self):
|
||||
detection_image = cv2.imread(Constant.feed_sign_path)
|
||||
self.update_detect_image.emit(detection_image)
|
||||
|
||||
def run_detect(self):
|
||||
while self.is_detected:
|
||||
self.detect.run()
|
||||
time.sleep(0.02)
|
||||
|
||||
def run(self):
|
||||
self.catch.run()
|
||||
# 获取事件坐标
|
||||
real_position = Real_Position()
|
||||
self.detect.position_index = 0
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
# real_position.init_position(0,
|
||||
# 0,
|
||||
# 0,
|
||||
# 0,
|
||||
# 0,
|
||||
# 0);
|
||||
|
||||
# img_path = f'Image/{self.index}.png'
|
||||
# img = cv2.imread(img_path)
|
||||
# self.index += 1
|
||||
# self.index = self.index % 4
|
||||
# self.detection_image = img
|
||||
|
||||
if self.feedConfig == None:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
|
||||
if self.feedConfig !=None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(real_position,is_action=True):
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.is_reverse = False
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
|
||||
|
||||
if self.feedStatus == FeedStatus.FNone or self.pause:
|
||||
return
|
||||
|
||||
elif self.feedStatus == FeedStatus.FCheck:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_check)
|
||||
# 1, 检查是否是三列
|
||||
# 2, 检查是否有人
|
||||
# if self.safe_check_columns() and self.safe_check_person():
|
||||
# pass
|
||||
# else:
|
||||
# if self.feedConfig.num != 0:
|
||||
# self.next_target()
|
||||
# if == 原点 继续判断
|
||||
# else:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_sae_error_msgbox)
|
||||
|
||||
if self.is_reverse:
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
else:
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
|
||||
if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position):
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FStart:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_start)
|
||||
self.relay_controller.open(conveyor2=True)#开电机
|
||||
#self.sensor2_thread = threading.Thread(target=self.relay_controller.handle_sensor2, daemon=True)#线程2的开始，但是在那里设置结束呢
|
||||
#self.sensor2_thread.start()
|
||||
if not self.robotClient.origin_position.compare(real_position,is_action=True) and not self.is_reverse:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error)
|
||||
self.need_origin_signal.emit(Constant.str_feed_start_error)
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return
|
||||
if self.is_reverse:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.is_reverse = False
|
||||
return
|
||||
|
||||
self.feedConfig.feedLine.get_position_list()
|
||||
self.detect.detect_status = DetectStatus.DNone
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FMid:
|
||||
feed_pos = self.get_current_position(self.is_reverse)
|
||||
if feed_pos.get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
|
||||
self.next_position(self.is_reverse)
|
||||
# 增加计数器逻辑
|
||||
self.mid_take_count += 1
|
||||
# 可选：在 Debug1 模式下输出日志
|
||||
if Constant.Debug1:
|
||||
self.log_signal.emit(
|
||||
logging.INFO,
|
||||
f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次"
|
||||
)
|
||||
if self.feedStatus == FeedStatus.FTake:
|
||||
self.catch.catch_status = CatchStatus.CTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
if self.feedConfig.num == 0:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_finish)
|
||||
self.is_reverse = True
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
self.feedConfig.feedLine.start2take_pos_index = len(self.feedConfig.feedLine.start_to_take) - 2
|
||||
self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2
|
||||
self.next_position(self.is_reverse)
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
|
||||
self.init_detection_image()
|
||||
return
|
||||
|
||||
if not Constant.Debug:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto)
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Feed
|
||||
self.detect.detect_status = DetectStatus.DOk
|
||||
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)
|
||||
self.next_position()
|
||||
self.detect.detect_status = DetectStatus.DNone
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing2)
|
||||
#self.feedStatus = FeedStatus.FTake
|
||||
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_take)
|
||||
take_position = self.feedConfig.feedLine.get_take_position()
|
||||
if not take_position or not take_position.get_position():
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
return
|
||||
if not take_position.get_position().compare(real_position, is_action=True):
|
||||
self.log_signal.emit(logging.INFO, "🟡 机器人尚未到达抓料点位")
|
||||
return
|
||||
self.log_signal.emit(logging.INFO, "🟢 机器人已到达抓料点位")
|
||||
'''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) # 每秒检查一次
|
||||
# 执行抓取动作
|
||||
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.detect.detect_status = DetectStatus.DOk
|
||||
self.log_signal.emit(logging.INFO, "修改丢包点")
|
||||
# 输出当前抓取的索引（当前使用的点）
|
||||
print(f"[调试] 即将丢包点位索引: {self.detect.position_index}")
|
||||
# ✅ 先递增索引（准备下一个点）
|
||||
self.detect.position_index += 1
|
||||
# ✅ 输出下一个要加载的索引
|
||||
print(f"[调试] 下一个要加载的索引: {self.detect.position_index}")
|
||||
# ✅ 重新加载下一个点位（基于更新后的 index）
|
||||
if not self.detect.run():
|
||||
self.log_signal.emit(logging.ERROR, "❌ 加载新点位失败，停止流程")
|
||||
return
|
||||
# ✅ 设置丢包点为新加载的点位
|
||||
self.feedConfig.feedLine.set_drop_position(self.detect.detect_position)
|
||||
# ✅ 成功加载新点，继续执行后续逻辑
|
||||
self.debug_run_count += 1
|
||||
self.next_position()
|
||||
return
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken2:
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FShake:
|
||||
if self.get_current_position().get_position().compare(real_position,is_action=True):
|
||||
# TODO 震动方案
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
self.catch.catch_status = CatchStatus.CShake
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.CShake:
|
||||
# if self.feedConfig.feedLine.feeding_to_end[
|
||||
# self.feedConfig.feedLine.feeding2end_pos_index + 1].status != FeedStatus.FShake:
|
||||
# self.catch.catch_status = CatchStatus.COk
|
||||
# else:
|
||||
self.catch.shake_continue.SetReset()
|
||||
self.next_position()
|
||||
if self.feedStatus!=FeedStatus.FShake:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
return
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
if self.get_current_position().get_position().compare(real_position,is_action=True):
|
||||
# self.sendIOControl(self.robotClient.con_ios[0], 0)
|
||||
# self.sendIOControl(self.robotClient.con_ios[1], 0)
|
||||
# self.sendIOControl(self.robotClient.con_ios[2], 0)
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
self.catch.catch_status = CatchStatus.CDrop
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.CDrop:
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
# time.sleep(self.robotClient.time_delay_put)
|
||||
# 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.feedConfig.feedLine.set_take_position(self.get_take_position())
|
||||
if self.detect.detect_status == DetectStatus.DNone:
|
||||
self.detect.detect_status = DetectStatus.DDetect
|
||||
self.feedConfig.num = self.feedConfig.num - 1
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
|
||||
self.next_position()
|
||||
|
||||
def run_reset(self):
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
if self.reset_status == ResetStatus.RNone:
|
||||
return
|
||||
|
||||
if self.reset_status == ResetStatus.RStart:
|
||||
if self.feedConfig == None: return
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
# if self.feedConfig.feedLine.positions[index].status == 2:
|
||||
# start_index = index
|
||||
self.pos_index = -1
|
||||
self.pos_near_index = -1
|
||||
self.reversed_positions = []
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position,is_action=True):
|
||||
self.pos_index = index
|
||||
break
|
||||
|
||||
if self.pos_index == -1:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
|
||||
min_distance = 99999999
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if get_distance(pos_model.get_position(), real_position) < min_distance:
|
||||
min_distance = get_distance(pos_model.get_position(), real_position)
|
||||
self.pos_near_index = index
|
||||
if self.pos_near_index != -1:
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1]
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index+1]
|
||||
self.reversed_positions = list(reversed(self.reversed_positions))
|
||||
self.reverse_index = 0
|
||||
self.send_emergency_sound()
|
||||
self.current_position = PositionModel()
|
||||
self.current_position.init_position(real_position)
|
||||
self.reset_status = ResetStatus.RRunging
|
||||
|
||||
if self.reset_status == ResetStatus.RRunging:
|
||||
if not real_position.compare(self.current_position.get_position(),is_action=True):
|
||||
return
|
||||
pos_model = self.reversed_positions[self.reverse_index]
|
||||
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
|
||||
pos_model = self.reversed_positions[self.reverse_index + 1]
|
||||
self.reverse_index = self.reverse_index+1
|
||||
|
||||
if pos_model.lineType == LineType.CureMid.value:
|
||||
pos_model1 = self.reversed_positions[self.reverse_index + 1]
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index + 2
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
|
||||
if self.reverse_index == len(self.reversed_positions):
|
||||
self.reset_status = ResetStatus.ROk
|
||||
if self.reset_status == ResetStatus.ROk:
|
||||
self.reset_status = ResetStatus.RNone
|
||||
self.send_emergency_stop()
|
||||
|
||||
def return_original_position(self):
|
||||
|
||||
self.run_reverse = True
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
if self.feedConfig == None: return
|
||||
start_index = -1
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
# if self.feedConfig.feedLine.positions[index].status == 2:
|
||||
# start_index = index
|
||||
pos_index = -1
|
||||
pos_near_index = -1
|
||||
reversed_positions = []
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position):
|
||||
pos_index = index
|
||||
break
|
||||
|
||||
if pos_index == -1:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
|
||||
min_distance = 99999999
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if get_distance(pos_model.get_position(), real_position)<min_distance:
|
||||
min_distance = get_distance(pos_model.get_position(), real_position)
|
||||
pos_near_index = index
|
||||
if pos_near_index != -1:
|
||||
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_near_index+1]
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_index]
|
||||
reversed_positions = list(reversed(reversed_positions))
|
||||
self.reverse_index = 0
|
||||
self.send_emergency_sound()
|
||||
current_position = PositionModel()
|
||||
current_position.init_position(real_position)
|
||||
while self.run_reverse and self.reverse_index!=len(reversed_positions):
|
||||
if self.reverse_index!=0 and not real_position.compare(current_position.get_position()):
|
||||
continue
|
||||
#todo 缺少比对
|
||||
pos_model = reversed_positions[self.reverse_index]
|
||||
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
|
||||
|
||||
pos_model = reversed_positions[self.reverse_index + 1]
|
||||
# TODO take节点判断
|
||||
|
||||
if pos_model.lineType == LineType.CureMid.value:
|
||||
pos_model1 = reversed_positions[self.reverse_index + 1]
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(),speed= self.robotClient.reset_speed)
|
||||
current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index+2
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(),speed=self.robotClient.reset_speed)
|
||||
current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
self.send_emergency_stop()
|
||||
return True
|
||||
def send_emergency_sound(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 1)
|
||||
|
||||
def send_emergency_stop(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 0)
|
||||
|
||||
def sendIOControl(self, IO_bit, IO_Status: int):
|
||||
|
||||
IO_command = CMDInstructRequest()
|
||||
io_instruction = Instruction()
|
||||
io_instruction.IO = True
|
||||
io_instruction.io_status = IO_Status
|
||||
io_instruction.point = IO_bit # {"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
|
||||
IO_command.dsID = 'HCRemoteCommand'
|
||||
IO_command.instructions.append(io_instruction)
|
||||
self.robotClient.add_sendQuene(IO_command.toString())
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
|
||||
pass
|
||||
|
||||
def sendTargPosition1(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
|
||||
pass
|
||||
|
||||
|
||||
def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5,real_position1=None):
|
||||
position_instruction = Instruction()
|
||||
position_instruction.speed = speed
|
||||
position_instruction.m0 = real_position.X
|
||||
position_instruction.m1 = real_position.Y
|
||||
position_instruction.m2 = real_position.Z
|
||||
position_instruction.m3 = real_position.U
|
||||
position_instruction.m4 = real_position.V
|
||||
position_instruction.m5 = real_position.W
|
||||
position_instruction.smooth = self.robotClient.smooth
|
||||
|
||||
position_instruction.action = move_type.value
|
||||
if position_instruction.action == 17:
|
||||
position_instruction.m0_p = real_position1.X
|
||||
position_instruction.m1_p = real_position1.Y
|
||||
position_instruction.m2_p = real_position1.Z
|
||||
position_instruction.m3_p = real_position1.U
|
||||
position_instruction.m4_p = real_position1.V
|
||||
position_instruction.m5_p = real_position1.W
|
||||
instruction_command = CMDInstructRequest()
|
||||
instruction_command.instructions.append(position_instruction)
|
||||
request_command = instruction_command.toString()
|
||||
|
||||
log_str = f'移动到位置:{ "姿势直线" if move_type==MoveType.WORLD else "姿势曲线"}：' \
|
||||
f'X:{position_instruction.m0}-' \
|
||||
f'Y:{position_instruction.m1}-' \
|
||||
f'Z:{position_instruction.m2}-' \
|
||||
f'U:{position_instruction.m3}-' \
|
||||
f'V:{position_instruction.m4}-' \
|
||||
f'W:{position_instruction.m5}'
|
||||
|
||||
|
||||
try:
|
||||
print('fuck1',log_str)
|
||||
self.log_signal.emit(logging.INFO, log_str)
|
||||
print('fuck2',log_str)
|
||||
except:
|
||||
return
|
||||
print(request_command)
|
||||
self.robotClient.add_sendQuene(request_command)
|
||||
pass
|
||||
# def get_take_position(self):
|
||||
# if Constant.Debug:
|
||||
# return self.robotClient.status_model.getRealPosition()
|
||||
# _, img, xyz, uvw, points = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,save_img_point=0, Height_reduce=30, width_reduce=30)
|
||||
# self.detection_image = img.copy()
|
||||
# if xyz ==None or uvw==None or points==None:
|
||||
# return None
|
||||
# target_position,noraml_base = getPosition(*xyz,*uvw,None,points)
|
||||
#
|
||||
# position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
|
||||
# position.Z = position.Z+200
|
||||
# return position
|
||||
def next_start(self,reverse=False):
|
||||
start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
if start_pos.lineType == LineType.CureMid.value:
|
||||
start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
self.feedStatus = FeedStatus(start_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed, move_type=MoveType.Cure, real_position1=start_pos1.get_position())
|
||||
elif start_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(start_pos.get_position(),self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed,move_type=MoveType.AXIS)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.reset_speed)
|
||||
pass
|
||||
|
||||
def next_take(self,reverse=False):
|
||||
|
||||
take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
if take_pos.lineType == LineType.CureMid.value:
|
||||
take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure, real_position1=take_pos1.get_position(),speed=self.robotClient.feed_speed)
|
||||
elif take_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(take_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
pass
|
||||
|
||||
def next_Feed(self,reverse=False):
|
||||
feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
if feed_pos.lineType == LineType.CureMid.value:
|
||||
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure, real_position1=feed_pos1.get_position(),speed=self.robotClient.feed_speed)
|
||||
elif feed_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
# if not reverse and self.feedStatus == FeedStatus.FShake:
|
||||
# if not reverse:
|
||||
# if self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, True):
|
||||
# pass
|
||||
# elif self.cRis_shake.Q(self.feedStatus == FeedStatus.FShake, False):
|
||||
# self.feedStatus = FeedStatus.FShake
|
||||
# self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
# return
|
||||
|
||||
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(),speed=self.robotClient.feed_speed)
|
||||
|
||||
def get_current_position(self,is_reverse=False):
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
return self.feedConfig.feedLine.get_current_start_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
return self.feedConfig.feedLine.get_current_take_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
return self.feedConfig.feedLine.get_current_feed_position(self.is_reverse)
|
||||
def next_position(self,reverse=False):
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
self.next_start(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_start')
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
self.next_take(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_take')
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
self.next_Feed(reverse)
|
||||
if Constant.Debug1:
|
||||
print('next_feed')
|
||||
|
||||
def safe_check_columns(self):
|
||||
return True
|
||||
pass
|
||||
|
||||
def safe_check_person(self):
|
||||
return True
|
||||
pass
|
||||
|
||||
BIN
CU/__pycache__/Catch.cpython-39.pyc
Normal file
BIN
CU/__pycache__/Catch.cpython-39.pyc
Normal file
Binary file not shown.
BIN
CU/__pycache__/Command.cpython-39.pyc
Normal file
BIN
CU/__pycache__/Command.cpython-39.pyc
Normal file
Binary file not shown.
BIN
CU/__pycache__/Detect.cpython-39.pyc
Normal file
BIN
CU/__pycache__/Detect.cpython-39.pyc
Normal file
Binary file not shown.
BIN
CU/__pycache__/EMV.cpython-39.pyc
Normal file
BIN
CU/__pycache__/EMV.cpython-39.pyc
Normal file
Binary file not shown.
BIN
CU/__pycache__/Feeding.cpython-39.pyc
Normal file
BIN
CU/__pycache__/Feeding.cpython-39.pyc
Normal file
Binary file not shown.
BIN
CU/__pycache__/test_Catch_EMV.cpython-39.pyc
Normal file
BIN
CU/__pycache__/test_Catch_EMV.cpython-39.pyc
Normal file
Binary file not shown.
12
CU/list.ini
Normal file
12
CU/list.ini
Normal file
@ -0,0 +1,12 @@
|
||||
[positions]
|
||||
0 = -42.885, -1432.385, -415.451, -59.044, 17.258, 17.258
|
||||
1 = -42.885, -1432.385, -410.451, -59.044, 7.258, 7.258
|
||||
2 = -42.885, -1432.385, -380.451, -59.044, 7.258, 7.258
|
||||
3 = -42.885, -1432.385, -340.451, -59.044, 7.258, 7.258
|
||||
4 = -42.885, -1432.385, -300.451, -59.044, 7.258, 7.258
|
||||
5 = -42.885, -1432.385, -280.451, -59.044, 7.258, 7.258
|
||||
6 = -42.885, -1432.385, -410.451, -59.044, 7.258, 7.258
|
||||
7 = -42.885, -1432.385, -380.451, -59.044, 7.258, 7.258
|
||||
8 = -42.885, -1432.385, -340.451, -59.044, 7.258, 7.258
|
||||
9 = -42.885, -1432.385, -300.451, -59.044, 7.258, 7.258
|
||||
10 = -42.885, -1432.385, -280.451, -59.044, 7.258, 7.258
|
||||
178
CU/sae.py
Normal file
178
CU/sae.py
Normal file
@ -0,0 +1,178 @@
|
||||
import logging
|
||||
from types import SimpleNamespace
|
||||
|
||||
# 模拟 Constant
|
||||
class Constant:
|
||||
str_feed_take = "[测试] 开始抓料流程"
|
||||
str_feed_take_success = "[测试] ✅ 抓料成功"
|
||||
str_feed_takePhoto_fail = "[测试] ❌ 抓料点位获取失败"
|
||||
|
||||
# 模拟 FeedStatus 和 CatchStatus
|
||||
class FeedStatus:
|
||||
FTake = "FTake"
|
||||
|
||||
class CatchStatus:
|
||||
CNone = "CNone"
|
||||
CTake = "CTake"
|
||||
COk = "COk"
|
||||
|
||||
class DetectStatus:
|
||||
DOk = "DOk"
|
||||
|
||||
# 模拟 Position 类
|
||||
class Position:
|
||||
def __init__(self, x=0, y=0, z=0, a=0, b=0, c=0):
|
||||
self.X = x
|
||||
self.Y = y
|
||||
self.Z = z
|
||||
self.A = a
|
||||
self.B = b
|
||||
self.C = c
|
||||
|
||||
def get_position(self):
|
||||
return self
|
||||
|
||||
def compare(self, other, is_action=False):
|
||||
return (
|
||||
abs(self.X - other.X) < 0.1 and
|
||||
abs(self.Y - other.Y) < 0.1 and
|
||||
abs(self.Z - other.Z) < 0.1
|
||||
)
|
||||
|
||||
# 模拟 Detect 类
|
||||
class Detect:
|
||||
def __init__(self):
|
||||
self.detect_position = None
|
||||
self.position_index = 0
|
||||
self.detect_status = DetectStatus.DOk
|
||||
# 模拟配置点位（类似 list.ini）
|
||||
self.positions = {
|
||||
0: Position(100, 200, 300, 0, 0, 0),
|
||||
1: Position(150, 250, 350, 0, 0, 0),
|
||||
2: Position(200, 300, 400, 0, 0, 0),
|
||||
3: Position(100, 200, 300, 0, 0, 0),
|
||||
4: Position(150, 250, 350, 0, 0, 0),
|
||||
5: Position(200, 300, 400, 0, 0, 0),
|
||||
|
||||
}
|
||||
|
||||
def run(self):
|
||||
"""根据当前索引加载点位"""
|
||||
pos = self.positions.get(self.position_index, None)
|
||||
if pos:
|
||||
self.detect_position = pos
|
||||
return True
|
||||
else:
|
||||
print(f"[❌ 失败] 索引 {self.position_index} 没有对应点位")
|
||||
return False
|
||||
|
||||
# 模拟 FeedLine 类
|
||||
class FeedLine:
|
||||
def __init__(self):
|
||||
self.drop_position = Position()
|
||||
|
||||
def get_take_position(self):
|
||||
return SimpleNamespace(get_position=lambda: Position(100, 200, 300))
|
||||
|
||||
def set_drop_position(self, position):
|
||||
self.drop_position = position
|
||||
|
||||
# 模拟 RelayController
|
||||
class RelayController:
|
||||
def get_all_device_status(self, *args):
|
||||
return {"SENSOR2": True}
|
||||
|
||||
def open(self, conveyor2=False):
|
||||
pass
|
||||
|
||||
# 模拟主类，用于测试
|
||||
class TestRunner:
|
||||
def __init__(self):
|
||||
self.feedStatus = FeedStatus.FTake
|
||||
self.catch = SimpleNamespace(catch_status=CatchStatus.CNone)
|
||||
self.detect = Detect()
|
||||
self.debug_run_count = 0
|
||||
self.sensor2_ready = True
|
||||
self.relay_controller = RelayController()
|
||||
|
||||
# feedConfig 模拟
|
||||
self.feedConfig = SimpleNamespace(feedLine=FeedLine())
|
||||
|
||||
# real_position 模拟
|
||||
self.real_position = Position(100, 200, 300)
|
||||
|
||||
def log_signal(self, level, msg):
|
||||
logging.log(level, msg)
|
||||
|
||||
def next_position(self):
|
||||
self.log_signal(logging.INFO, "[调试] 模拟移动到下一个位置")
|
||||
|
||||
def run_test(self):
|
||||
if self.feedStatus == FeedStatus.FTake:
|
||||
self.log_signal(logging.INFO, Constant.str_feed_take)
|
||||
take_position = self.feedConfig.feedLine.get_take_position()
|
||||
if not take_position or not take_position.get_position():
|
||||
self.log_signal(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
return
|
||||
|
||||
if not take_position.get_position().compare(self.real_position, is_action=True):
|
||||
self.log_signal(logging.INFO, "🟡 机器人尚未到达抓料点位")
|
||||
return
|
||||
|
||||
self.log_signal(logging.INFO, "🟢 机器人已到达抓料点位")
|
||||
|
||||
# 传感器等待逻辑（模拟已触发）
|
||||
sensors = self.relay_controller.get_all_device_status('sensors')
|
||||
sensor2_value = sensors.get("SENSOR2", False)
|
||||
if not sensor2_value:
|
||||
self.log_signal(logging.INFO, "⏳ 等待传感器2料包信号...")
|
||||
return
|
||||
|
||||
self.log_signal(logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取")
|
||||
|
||||
# 执行抓取动作
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
self.catch.catch_status = CatchStatus.CTake
|
||||
if self.catch.catch_status == CatchStatus.CTake:
|
||||
self.log_signal(logging.INFO, "正在执行抓料动作...")
|
||||
self.catch.catch_status = CatchStatus.COk
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
self.log_signal(logging.INFO, Constant.str_feed_take_success)
|
||||
if not self.sensor2_ready:
|
||||
self.log_signal(logging.INFO, "抓取完成，重新启动 conveyor2")
|
||||
self.relay_controller.open(conveyor2=True)
|
||||
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
self.detect.detect_status = DetectStatus.DOk
|
||||
self.log_signal(logging.INFO, "修改抓取点")
|
||||
print(f"[调试] 即将丢包点位索引: {self.detect.position_index}")
|
||||
# ✅ 索引递增
|
||||
self.detect.position_index += 1
|
||||
print(f"[调试] 下一个要加载的索引: {self.detect.position_index}")
|
||||
|
||||
# ✅ 加载新点位
|
||||
if not self.detect.run():
|
||||
self.log_signal(logging.ERROR, "❌ 加载新点位失败，停止流程")
|
||||
return
|
||||
|
||||
# ✅ 设置丢包点
|
||||
self.feedConfig.feedLine.set_drop_position(self.detect.detect_position)
|
||||
|
||||
self.debug_run_count += 1
|
||||
self.next_position()
|
||||
return
|
||||
else:
|
||||
self.log_signal(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
|
||||
# ========================
|
||||
# 主程序入口
|
||||
# ========================
|
||||
if __name__ == "__main__":
|
||||
logging.basicConfig(level=logging.INFO, format='%(asctime)s [%(levelname)s] %(message)s')
|
||||
|
||||
runner = TestRunner()
|
||||
print("🔧 开始测试索引递增和点位加载逻辑...\n")
|
||||
|
||||
for i in range(5):
|
||||
print(f"\n🔄 第 {i+1} 次测试")
|
||||
runner.run_test()
|
||||
941
CU/save_feeding.py
Normal file
941
CU/save_feeding.py
Normal file
@ -0,0 +1,941 @@
|
||||
import copy
|
||||
import logging
|
||||
import random
|
||||
import threading
|
||||
import time
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from PyQt5.QtWidgets import QMessageBox
|
||||
from PySide6.QtCore import Signal, QObject
|
||||
from PySide6.QtGui import QPixmap
|
||||
|
||||
import Constant
|
||||
import Expection
|
||||
from CU.Catch import Catch, CatchStatus
|
||||
from CU.Detect import Detect, DetectStatus
|
||||
from Model.FeedModel import PositionModel
|
||||
from Model.Position import Real_Position, Detection_Position
|
||||
from enum import Enum, IntEnum
|
||||
from COM.COM_Robot import RobotClient, DetectType
|
||||
from Model.RobotModel import CMDInstructRequest, MoveType
|
||||
from Trace.handeye_calibration import getPosition
|
||||
from Trace.handeye_calibration import getxyz1,getxyz
|
||||
from Util.util_math import get_distance
|
||||
from Util.util_time import CRisOrFall
|
||||
#from Vision.camera_coordinate_dete import Detection
|
||||
from Util.util_log import log
|
||||
from Model.RobotModel import Instruction
|
||||
|
||||
|
||||
class ResetStatus(Enum):
|
||||
RNone = 0
|
||||
RStart = 1
|
||||
RRunging = 2
|
||||
ROk = 3
|
||||
|
||||
|
||||
class FeedStatus(IntEnum):
|
||||
FNone = 0
|
||||
FStart = 1
|
||||
FCheck = 2
|
||||
FMid = 3
|
||||
FPhoto = 4
|
||||
FTake = 5
|
||||
FBroken1 = 6
|
||||
FBroken2 = 7
|
||||
FShake = 8
|
||||
FDropBag = 9
|
||||
FFinished = 10
|
||||
FReverse = 11
|
||||
FStartReverse = 12
|
||||
|
||||
|
||||
class LineType(Enum):
|
||||
Straight = 0
|
||||
CureMid = 2
|
||||
CureEnd = 3
|
||||
WORLD = 4
|
||||
|
||||
|
||||
class FeedMidStatus(Enum):
|
||||
FMid_Start = 1
|
||||
FMid_Take = 2
|
||||
FMid_Feed = 3
|
||||
|
||||
|
||||
class FeedPosition:
|
||||
def __init__(self, status: FeedStatus, position: Real_Position):
|
||||
self.status = status
|
||||
self.position = position
|
||||
|
||||
|
||||
class FeedLine:
|
||||
"""
|
||||
FeedLine 类负责管理一个投料路径的所有位置点。
|
||||
它将路径分割成不同的阶段，并提供获取当前/下一个位置的方法。
|
||||
"""
|
||||
|
||||
def __init__(self, id, name, feed_positions: list):
|
||||
"""
|
||||
初始化 FeedLine。
|
||||
:param id: 路径ID
|
||||
:param name: 路径名称
|
||||
:param feed_positions: 包含所有 PositionModel 对象的列表
|
||||
"""
|
||||
# 深拷贝位置列表，避免外部修改影响
|
||||
self.feed_positions = copy.deepcopy(feed_positions)
|
||||
# 初始化各个阶段的位置索引
|
||||
self.feeding2end_pos_index = 0 # 投料到结束的索引 (用于 feeding_to_end 列表)
|
||||
self.origin2start_pos_index = 0 # 原点到起始点的索引 (用于 origin_to_start 列表)
|
||||
self.start2take_pos_index = 0 # 起始点到抓取点的索引 (用于 start_to_take 列表)
|
||||
self.name = name
|
||||
self.id = id
|
||||
|
||||
# 初始化分割后的子列表
|
||||
self.origin_to_start = []
|
||||
self.start_to_take = []
|
||||
self.feeding_to_end = []
|
||||
|
||||
def get_current_feed_position(self, is_reverse):
|
||||
"""
|
||||
获取当前投料阶段的目标位置。
|
||||
:param is_reverse: 是否反向运行 (此参数当前未用于索引计算)
|
||||
:return: PositionModel 对象或 None
|
||||
"""
|
||||
if not self.feeding_to_end or self.feeding2end_pos_index < 0 or self.feeding2end_pos_index >= len(
|
||||
self.feeding_to_end):
|
||||
return None # 或抛出异常
|
||||
# 直接返回当前索引指向的位置
|
||||
return self.feeding_to_end[self.feeding2end_pos_index]
|
||||
|
||||
def get_current_take_position(self, is_reverse):
|
||||
"""获取当前抓取阶段的目标位置 """
|
||||
if not self.start_to_take or self.start2take_pos_index < 0 or self.start2take_pos_index >= len(
|
||||
self.start_to_take):
|
||||
return None # 或抛出异常
|
||||
return self.start_to_take[self.start2take_pos_index]
|
||||
|
||||
def get_current_start_position(self, is_reverse):
|
||||
"""获取当前起始阶段的目标位置 """
|
||||
if not self.origin_to_start or self.origin2start_pos_index < 0 or self.origin2start_pos_index >= len(
|
||||
self.origin_to_start):
|
||||
return None
|
||||
return self.origin_to_start[self.origin2start_pos_index]
|
||||
|
||||
def _update_index(self, index_attr, list_attr, reverse):
|
||||
"""辅助函数：更新索引"""
|
||||
current_index = getattr(self, index_attr)
|
||||
current_list = getattr(self, list_attr)
|
||||
if reverse:
|
||||
current_index -= 1
|
||||
if current_index < 0:
|
||||
current_index = len(current_list) - 1
|
||||
else:
|
||||
current_index += 1
|
||||
if current_index >= len(current_list):
|
||||
current_index = 0
|
||||
setattr(self, index_attr, current_index)
|
||||
return current_index
|
||||
|
||||
def get_next_feed_position(self, reverse: bool = False):
|
||||
"""
|
||||
获取下一个投料位置，并更新索引。
|
||||
这个方法负责处理 feeding_to_end 列表中的所有点，包括 FTake 前后动态点和 FDropBag 点。
|
||||
:param reverse: 是否反向获取
|
||||
:return: PositionModel 对象
|
||||
"""
|
||||
if not self.feeding_to_end or self.feeding2end_pos_index < 0 or self.feeding2end_pos_index >= len(
|
||||
self.feeding_to_end):
|
||||
return None # 或抛出异常
|
||||
# 1. 获取当前索引指向的位置
|
||||
pos = self.feeding_to_end[self.feeding2end_pos_index]
|
||||
# 2. 更新索引 (关键：实现顺序访问)
|
||||
self._update_index('feeding2end_pos_index', 'feeding_to_end', reverse)
|
||||
# 3. 返回之前获取的位置
|
||||
return pos
|
||||
|
||||
def get_next_start_position(self, reverse: bool = False):
|
||||
"""获取下一个起始位置，并更新索引。"""
|
||||
pos = self.origin_to_start[self.origin2start_pos_index]
|
||||
self._update_index('origin2start_pos_index', 'origin_to_start', reverse)
|
||||
return pos
|
||||
|
||||
def get_next_take_position(self, reverse: bool = False):
|
||||
"""获取下一个抓取位置，并更新索引。"""
|
||||
pos = self.start_to_take[self.start2take_pos_index]
|
||||
self._update_index('start2take_pos_index', 'start_to_take', reverse)
|
||||
return pos
|
||||
|
||||
def get_take_position(self):
|
||||
"""
|
||||
查找并返回 FTake 状态的位置。
|
||||
:return: PositionModel 对象或 None
|
||||
"""
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
return self.feeding_to_end[i]
|
||||
|
||||
def set_take_position(self, position: Real_Position, dynamic_height=0):
|
||||
"""
|
||||
设置 FTake 位置，并更新其前后动态点的位置。
|
||||
:param position: 新的抓取位置
|
||||
:param dynamic_height: 动态高度调整 (如果需要)
|
||||
"""
|
||||
for i in range(len(self.feeding_to_end)):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
# 计算 XYZ 坐标
|
||||
#xyz = getxyz(position.X, position.Y, position.Z, position.a, position.b, position.c)
|
||||
xyz = getxyz1(position.X, position.Y, position.Z, position.a, position.b, position.c)
|
||||
# 创建 before 和 after 位置
|
||||
befor_take_position = Real_Position().init_position(xyz[0],
|
||||
xyz[1],
|
||||
xyz[2],
|
||||
position.U,
|
||||
position.V,
|
||||
position.W)
|
||||
after_take_position = Real_Position().init_position(xyz[0],
|
||||
xyz[1],
|
||||
xyz[2],
|
||||
position.U,
|
||||
position.V,
|
||||
position.W)
|
||||
# 安全检查索引
|
||||
if i > 0:
|
||||
self.feeding_to_end[i - 1].set_position(befor_take_position)
|
||||
else:
|
||||
print("Warning: No position before FTake to update.")
|
||||
self.feeding_to_end[i].set_position(position)
|
||||
if i + 1 < len(self.feeding_to_end):
|
||||
self.feeding_to_end[i + 1].set_position(after_take_position)
|
||||
else:
|
||||
print("Warning: No position after FTake to update.")
|
||||
break # 抓料点暂时就一个
|
||||
|
||||
|
||||
|
||||
def get_position_list(self):
|
||||
"""
|
||||
根据状态将 feed_positions 分割成 origin_to_start, start_to_take, feeding_to_end 三个列表。
|
||||
并在 FTake 前后插入动态点。
|
||||
"""
|
||||
index_start = -1
|
||||
index_take = -1
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FCheck.value:
|
||||
index_start = i
|
||||
break
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FPhoto.value:
|
||||
index_take = i
|
||||
|
||||
if index_start == -1 or index_take == -1:
|
||||
print("Error: FCheck or FPhoto position not found in feed_positions.")
|
||||
# 可能需要抛出异常或设置默认值
|
||||
return
|
||||
|
||||
self.origin_to_start = self.feed_positions[: index_start + 1]
|
||||
self.start_to_take = self.feed_positions[index_start:index_take + 1]
|
||||
self.feeding_to_end = self.feed_positions[index_take:]
|
||||
|
||||
# 查找 FTake 并插入动态点
|
||||
take_found = False
|
||||
i = 0
|
||||
while i < len(self.feeding_to_end):
|
||||
if self.feeding_to_end[i].status == FeedStatus.FTake.value:
|
||||
if take_found:
|
||||
print("Warning: Multiple FTake positions found. Processing first one.")
|
||||
break # 或处理多个，或抛出异常
|
||||
|
||||
# 创建动态点
|
||||
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) # 在 FTake 前插入
|
||||
i += 1 # 列表变长，索引后移
|
||||
self.feeding_to_end.insert(i + 1, after_position_model) # 在 FTake 后插入 (原 FTake 现在是 i)
|
||||
# i += 1 # 如果需要继续遍历，可以增加索引
|
||||
take_found = True
|
||||
break # 处理完第一个 FTake 就退出
|
||||
i += 1
|
||||
|
||||
|
||||
class FeedingConfig:
|
||||
"""
|
||||
FeedingConfig 类存储当前投料任务的配置信息，
|
||||
包括剩余数量、路径信息和拍照位置。
|
||||
"""
|
||||
|
||||
def __init__(self, num: int, feedLine: FeedLine, photo_locs):
|
||||
"""
|
||||
初始化投料配置。
|
||||
:param num: 剩余袋数
|
||||
:param feedLine: FeedLine 对象，包含路径信息
|
||||
:param photo_locs: 拍照位置列表
|
||||
"""
|
||||
self.num = num
|
||||
self.feedLine = feedLine
|
||||
self.photo_locs = [self.deal_photo_locs(p) for p in photo_locs]
|
||||
|
||||
def deal_photo_locs(self, photo_loc):
|
||||
"""
|
||||
将元组或列表形式的坐标转换为 Real_Position 对象。
|
||||
:param photo_loc: [X, Y, Z, U, V, W] 的列表或元组
|
||||
:return: 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])
|
||||
return position_photo
|
||||
|
||||
def get_line_info(self):
|
||||
pass
|
||||
|
||||
|
||||
class Feeding(QObject):
|
||||
need_origin_signal = Signal(str)
|
||||
take_no_photo_sigal = Signal()
|
||||
update_detect_image = Signal(np.ndarray)
|
||||
log_signal = Signal(int, str)
|
||||
|
||||
def __init__(self, robotClient: RobotClient):
|
||||
super().__init__()
|
||||
self.feedConfig = None
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.robotClient = robotClient
|
||||
|
||||
self.detection_image = None
|
||||
self.init_detection_image()
|
||||
self.pause = False
|
||||
self.cRis_photo = CRisOrFall()
|
||||
self.cRis_shake = CRisOrFall()
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
self.is_reverse = False
|
||||
# 复位集合
|
||||
self.run_reverse = False
|
||||
self.take_no_photo = False
|
||||
self.reset_status = ResetStatus.RNone
|
||||
self.reversed_positions = []
|
||||
self.current_position = None
|
||||
self.index = 1
|
||||
self.pos_index = -1
|
||||
self.pos_near_index = -1
|
||||
self.catch = Catch(self.robotClient)
|
||||
self.detect = Detect()
|
||||
self.is_detected = True
|
||||
self.detect_thread = threading.Thread(target=self.run_detect)
|
||||
self.detect_thread.start()
|
||||
self.onekey = False
|
||||
self.mid_take_count = 0
|
||||
self.debug_run_count = 0
|
||||
pass
|
||||
|
||||
def close_feed(self):
|
||||
self.is_detected = False
|
||||
self.detect_thread.join()
|
||||
self.detect.detection.release()
|
||||
|
||||
def init_detection_image(self):
|
||||
detection_image = cv2.imread(Constant.feed_sign_path)
|
||||
self.update_detect_image.emit(detection_image)
|
||||
|
||||
def run_detect(self):
|
||||
"""检测线程函数。"""
|
||||
while self.is_detected:
|
||||
self.detect.run()
|
||||
time.sleep(0.02) # 控制检测频率，米厂可以设置较大值
|
||||
|
||||
def run(self):
|
||||
"""
|
||||
主运行逻辑，由外部循环调用。
|
||||
这是一个状态机，根据 self.feedStatus 的值执行不同的操作。
|
||||
"""
|
||||
self.catch.run()
|
||||
# 获取机械臂位置
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
|
||||
# --- 状态机主逻辑 ---
|
||||
if self.feedConfig == None:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
|
||||
# 检查是否完成反向运行并回到原点
|
||||
if self.feedConfig != None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(
|
||||
real_position, is_action=True):
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.is_reverse = False
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
|
||||
|
||||
if self.feedStatus == FeedStatus.FNone or self.pause:
|
||||
return
|
||||
|
||||
elif self.feedStatus == FeedStatus.FCheck:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_check)
|
||||
# 1, 检查是否是三列
|
||||
# 2, 检查是否有人
|
||||
# if self.safe_check_columns() and self.safe_check_person():
|
||||
# pass
|
||||
# else:
|
||||
# if self.feedConfig.num != 0:
|
||||
# self.next_target()
|
||||
# if == 原点 继续判断
|
||||
# else:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_safe_error_msgbox)
|
||||
|
||||
# 根据是否反转，进行状态设置
|
||||
if self.is_reverse:
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
else:
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
|
||||
# 如果在起始点，则移动到下一个位置
|
||||
if self.feedConfig.feedLine.start_to_take[0].get_position().compare(real_position):
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FStart:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_start)
|
||||
# 检查是否在原点 (正向启动时)
|
||||
if not self.robotClient.origin_position.compare(real_position, is_action=True) and not self.is_reverse:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_start_error)
|
||||
self.need_origin_signal.emit(Constant.str_feed_start_error)
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return
|
||||
# 反向运行结束
|
||||
if self.is_reverse:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.is_reverse = False
|
||||
return
|
||||
|
||||
# 初始化位置列表
|
||||
self.feedConfig.feedLine.get_position_list()
|
||||
self.detect.detect_status = DetectStatus.DNone
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Start
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FMid:
|
||||
feed_pos = self.get_current_position(self.is_reverse)
|
||||
if feed_pos.get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
|
||||
self.next_position(self.is_reverse)
|
||||
|
||||
# 增加计数器逻辑
|
||||
self.mid_take_count += 1
|
||||
# 可选：在 Debug1 模式下输出日志
|
||||
if Constant.Debug1:
|
||||
self.log_signal.emit(
|
||||
logging.INFO,
|
||||
f"[调试计数] 已进入 FMid 分支 {self.mid_take_count} 次"
|
||||
)
|
||||
if self.feedStatus == FeedStatus.FTake:
|
||||
self.catch.catch_status = CatchStatus.CTake
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
if self.feedConfig.num == 0:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_finish)
|
||||
self.is_reverse = True
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Take
|
||||
self.feedConfig.feedLine.start2take_pos_index = len(self.feedConfig.feedLine.start_to_take) - 2
|
||||
self.feedConfig.feedLine.origin2start_pos_index = len(self.feedConfig.feedLine.origin_to_start) - 2
|
||||
self.next_position(self.is_reverse)
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
|
||||
self.init_detection_image()
|
||||
return
|
||||
|
||||
|
||||
if not Constant.Debug:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto)
|
||||
self.feed_Mid_Status = FeedMidStatus.FMid_Feed
|
||||
self.detect.detect_status = DetectStatus.DOk
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_success)
|
||||
# ✅ 输出当前抓取的索引
|
||||
print(f"[调试] 即将抓取点位索引: {self.detect.position_index}")
|
||||
self.log_signal.emit(logging.INFO, f"[调试] 当前抓取点位索引: {self.detect.position_index}")
|
||||
# 设置当前抓取位置（使用当前 index）
|
||||
self.feedConfig.feedLine.set_take_position(self.detect.detect_position, self.detect.position_index)
|
||||
# ✅ 递增索引
|
||||
self.detect.position_index += 1
|
||||
# ✅ 输出下一个要加载的索引
|
||||
print(f"[调试] 下一个要加载的索引: {self.detect.position_index}")
|
||||
# ✅ 重新加载下一个点位（基于更新后的 index）
|
||||
self.detect.run()
|
||||
# 增加计数器（用于调试显示执行次数）
|
||||
self.debug_run_count += 1
|
||||
if Constant.Debug1:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing)
|
||||
self.log_signal.emit(logging.INFO, f"[调试计数] 当前已执行 {self.debug_run_count} 次")
|
||||
self.next_position()
|
||||
self.detect.detect_status = DetectStatus.DNone
|
||||
if Constant.Debug1:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing1)
|
||||
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)
|
||||
if self.feedConfig.feedLine.get_take_position().get_position() != None:
|
||||
# 检查是否到达抓取点
|
||||
# self.take_no_photo = False
|
||||
if self.feedConfig.feedLine.get_take_position().get_position().compare(real_position, is_action=True):
|
||||
# 打开吸嘴并返回
|
||||
# self.sendIOControl(self.robotClient.con_ios[0], 1)
|
||||
# self.sendIOControl(self.robotClient.con_ios[1], 1)
|
||||
# self.sendIOControl(self.robotClient.con_ios[2], 1)
|
||||
|
||||
self.log_signal.emit(logging.INFO, "到达抓料点位")
|
||||
# 触发抓取动作
|
||||
# self.feedConfig.feedLine.set_take_position(None)
|
||||
# time.sleep(self.robotClient.time_delay_take)
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
# 抓取完成，移动到下一步
|
||||
self.next_position()
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
if Constant.Debug1:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing3) # 运行到这里了
|
||||
self.feedConfig.feedLine.get_take_position()
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.CTake:
|
||||
# 抓取动作已发出，等待完成 (COk)
|
||||
# self.catch.catch_status 应由 Catch 类在完成后设置为 COk
|
||||
self.catch.catch_status = CatchStatus.COk
|
||||
if Constant.Debug1:
|
||||
self.log_signal.emit(logging.INFO, Constant.str_sys_runing1) # 运行到这里了2
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
# self.take_no_photo = True
|
||||
|
||||
# 继续获取图像
|
||||
# TODO
|
||||
# self.feedConfig.feedLine.set_take_position(self.get_take_position())
|
||||
|
||||
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken2:
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FShake:
|
||||
|
||||
if self.get_current_position().get_position().compare(real_position, is_action=True):
|
||||
# TODO 震动方案
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
self.catch.catch_status = CatchStatus.CShake
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.CShake:
|
||||
# if self.feedConfig.feedLine.feeding_to_end[
|
||||
# self.feedConfig.feedLine.feeding2end_pos_index + 1].status != FeedStatus.FShake:
|
||||
# self.catch.catch_status = CatchStatus.COk
|
||||
# else:
|
||||
self.catch.shake_continue.SetReset()
|
||||
self.next_position()
|
||||
if self.feedStatus != FeedStatus.FShake:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
return
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
# *** 处理投料点 (FDropBag) 的核心逻辑 ***
|
||||
# 1. 确认机械臂是否已精确到达当前目标投料点
|
||||
# get_current_position() 会根据 self.feed_Mid_Status (应为 FMid_Feed)
|
||||
# 调用 feedLine.get_current_feed_position()，从 feeding_to_end 列表获取
|
||||
# 由 feeding2end_pos_index 指向的点。
|
||||
if self.get_current_position().get_position().compare(real_position, is_action=True):
|
||||
# 2. 记录日志：已到达投料点
|
||||
self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
|
||||
# 3. 与 Catch 模块进行状态交互来驱动投料动作
|
||||
# a. 初始状态 (CNone): 触发投料动作
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
# 将 Catch 状态设置为 CDrop，通知 Catch 模块开始执行物理投料动作
|
||||
self.catch.catch_status = CatchStatus.CDrop
|
||||
# 立即返回，等待 Catch 模块处理
|
||||
return
|
||||
|
||||
# b. 投料进行中 (CDrop): 等待完成
|
||||
if self.catch.catch_status == CatchStatus.CDrop:
|
||||
# 什么都不做，等待 Catch 模块完成动作并更新状态
|
||||
return
|
||||
# c. 投料完成 (COk): 处理后续逻辑并移动到下一个点
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
# 重置 Catch 状态，为下一次操作做准备
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
|
||||
# (后续增加) 延时: 让物料稳定
|
||||
# time.sleep(self.robotClient.time_delay_put)
|
||||
|
||||
# (后续增加) 视觉确认: 拍照确认袋子已放置
|
||||
# self.detection.get_position(...)
|
||||
|
||||
# (后续增加) 更新抓取点: 如果需要根据放置情况调整下次抓取
|
||||
# self.feedConfig.feedLine.set_take_position(...)
|
||||
|
||||
# 触发放置后检测 (如果需要，米厂可以不用)
|
||||
if self.detect.detect_status == DetectStatus.DNone:
|
||||
self.detect.detect_status = DetectStatus.DDetect
|
||||
|
||||
# 4. 更新业务逻辑：减少剩余袋数
|
||||
self.feedConfig.num = self.feedConfig.num - 1
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
|
||||
|
||||
# 5. *** 关键步骤 ***: 移动到路径中的下一个点
|
||||
# next_position() 会根据当前的 feed_Mid_Status (FMid_Feed)
|
||||
# 调用 next_Feed()。
|
||||
self.next_position()
|
||||
|
||||
def run_reset(self):
|
||||
"""执行复位操作 (状态机驱动)。"""
|
||||
# 获取当前机械臂位置
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
if self.reset_status == ResetStatus.RNone:
|
||||
# 无复位请求
|
||||
return
|
||||
|
||||
if self.reset_status == ResetStatus.RStart:
|
||||
# 无配置无法复位
|
||||
if self.feedConfig == None: return
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
# if self.feedConfig.feedLine.positions[index].status == 2:
|
||||
# start_index = index
|
||||
self.pos_index = -1
|
||||
self.pos_near_index = -1
|
||||
self.reversed_positions = []
|
||||
|
||||
# 查找当前位置在路径中的索引
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position, is_action=True):
|
||||
self.pos_index = index
|
||||
break
|
||||
|
||||
if self.pos_index == -1:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
|
||||
# 如果未精确找到，查找最近的位置
|
||||
min_distance = 99999999
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if get_distance(pos_model.get_position(), real_position) < min_distance:
|
||||
min_distance = get_distance(pos_model.get_position(), real_position)
|
||||
self.pos_near_index = index
|
||||
# 确定复位路径 (从开始到最近/当前点)
|
||||
if self.pos_near_index != -1:
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_near_index + 1]
|
||||
else:
|
||||
self.log_signal.emit(logging.ERROR, "Cannot determine reset path.")
|
||||
return False
|
||||
else:
|
||||
self.reversed_positions = self.feedConfig.feedLine.feed_positions[:self.pos_index + 1]
|
||||
# 反转路径
|
||||
self.reversed_positions = list(reversed(self.reversed_positions))
|
||||
self.reverse_index = 0 # 复位路径索引
|
||||
self.send_emergency_sound() # 发出急停声音
|
||||
self.current_position = PositionModel()
|
||||
self.current_position.init_position(real_position)
|
||||
self.reset_status = ResetStatus.RRunging # 进入运行状态
|
||||
|
||||
if self.reset_status == ResetStatus.RRunging:
|
||||
if not real_position.compare(self.current_position.get_position(), is_action=True):
|
||||
return
|
||||
pos_model = self.reversed_positions[self.reverse_index]
|
||||
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
|
||||
pos_model = self.reversed_positions[self.reverse_index + 1]
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
|
||||
if pos_model.lineType == LineType.CureMid.value:
|
||||
pos_model1 = self.reversed_positions[self.reverse_index + 1]
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index + 2
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
|
||||
self.current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
|
||||
if self.reverse_index == len(self.reversed_positions):
|
||||
self.reset_status = ResetStatus.ROk
|
||||
if self.reset_status == ResetStatus.ROk:
|
||||
self.reset_status = ResetStatus.RNone
|
||||
self.send_emergency_stop()
|
||||
|
||||
# 废弃代码
|
||||
def return_original_position(self):
|
||||
|
||||
self.run_reverse = True
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
self.robotClient.status_model.world_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
if self.feedConfig == None: return
|
||||
start_index = -1
|
||||
# for index in range(len(self.feedConfig.feedLine.positions)):
|
||||
# if self.feedConfig.feedLine.positions[index].status == 2:
|
||||
# start_index = index
|
||||
pos_index = -1
|
||||
pos_near_index = -1
|
||||
reversed_positions = []
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if pos_model.get_position().compare(real_position):
|
||||
pos_index = index
|
||||
break
|
||||
|
||||
if pos_index == -1:
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_return_original_position_fail)
|
||||
min_distance = 99999999
|
||||
for index, pos_model in enumerate(self.feedConfig.feedLine.feed_positions):
|
||||
if get_distance(pos_model.get_position(), real_position) < min_distance:
|
||||
min_distance = get_distance(pos_model.get_position(), real_position)
|
||||
pos_near_index = index
|
||||
if pos_near_index != -1:
|
||||
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_near_index + 1]
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
reversed_positions = self.feedConfig.feedLine.feed_positions[:pos_index]
|
||||
reversed_positions = list(reversed(reversed_positions))
|
||||
self.reverse_index = 0
|
||||
self.send_emergency_sound()
|
||||
current_position = PositionModel()
|
||||
current_position.init_position(real_position)
|
||||
while self.run_reverse and self.reverse_index != len(reversed_positions):
|
||||
if self.reverse_index != 0 and not real_position.compare(current_position.get_position()):
|
||||
continue
|
||||
# todo 缺少比对
|
||||
pos_model = reversed_positions[self.reverse_index]
|
||||
if pos_model.status == FeedStatus.FTake.value: # 跳过取袋节点
|
||||
|
||||
pos_model = reversed_positions[self.reverse_index + 1]
|
||||
# TODO take节点判断
|
||||
|
||||
if pos_model.lineType == LineType.CureMid.value:
|
||||
pos_model1 = reversed_positions[self.reverse_index + 1]
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=pos_model1.get_position(), speed=self.robotClient.reset_speed)
|
||||
current_position = pos_model1
|
||||
self.reverse_index = self.reverse_index + 2
|
||||
else:
|
||||
self.sendTargPosition(real_position=pos_model.get_position(), speed=self.robotClient.reset_speed)
|
||||
current_position = pos_model
|
||||
self.reverse_index = self.reverse_index + 1
|
||||
self.send_emergency_stop()
|
||||
return True
|
||||
|
||||
def send_emergency_sound(self):
|
||||
"""发送急停声音 IO 控制。"""
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 1)
|
||||
|
||||
def send_emergency_stop(self):
|
||||
"""停止急停声音 IO 控制。"""
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 0)
|
||||
|
||||
def sendIOControl(self, IO_bit, IO_Status: int):
|
||||
"""发送 IO 控制指令。"""
|
||||
IO_command = CMDInstructRequest()
|
||||
io_instruction = Instruction()
|
||||
io_instruction.IO = True
|
||||
io_instruction.io_status = IO_Status
|
||||
io_instruction.point = IO_bit # {"dsID":"HCRemoteCommand","reqType":"AddRCC","emptyList":"1","instructions":[{"oneshot":"1","action":"200","type":"0","io_status":"1","point":"15","delay":"0"}]}
|
||||
IO_command.dsID = 'HCRemoteCommand'
|
||||
IO_command.instructions.append(io_instruction)
|
||||
self.robotClient.add_sendQuene(IO_command.toString())
|
||||
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_io_control}{IO_bit}，{IO_Status}')
|
||||
pass
|
||||
|
||||
def sendTargPosition(self, real_position, move_type: MoveType = MoveType.WORLD, speed=5, real_position1=None):
|
||||
"""发送目标位置移动指令。"""
|
||||
position_instruction = Instruction()
|
||||
position_instruction.speed = speed
|
||||
position_instruction.m0 = real_position.X
|
||||
position_instruction.m1 = real_position.Y
|
||||
position_instruction.m2 = real_position.Z
|
||||
position_instruction.m3 = real_position.U
|
||||
position_instruction.m4 = real_position.V
|
||||
position_instruction.m5 = real_position.W
|
||||
position_instruction.smooth = self.robotClient.smooth
|
||||
|
||||
position_instruction.action = move_type.value
|
||||
if position_instruction.action == 17:
|
||||
position_instruction.m0_p = real_position1.X
|
||||
position_instruction.m1_p = real_position1.Y
|
||||
position_instruction.m2_p = real_position1.Z
|
||||
position_instruction.m3_p = real_position1.U
|
||||
position_instruction.m4_p = real_position1.V
|
||||
position_instruction.m5_p = real_position1.W
|
||||
instruction_command = CMDInstructRequest()
|
||||
instruction_command.instructions.append(position_instruction)
|
||||
request_command = instruction_command.toString()
|
||||
|
||||
log_str = f'移动到位置:{"姿势直线" if move_type == MoveType.WORLD else "姿势曲线"}：' \
|
||||
f'X:{position_instruction.m0}-' \
|
||||
f'Y:{position_instruction.m1}-' \
|
||||
f'Z:{position_instruction.m2}-' \
|
||||
f'U:{position_instruction.m3}-' \
|
||||
f'V:{position_instruction.m4}-' \
|
||||
f'W:{position_instruction.m5}'
|
||||
|
||||
try:
|
||||
self.log_signal.emit(logging.INFO, log_str)
|
||||
except Exception as e:
|
||||
self.log_signal.emit(logging.ERROR, f"Logging error in sendTargPosition: {e}")
|
||||
return # 如果日志发送失败，至少不要中断主流程
|
||||
print(request_command)
|
||||
self.robotClient.add_sendQuene(request_command)
|
||||
|
||||
# def get_take_position(self):
|
||||
# if Constant.Debug:
|
||||
# return self.robotClient.status_model.getRealPosition()
|
||||
# _, img, xyz, uvw, points = self.detection.get_position(Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30,save_img_point=0, Height_reduce=30, width_reduce=30)
|
||||
# self.detection_image = img.copy()
|
||||
# if xyz ==None or uvw==None or points==None:
|
||||
# return None
|
||||
# target_position,noraml_base = getPosition(*xyz,*uvw,None,points)
|
||||
#
|
||||
# position = Real_Position().init_position(*target_position[:3],*noraml_base[:3])
|
||||
# position.Z = position.Z+200
|
||||
# return position
|
||||
|
||||
def next_start(self, reverse=False):
|
||||
"""移动到下一个起始位置。"""
|
||||
start_pos = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
self.feedStatus = FeedStatus(start_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
if start_pos.lineType == LineType.CureMid.value:
|
||||
start_pos1 = self.feedConfig.feedLine.get_next_start_position(reverse)
|
||||
self.feedStatus = FeedStatus(start_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=start_pos.get_position(), speed=self.robotClient.reset_speed,
|
||||
move_type=MoveType.Cure, real_position1=start_pos1.get_position())
|
||||
elif start_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(start_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(), speed=self.robotClient.reset_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
else:
|
||||
self.sendTargPosition(real_position=start_pos.get_position(), speed=self.robotClient.reset_speed)
|
||||
pass
|
||||
|
||||
def next_take(self, reverse=False):
|
||||
"""移动到下一个抓取位置。"""
|
||||
take_pos = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
if take_pos.lineType == LineType.CureMid.value:
|
||||
take_pos1 = self.feedConfig.feedLine.get_next_take_position(reverse)
|
||||
self.feedStatus = FeedStatus(take_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=take_pos1.get_position(), speed=self.robotClient.feed_speed)
|
||||
elif take_pos.lineType == LineType.WORLD.value:
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(take_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
self.sendTargPosition(real_position=take_pos.get_position(), speed=self.robotClient.feed_speed)
|
||||
pass
|
||||
|
||||
def next_Feed(self, reverse=False):
|
||||
"""
|
||||
移动到 feeding_to_end 列表中的下一个位置。
|
||||
这个方法负责处理所有在 feeding_to_end 列表中的点，包括 FTake 前后的动态点、FDropBag 点等。
|
||||
"""
|
||||
# 1. 调用 FeedLine 的方法获取下一个位置对象 (并更新内部索引)
|
||||
# get_next_feed_position 会返回当前索引指向的点，
|
||||
# 然后根据 reverse 参数更新 feeding2end_pos_index。
|
||||
feed_pos = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
|
||||
# 2. 更新主状态机，使其与即将移动到的位置状态一致
|
||||
self.feedStatus = FeedStatus(feed_pos.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
# 3. 根据位置的 lineType 发送相应的移动指令
|
||||
if feed_pos.lineType == LineType.CureMid.value:
|
||||
# 处理曲线运动 (需要两个点)
|
||||
feed_pos1 = self.feedConfig.feedLine.get_next_feed_position(reverse)
|
||||
self.feedStatus = FeedStatus(feed_pos1.status) if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), move_type=MoveType.Cure,
|
||||
real_position1=feed_pos1.get_position(), speed=self.robotClient.feed_speed)
|
||||
elif feed_pos.lineType == LineType.WORLD.value:
|
||||
# 处理直线/关节运动
|
||||
if self.robotClient.status_model.getAnglePosition().is_error_angel_move(feed_pos.get_position(),
|
||||
self.robotClient.max_angle_interval):
|
||||
self.feedStatus = None
|
||||
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
|
||||
else:
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed,
|
||||
move_type=MoveType.AXIS)
|
||||
|
||||
else:
|
||||
# 默认处理 (通常是直线 WORLD 运动)
|
||||
# *** 处理 FDropBag 点 ***
|
||||
# 当 feeding_to_end 列表中的点是 FDropBag (status=9) 时，
|
||||
# 这段默认代码会被执行，发送移动到该点的指令。
|
||||
# 一旦机械臂到达该点，run() 方法的 FDropBag 分支就会被触发，
|
||||
# 完成投料后，next_position() 又会被调用，继续移动到列表中的下一个点
|
||||
|
||||
self.sendTargPosition(real_position=feed_pos.get_position(), speed=self.robotClient.feed_speed)
|
||||
|
||||
def get_current_position(self, is_reverse=False):
|
||||
"""
|
||||
根据当前子阶段状态获取对应的目标位置。
|
||||
:param is_reverse: 是否反向 (传递给 FeedLine 方法)
|
||||
:return: PositionModel 对象或 None
|
||||
"""
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
return self.feedConfig.feedLine.get_current_start_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
return self.feedConfig.feedLine.get_current_take_position(self.is_reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
return self.feedConfig.feedLine.get_current_feed_position(self.is_reverse)
|
||||
|
||||
def next_position(self, reverse=False):
|
||||
"""
|
||||
根据当前子阶段状态移动到下一个位置。
|
||||
这是驱动整个喂料流程前进的核心方法。
|
||||
:param reverse: 是否反向移动
|
||||
"""
|
||||
if self.feed_Mid_Status == FeedMidStatus.FMid_Start:
|
||||
self.next_start(reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Take:
|
||||
self.next_take(reverse)
|
||||
elif self.feed_Mid_Status == FeedMidStatus.FMid_Feed:
|
||||
# *** 关键调用 ***:
|
||||
# 当 feed_Mid_Status 是 FMid_Feed 时，next_position() 会调用 next_Feed()。
|
||||
# next_Feed() 内部调用 feedLine.get_next_feed_position()，该方法会获取
|
||||
# feeding_to_end 列表中当前索引指向的点，并将索引递增，从而实现按顺序访问。
|
||||
# 这使得 FTake 前后动态点和 FDropBag 点都能被顺序处理。
|
||||
self.next_Feed(reverse)
|
||||
|
||||
def safe_check_columns(self):
|
||||
return True
|
||||
pass
|
||||
|
||||
def safe_check_person(self):
|
||||
return True
|
||||
pass
|
||||
34
CU/test_Catch_EMV.py
Normal file
34
CU/test_Catch_EMV.py
Normal file
@ -0,0 +1,34 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
from unittest.mock import patch, MagicMock
|
||||
|
||||
# 假设你的模块名为 CU，Catch 定义在 CU/Catch.py
|
||||
from CU.Catch import Catch, CatchStatus
|
||||
|
||||
class TestCatchRelayControl(unittest.TestCase):
|
||||
|
||||
@patch('CU.EMV.close') # 只mock close函数，因为我们只关心关闭电磁阀1
|
||||
def test_catch_status_ctake_calls_close_relay(self, mock_close):
|
||||
# 创建一个 mock 的 RobotClient（不需要真实实现）
|
||||
mock_robot_client = MagicMock()
|
||||
mock_robot_client.time_delay_put = 0.5
|
||||
mock_robot_client.con_ios = [0, 1, 2]
|
||||
|
||||
# 实例化 Catch 类
|
||||
catch_instance = Catch(mock_robot_client)
|
||||
|
||||
# 设置状态为 CTake
|
||||
catch_instance.catch_status = CatchStatus.CTake
|
||||
|
||||
# 第一次 run，应该触发 close(1, 0, 0)
|
||||
catch_instance.run()
|
||||
|
||||
# 验证 close 是否被调用，并且是 close(1, 0, 0)
|
||||
mock_close.assert_called_once_with(1, 0, 0)
|
||||
|
||||
# 再次 run，不应再次调用 close
|
||||
catch_instance.run()
|
||||
self.assertEqual(mock_close.call_count, 1) # 确保只调用了一次
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
90
CU/test_Detect.py
Normal file
90
CU/test_Detect.py
Normal file
@ -0,0 +1,90 @@
|
||||
import os
|
||||
import configparser
|
||||
|
||||
# 模拟 Real_Position 类
|
||||
class Real_Position:
|
||||
def __init__(self):
|
||||
self.X = 0
|
||||
self.Y = 0
|
||||
self.Z = 0
|
||||
self.A = 0
|
||||
self.B = 0
|
||||
self.C = 0
|
||||
|
||||
def init_position(self, x, y, z, a, b, c):
|
||||
self.X = x
|
||||
self.Y = y
|
||||
self.Z = z
|
||||
self.A = a
|
||||
self.B = b
|
||||
self.C = c
|
||||
|
||||
def get_position(self):
|
||||
return self
|
||||
|
||||
def __str__(self):
|
||||
return f"X:{self.X:.3f}, Y:{self.Y:.3f}, Z:{self.Z:.3f}, A:{self.A:.3f}, B:{self.B:.3f}, C:{self.C:.3f}"
|
||||
|
||||
|
||||
# 模拟 DetectStatus
|
||||
class DetectStatus:
|
||||
DOk = "DOk"
|
||||
|
||||
|
||||
# 实际要测试的类
|
||||
class Detect:
|
||||
def __init__(self):
|
||||
self.detection = ""
|
||||
self.detect_status = DetectStatus.DOk
|
||||
self.detect_position = None
|
||||
self.position_index = 0 # 默认读取索引为 0 的点位
|
||||
|
||||
def run(self):
|
||||
if self.detect_status == DetectStatus.DOk:
|
||||
self.detect_position = None
|
||||
config = configparser.ConfigParser()
|
||||
config_file = os.path.join(os.path.dirname(__file__), 'list.ini') # 配置文件地址
|
||||
if not os.path.exists(config_file):
|
||||
print("配置文件 list.ini 不存在")
|
||||
return False
|
||||
config.read(config_file)
|
||||
if not config.has_section('positions'):
|
||||
print("配置文件中没有 [positions] 段")
|
||||
return False
|
||||
if not config.has_option('positions', str(self.position_index)):
|
||||
print(f"没有索引为 {self.position_index} 的点位")
|
||||
return False
|
||||
try:
|
||||
# 读取配置项
|
||||
data = config.get('positions', str(self.position_index)).strip().split(',')
|
||||
if len(data) != 6:
|
||||
raise ValueError(f"点位数据格式错误（应为6个值）: {data}")
|
||||
|
||||
x, y, z, a, b, c = map(float, data)
|
||||
# 初始化坐标
|
||||
self.detect_position = Real_Position()
|
||||
self.detect_position.init_position(x, y, z, a, b, c)
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
print(f"读取点位时出错: {e}")
|
||||
return False
|
||||
return False
|
||||
|
||||
|
||||
# ========================
|
||||
# 测试逻辑
|
||||
# ========================
|
||||
if __name__ == "__main__":
|
||||
detect = Detect()
|
||||
|
||||
print("🔧 开始测试 Detect.run() 是否能正确加载点位...\n")
|
||||
|
||||
for index in range(12): # 测试索引 0~11
|
||||
detect.position_index = index
|
||||
print(f"\n🔄 测试索引: {index}")
|
||||
success = detect.run()
|
||||
if success:
|
||||
print(f"✅ 加载成功: {detect.detect_position}")
|
||||
else:
|
||||
print(f"❌ 加载失败（可能是越界或配置错误）")
|
||||
Reference in New Issue
Block a user