xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update 更新信息填写

Browse Source
This commit is contained in:
FrankCV2048
2024-12-21 14:19:27 +08:00
parent cd7e354cbd
commit c7edb09b08
4 changed files with 297 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
CU/Feeding.py
View File

@ -196,6 +196,7 @@ 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
@ -268,13 +269,13 @@ class Feeding(QObject):
if self.feedConfig !=None and self.feedConfig.num == 0 and self.is_reverse and self.robotClient.origin_position.compare(real_position):
self.feedStatus = FeedStatus.FNone
self.is_reverse = False
log.log_message(logging.INFO, Constant.str_feed_reverse)
self.log_signal.emit(logging.INFO, Constant.str_feed_reverse)
if self.feedStatus == FeedStatus.FNone or self.pause:
return
elif self.feedStatus == FeedStatus.FCheck:
log.log_message(logging.INFO, Constant.str_feed_check)
self.log_signal.emit(logging.INFO, Constant.str_feed_check)
# 1, 检查是否是三列
# 2, 检查是否有人
# if self.safe_check_columns() and self.safe_check_person():
@ -295,10 +296,10 @@ class Feeding(QObject):
self.next_position(self.is_reverse)
elif self.feedStatus == FeedStatus.FStart:
log.log_message(logging.INFO, Constant.str_feed_start)
self.log_signal.emit(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 引起异常
log.log_message(logging.ERROR, Constant.str_feed_start_error)
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
@ -314,7 +315,7 @@ class Feeding(QObject):
elif self.feedStatus == FeedStatus.FMid:
log.log_message(logging.INFO, Constant.str_feed_mid)
self.log_signal.emit(logging.INFO, Constant.str_feed_mid)
feed_pos = self.get_current_position(self.is_reverse)
if feed_pos.get_position().compare(real_position):
self.next_position(self.is_reverse)
@ -322,9 +323,9 @@ class Feeding(QObject):
elif self.feedStatus == FeedStatus.FPhoto:
log.log_message(logging.INFO, Constant.str_feed_photo)
self.log_signal.emit(logging.INFO, Constant.str_feed_photo)
if self.feedConfig.num == 0:
log.log_message(logging.INFO, Constant.str_feed_finish)
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
@ -341,7 +342,7 @@ class Feeding(QObject):
if Constant.Debug:
if self.feedConfig.num == 990 and self.onekey:
self.feedConfig.num = 0
log.log_message(logging.ERROR, Constant.str_feed_finish)
self.log_signal.emit(logging.ERROR, Constant.str_feed_finish)
return
self.detect.detect_status = DetectStatus.DNone
self.feedConfig.feedLine.set_take_position(real_position)
@ -349,7 +350,7 @@ class Feeding(QObject):
return
if self.detect.detect_position != None:
log.log_message(logging.INFO, Constant.str_feed_takePhoto_success)
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_success)
self.feedConfig.feedLine.set_take_position(self.detect.detect_position)
self.update_detect_image.emit(self.detect.detection_image)
self.next_position()
@ -357,9 +358,9 @@ class Feeding(QObject):
else:
if self.onekey:
self.feedConfig.num = 0
log.log_message(logging.ERROR, Constant.str_feed_finish)
self.log_signal.emit(logging.ERROR, Constant.str_feed_finish)
else:
log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
self.take_no_photo_sigal.emit()
return
detect_pos_list = []
@ -369,7 +370,7 @@ class Feeding(QObject):
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)
self.log_signal.emit(logging.INFO, Constant.str_feed_photo_confirm)
# TODO 返回初始状态
for pos in self.feedConfig.photo_locs:
@ -381,7 +382,7 @@ class Feeding(QObject):
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)
self.log_signal.emit(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,
@ -394,9 +395,9 @@ class Feeding(QObject):
# 黄老师给我的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)
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_success)
else:
log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail + real_position.to_string())
self.log_signal.emit(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))
@ -407,30 +408,30 @@ class Feeding(QObject):
# 拍照位置从五个变为三个
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)
self.log_signal.emit(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)
self.log_signal.emit(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)
self.log_signal.emit(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.log_signal.emit(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.log_signal.emit(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)
self.log_signal.emit(logging.INFO, Constant.str_feed_takePhoto_move)
elif self.feedStatus == FeedStatus.FTake:
log.log_message(logging.INFO, Constant.str_feed_take)
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):
@ -439,7 +440,7 @@ class Feeding(QObject):
# self.sendIOControl(self.robotClient.con_ios[1], 1)
# self.sendIOControl(self.robotClient.con_ios[2], 1)
log.log_message(logging.INFO, "到达抓料点位")
self.log_signal.emit(logging.INFO, "到达抓料点位")
if self.catch.catch_status == CatchStatus.CNone :
self.catch.catch_status = CatchStatus.CTake
return
@ -449,11 +450,11 @@ class Feeding(QObject):
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.log_signal.emit(logging.INFO, Constant.str_feed_take_success)
self.next_position()
else:
log.log_message(logging.ERROR, Constant.str_feed_takePhoto_fail)
self.log_signal.emit(logging.ERROR, Constant.str_feed_takePhoto_fail)
# self.take_no_photo = True
# 继续获取图像
@ -464,18 +465,18 @@ class Feeding(QObject):
elif self.feedStatus == FeedStatus.FBroken1:
log.log_message(logging.INFO, Constant.str_feed_broken)
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
if self.get_current_position().get_position().compare(real_position):
self.next_position()
elif self.feedStatus == FeedStatus.FBroken2:
log.log_message(logging.INFO, Constant.str_feed_broken)
self.log_signal.emit(logging.INFO, Constant.str_feed_broken)
if self.get_current_position().get_position().compare(real_position):
self.next_position()
elif self.feedStatus == FeedStatus.FShake:
log.log_message(logging.INFO, Constant.str_feed_shake)
self.log_signal.emit(logging.INFO, Constant.str_feed_shake)
if self.get_current_position().get_position().compare(real_position,is_action=True):
# TODO 震动方案
@ -491,7 +492,7 @@ class Feeding(QObject):
self.next_position()
elif self.feedStatus == FeedStatus.FDropBag:
log.log_message(logging.INFO, Constant.str_feed_drop)
self.log_signal.emit(logging.INFO, Constant.str_feed_drop)
if self.get_current_position().get_position().compare(real_position,is_action=True):
# self.sendIOControl(self.robotClient.con_ios[0], 0)
@ -511,7 +512,7 @@ class Feeding(QObject):
if self.detect.detect_status == DetectStatus.DNone:
self.detect.detect_status = DetectStatus.DDetect
self.feedConfig.num = self.feedConfig.num - 1
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
self.log_signal.emit(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
self.next_position()
@ -541,7 +542,7 @@ class Feeding(QObject):
break
if self.pos_index == -1:
log.log_message(logging.ERROR, Constant.str_feed_return_original_position_fail)
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:
@ -612,7 +613,7 @@ class Feeding(QObject):
break
if pos_index == -1:
log.log_message(logging.ERROR, Constant.str_feed_return_original_position_fail)
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:
@ -667,7 +668,7 @@ class Feeding(QObject):
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}')
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):
@ -703,7 +704,7 @@ class Feeding(QObject):
try:
log.log_message(logging.INFO, log_str)
self.log_signal.emit(logging.INFO, log_str)
except:
return
@ -731,7 +732,7 @@ class Feeding(QObject):
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
log.log_message(logging.ERROR, Constant.str_feed_angle_error)
self.log_signal.emit(logging.ERROR, Constant.str_feed_angle_error)
else:
self.sendTargPosition(real_position=start_pos.get_position(),speed=self.robotClient.debug_speed,move_type=MoveType.AXIS)
else: