update 完成点位控制逻辑，UI可视化界面生成
This commit is contained in:
FrankCV2048
201
CU/Feeding_C.py
201
CU/Feeding_C.py
@ -8,7 +8,7 @@ import Constant
|
||||
import Expection
|
||||
from Model.Position import Real_Position, Detection_Position
|
||||
from enum import Enum, IntEnum
|
||||
from COM.COM_Robot import RobotClient
|
||||
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
|
||||
@ -30,6 +30,13 @@ class FeedStatus(IntEnum):
|
||||
FShake = 8
|
||||
FDropBag = 9
|
||||
FFinished = 10
|
||||
FReverse = 11
|
||||
FStartReverse = 12
|
||||
|
||||
class FeedMidStatus(Enum):
|
||||
FMid_Start = 1
|
||||
FMid_Take = 2
|
||||
FMid_Feed= 3
|
||||
|
||||
class FeedPosition:
|
||||
def __init__(self,status:FeedStatus,position:Real_Position):
|
||||
@ -46,22 +53,52 @@ class FeedLine:
|
||||
self.id = id
|
||||
self.get_position_list()
|
||||
|
||||
def get_current_position(self):
|
||||
def get_current_feed_position(self):
|
||||
pos = self.feeding_to_end[self.feeding2end_pos_index]
|
||||
return pos
|
||||
def get_next_feed_position(self):
|
||||
pos = self.feeding_to_end[self.feeding2end_pos_index]
|
||||
self.feeding2end_pos_index += 1
|
||||
return pos
|
||||
|
||||
def get_next_start_position(self):
|
||||
pos = self.origin2start_pos_index[self.origin2start_pos_index]
|
||||
self.origin2start_pos_index += 1
|
||||
return pos
|
||||
|
||||
def get_next_take_position(self):
|
||||
def get_current_take_position(self):
|
||||
pos = self.start2take_pos_index[self.start2take_pos_index]
|
||||
self.start2take_pos_index += 1
|
||||
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):
|
||||
@ -81,7 +118,7 @@ class FeedLine:
|
||||
index_start = i
|
||||
break
|
||||
for i in range(len(self.feed_positions)):
|
||||
if self.feed_positions[i].status == FeedStatus.FTake:
|
||||
if self.feed_positions[i].status == FeedStatus.FPhoto:
|
||||
index_take = i
|
||||
|
||||
self.origin_to_start = self.feed_positions[: index_start+1]
|
||||
@ -116,7 +153,8 @@ class Feeding:
|
||||
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):
|
||||
@ -140,17 +178,11 @@ class Feeding:
|
||||
|
||||
if self.feedConfig == None:
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
elif self.feedConfig.num == 0:
|
||||
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.FStart:
|
||||
log.log_message(logging.INFO, Constant.str_feed_start)
|
||||
self.feedStatus = FeedStatus.FCheck if self.feedStatus != FeedStatus.FNone else FeedStatus.FNone
|
||||
pass
|
||||
elif self.feedStatus == FeedStatus.FCheck:
|
||||
log.log_message(logging.INFO, Constant.str_feed_check)
|
||||
# 1, 检查是否是三列
|
||||
@ -158,21 +190,51 @@ class Feeding:
|
||||
# if self.safe_check_columns() and self.safe_check_person():
|
||||
# pass
|
||||
# else:
|
||||
if self.feedConfig.num != 0:
|
||||
self.next_target()
|
||||
# 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.feedConfig.feedLine.get_current_position()
|
||||
feed_pos = self.get_current_position()
|
||||
if feed_pos.position.compare(real_position):
|
||||
self.next_target()
|
||||
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:
|
||||
@ -197,7 +259,7 @@ class Feeding:
|
||||
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_2,
|
||||
self.robotClient.status_model.world_3,
|
||||
self.robotClient.status_model.world_4,
|
||||
self.robotClient.status_model.world_5)
|
||||
@ -231,66 +293,67 @@ class Feeding:
|
||||
log.log_message(logging.INFO, Constant.str_feed_takePhoto_line)
|
||||
|
||||
self.feedConfig.feedLine.set_take_position(take_position) ##TODO 检查有没有异常
|
||||
self.next_target()
|
||||
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_target()
|
||||
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.take_position != None and self.feedConfig.feedLine.get_current_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)
|
||||
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 检测是否通 不然报警
|
||||
# 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)
|
||||
|
||||
time.sleep(self.robotClient.time_delay_take)
|
||||
log.log_message(logging.INFO, Constant.str_feed_take_success)
|
||||
self.next_target()
|
||||
|
||||
|
||||
elif self.feedStatus == FeedStatus.FBroken1:
|
||||
log.log_message(logging.INFO, Constant.str_feed_broken)
|
||||
if self.feedConfig.feedLine.get_current_position.compare(real_position):
|
||||
self.next_target()
|
||||
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.feedConfig.feedLine.get_current_position.compare(real_position):
|
||||
self.next_target()
|
||||
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.feedConfig.feedLine.get_current_position.compare(real_position):
|
||||
if self.get_current_position().compare(real_position):
|
||||
# TODO 震动方案
|
||||
time.sleep(self.robotClient.time_delay_shake)
|
||||
self.next_target()
|
||||
self.next_position()
|
||||
|
||||
elif self.feedStatus == FeedStatus.FDropBag:
|
||||
log.log_message(logging.INFO, Constant.str_feed_drop)
|
||||
|
||||
if self.feedConfig.feedLine.get_current_position.compare(real_position):
|
||||
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()
|
||||
|
||||
|
||||
|
||||
if self.feedConfig.num ==0 and self.feedConfig.feedLine.pos_index == 0:
|
||||
log.log_message(logging.INFO, Constant.str_feed_finish)
|
||||
self.feedStatus = FeedStatus.FNone
|
||||
self.init_detection_image()
|
||||
|
||||
def send_emergency_sound(self):
|
||||
self.sendIOControl(Constant.IO_EmergencyPoint, 1)
|
||||
@ -341,11 +404,39 @@ class Feeding:
|
||||
self.robotClient.add_sendQuene(request_command)
|
||||
pass
|
||||
|
||||
def
|
||||
def next_target(self):
|
||||
feed_pos = self.feedConfig.feedLine.get_next_position()
|
||||
self.feedStatus = feed_pos.status
|
||||
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user