update 更新部署
This commit is contained in:
FrankCV2048
16
CU/Catch.py
16
CU/Catch.py
@ -1,5 +1,6 @@
|
||||
from enum import Enum
|
||||
|
||||
import Constant
|
||||
from COM.COM_Robot import RobotClient
|
||||
from Util.util_time import CClockPulse, CTon
|
||||
|
||||
@ -11,7 +12,7 @@ class CatchStatus(Enum):
|
||||
CShake = 3
|
||||
COk = 4
|
||||
|
||||
class catch:
|
||||
class Catch:
|
||||
def __init__(self, robotClient: RobotClient):
|
||||
self.robotClient = robotClient
|
||||
self.catch_status = CatchStatus.CNone
|
||||
@ -24,26 +25,27 @@ class catch:
|
||||
|
||||
if self.catch_status == CatchStatus.CTake:
|
||||
self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
|
||||
if self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
|
||||
if self.robotClient.check_outputQ(self.robotClient.con_ios[0]) or Constant.Debug:
|
||||
self.catch_status = CatchStatus.COk
|
||||
|
||||
if self.catch_status == CatchStatus.CDrop:
|
||||
self.robotClient.sendIOControl(self.robotClient.con_ios[0],0)
|
||||
self.robotClient.sendIOControl(self.robotClient.con_ios[1],1)
|
||||
if self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
|
||||
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.catch_status = CatchStatus.COk
|
||||
if self.catch_status == CatchStatus.CShake:
|
||||
if not self.shake_continue.Q(True, 3000):
|
||||
shakeQ = self.shakePulse.Q(True, 1000, 1000)
|
||||
if not self.shake_continue.Q(True, 2000):
|
||||
shakeQ = self.shakePulse.Q(True, 100, 100)
|
||||
self.robotClient.sendIOControl(self.robotClient.con_ios[2], shakeQ)
|
||||
print("正在震动" if shakeQ else "震动结束")
|
||||
else:
|
||||
self.shake_continue.SetReset()
|
||||
self.catch_status = CatchStatus.COk
|
||||
if self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
|
||||
if Constant.Debug or self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
|
||||
self.robotClient.sendIOControl(self.robotClient.con_ios[2], False)
|
||||
print("震动结束")
|
||||
|
||||
if self.catch_status == CatchStatus.COk:
|
||||
|
||||
pass
|
||||
|
||||
|
||||
|
||||
@ -3,9 +3,11 @@ import time
|
||||
|
||||
import cv2
|
||||
from PyQt5.QtWidgets import QMessageBox
|
||||
from PySide6.QtCore import Signal, QObject
|
||||
|
||||
import Constant
|
||||
import Expection
|
||||
from CU.Catch import Catch, CatchStatus
|
||||
from Model.FeedModel import PositionModel
|
||||
from Model.Position import Real_Position, Detection_Position
|
||||
from enum import Enum, IntEnum
|
||||
@ -156,8 +158,11 @@ class FeedingConfig:
|
||||
pass
|
||||
|
||||
|
||||
class Feeding:
|
||||
class Feeding(QObject):
|
||||
need_origin_signal = Signal(str)
|
||||
take_no_photo_sigal = Signal(str)
|
||||
def __init__(self, robotClient: RobotClient, detection: Detection):
|
||||
super().__init__()
|
||||
self.feedConfig = None
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.robotClient = robotClient
|
||||
@ -177,12 +182,14 @@ class Feeding:
|
||||
|
||||
self.pos_index = -1
|
||||
self.pos_near_index = -1
|
||||
self.catch = Catch(self.robotClient)
|
||||
pass
|
||||
|
||||
def init_detection_image(self):
|
||||
self.detection_image = cv2.imread(Constant.feed_sign_path)
|
||||
|
||||
def run(self):
|
||||
self.catch.run()
|
||||
# 获取事件坐标
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
@ -233,6 +240,7 @@ class Feeding:
|
||||
log.log_message(logging.INFO, Constant.str_feed_start)
|
||||
if not self.robotClient.origin_position.compare(real_position) and not self.is_reverse:
|
||||
# QMessageBox.information(None, "提示", Constant.str_feed_start_error) # Fuck 引起异常
|
||||
self.need_origin_signal.emit(Constant.str_feed_start_error)
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
return
|
||||
if self.is_reverse:
|
||||
@ -270,7 +278,6 @@ class Feeding:
|
||||
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)
|
||||
|
||||
return
|
||||
|
||||
if self.robotClient.type_detection == DetectType.EyeOutHand:
|
||||
@ -347,21 +354,31 @@ class Feeding:
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
log.log_message(logging.INFO, Constant.str_feed_take)
|
||||
if self.feedConfig.feedLine.get_take_position().get_position() != None:
|
||||
self.take_no_photo = False
|
||||
# self.take_no_photo = False
|
||||
if self.feedConfig.feedLine.get_take_position().get_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)
|
||||
self.next_position()
|
||||
|
||||
|
||||
if self.catch.catch_status == CatchStatus.CNone :
|
||||
self.catch.catch_status = CatchStatus.CTake
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.CTake:
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.COk :
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
self.feedConfig.feedLine.set_take_position(None)
|
||||
time.sleep(self.robotClient.time_delay_take)
|
||||
log.log_message(logging.INFO, Constant.str_feed_take_success)
|
||||
self.next_position()
|
||||
|
||||
else:
|
||||
log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
self.take_no_photo = True
|
||||
# self.take_no_photo = True
|
||||
self.take_no_photo_sigal.emit()
|
||||
# 继续获取图像
|
||||
# TODO
|
||||
|
||||
@ -383,8 +400,17 @@ class Feeding:
|
||||
log.log_message(logging.INFO, Constant.str_feed_shake)
|
||||
if self.get_current_position().get_position().compare(real_position):
|
||||
# TODO 震动方案
|
||||
time.sleep(self.robotClient.time_delay_shake)
|
||||
self.next_position()
|
||||
|
||||
|
||||
if self.catch.catch_status == CatchStatus.CNone:
|
||||
self.catch.catch_status = CatchStatus.CShake
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.CShake:
|
||||
return
|
||||
if self.catch.catch_status == CatchStatus.COk:
|
||||
self.catch.catch_status = CatchStatus.CNone
|
||||
time.sleep(self.robotClient.time_delay_shake)
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
log.log_message(logging.INFO, Constant.str_feed_drop)
|
||||
@ -393,16 +419,23 @@ class Feeding:
|
||||
# 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.feedLine.set_take_position(self.get_take_position())
|
||||
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 run_reseet(self):
|
||||
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())
|
||||
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 run_reset(self):
|
||||
real_position = Real_Position()
|
||||
real_position.init_position(self.robotClient.status_model.world_0,
|
||||
self.robotClient.status_model.world_1,
|
||||
@ -540,7 +573,7 @@ class Feeding:
|
||||
self.send_emergency_stop()
|
||||
return True
|
||||
def send_emergency_sound(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 1)
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 2)
|
||||
|
||||
def send_emergency_stop(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 0)
|
||||
@ -597,8 +630,12 @@ class Feeding:
|
||||
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
|
||||
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])
|
||||
|
||||
Reference in New Issue
Block a user