更新控制
This commit is contained in:
FrankCV2048
114
CU/Feeding.py
114
CU/Feeding.py
@ -2,12 +2,15 @@ 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
|
||||
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
|
||||
|
||||
@ -38,6 +41,7 @@ class FeedLine:
|
||||
self.name = name
|
||||
self.id=id
|
||||
|
||||
|
||||
class FeedingConfig:
|
||||
def __init__(self, num:int, feedLine:FeedLine,photo_locs):
|
||||
self.num = num
|
||||
@ -62,6 +66,7 @@ class Feeding :
|
||||
self.detection_image = None
|
||||
self.init_detection_image()
|
||||
self.pause = False
|
||||
self.cRis_photo = CRisOrFall()
|
||||
pass
|
||||
|
||||
def init_detection_image(self):
|
||||
@ -108,6 +113,7 @@ class Feeding :
|
||||
elif self.feedStatus==FeedStatus.FSafeP:
|
||||
log.log_message(logging.INFO,Constant.str_feed_safe)
|
||||
if self.feedConfig.feedLine.safe_position.compare(real_position):
|
||||
self.error_photo_count=0
|
||||
self.feedStatus = FeedStatus.FPhoto
|
||||
#self.sendTargPosition(self.feedConfig.feedLine.photo_position)
|
||||
#判断是哪一个问题
|
||||
@ -115,51 +121,67 @@ class Feeding :
|
||||
elif self.feedStatus == FeedStatus.FPhoto:
|
||||
log.log_message(logging.INFO, Constant.str_feed_photo)
|
||||
detect_pos_list = []
|
||||
for pos in self.feedConfig.photo_locs:
|
||||
self.sendTargPosition(pos)
|
||||
while not pos.compare(real_position): #可以优化 TODO
|
||||
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 - 10, #TODO 这个10 需要确定
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
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)
|
||||
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 - 10, #TODO 这个10 需要确定
|
||||
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, Constant.str_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]]
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[0]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_front_finish)
|
||||
else:
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_front)
|
||||
# 黄老师给我的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, Constant.str_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]]
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[0]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_front_finish)
|
||||
else:
|
||||
self.feedConfig.feedLine.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:
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[0]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_new_line)
|
||||
else:
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_line)
|
||||
self.feedStatus = FeedStatus.FTake
|
||||
self.sendTargPosition(self.feedConfig.feedLine.take_position)
|
||||
except:
|
||||
log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||||
self.error_photo_count +=1
|
||||
else:
|
||||
if z_diff < Constant.bag_height:
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[0]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_new_line)
|
||||
else:
|
||||
self.feedConfig.feedLine.take_position = detect_pos_list[max_z_index]
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_line)
|
||||
self.feedStatus = FeedStatus.FTake
|
||||
self.sendTargPosition(self.feedConfig.feedLine.take_position)
|
||||
self.feedConfig.feedLine.take_position = self.feedConfig.feedLine.safe_position
|
||||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||||
self.feedStatus = FeedStatus.FTake
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_move)
|
||||
|
||||
elif self.feedStatus == FeedStatus.FTake:
|
||||
@ -170,7 +192,7 @@ class Feeding :
|
||||
self.sendIOControl(self.robotClient.con_ios[1],1)
|
||||
self.sendIOControl(self.robotClient.con_ios[2],1)
|
||||
# TODO 检测是否通 不然报警
|
||||
time.sleep(2)
|
||||
time.sleep(self.robotClient.time_delay_take)
|
||||
self.feedStatus = FeedStatus.FSafeF
|
||||
log.log_message(logging.INFO, Constant.str_feed_take_success)
|
||||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||||
@ -198,7 +220,7 @@ class Feeding :
|
||||
self.sendTargPosition(self.feedConfig.feedLine.shake_position)
|
||||
if self.feedConfig.feedLine.shake_position.compare(real_position): # 延迟判断，如果最后点位延迟1s，则认为阶段完成
|
||||
# TODO 震动方案
|
||||
time.sleep(2)
|
||||
time.sleep(self.robotClient.time_delay_shake)
|
||||
self.feedStatus = FeedStatus.FDropBag
|
||||
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
|
||||
|
||||
@ -212,7 +234,7 @@ class Feeding :
|
||||
self.sendIOControl(self.robotClient.con_ios[1], 0)
|
||||
self.sendIOControl(self.robotClient.con_ios[2], 0)
|
||||
# TODO 检测是否断 不然报警
|
||||
time.sleep(2)
|
||||
time.sleep(self.robotClient.time_delay_put)
|
||||
self.feedConfig.num = self.feedConfig.num - 1
|
||||
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
|
||||
if self.feedConfig.num == 0: # 投料次数为0，直接返回
|
||||
|
||||
Reference in New Issue
Block a user