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-10-28 22:45:01 +08:00
parent 5d6588d1c2
commit 5500ac7f71
5 changed files with 277 additions and 290 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
CU/Feeding.py
View File

@ -27,30 +27,29 @@ class FeedStatus(IntEnum):
FFinished = 12
class FeedLine:
def __init__(self,id,name,safe_position:Real_Position,photo_position:Real_Position,mid_position:Real_Position,broken1_position:Real_Position,broken2_position:Real_Position,drop_bag_position:Real_Position,zip_bag_position:Real_Position,feed_position:Real_Position):
def __init__(self,id,name,safe_position:Real_Position,broken1_position:Real_Position,broken2_position:Real_Position,shake_position:Real_Position,drop_bag_position:Real_Position):
self.safe_position = safe_position
self.photo_position = photo_position
self.feed_position = feed_position
self.mid_position = mid_position
self.broken1_position = broken1_position
self.broken2_position = broken2_position
self.shake_position = shake_position
self.drop_bag_position = drop_bag_position
self.zip_bag_position = zip_bag_position
self.take_position = None
self.name = name
self.id=id
class FeedingConfig:
def __init__(self, num:int, feedLine:FeedLine):
def __init__(self, num:int, feedLine:FeedLine,photo_locs):
self.num = num
self.feedLine = feedLine
self.photo_locs = photo_locs
def get_line_info(self):
pass
class Feeding():
class Feeding:
def __init__(self,robotClient:RobotClient,detection:Detection):
self.feedConfig = None
self.feedStatus = FeedStatus.FNone
@ -107,6 +106,7 @@ class Feeding():
if self.feedConfig.feedLine.safe_position.compare(real_position):
self.feedStatus = FeedStatus.FPhoto
self.sendTargPosition(self.feedConfig.feedLine.photo_position)
#判断是哪一个问题
elif self.feedStatus == FeedStatus.FPhoto:
log.log_message(logging.INFO, Constant.str_feed_photo)
@ -139,75 +139,57 @@ class Feeding():
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.take_position.compare(real_position):
self.feedStatus = FeedStatus.FSafeF
# 打开吸嘴并返回 #TODO
time.sleep(2)
self.feedStatus = FeedStatus.FSafeF
log.log_message(logging.INFO, Constant.str_feed_take_success)
self.sendTargPosition(self.feedConfig.feedLine.mid_position)
pass #打开吸嘴并返回
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
pass
elif self.feedStatus == FeedStatus.FSafeF:
log.log_message(logging.INFO, Constant.str_feed_mid)
if self.feedConfig.feedLine.mid_position.compare(real_position):
self.feedStatus = FeedStatus.FFeedP
self.sendTargPosition(self.feedConfig.feedLine.feed_position)
pass #吸嘴开始
elif self.feedStatus==FeedStatus.FFeedP:
log.log_message(logging.INFO, Constant.str_feed_feed)
if self.feedConfig.feedLine.feed_position.compare(real_position):
if self.feedConfig.feedLine.safe_position.compare(real_position):
self.feedStatus = FeedStatus.FBroken
self.sendTargPosition(self.feedConfig.feedLine.broken1_position)
pass #破袋
pass #吸嘴开始
# elif self.feedStatus==FeedStatus.FFeedP:
# log.log_message(logging.INFO, Constant.str_feed_feed)
# if self.feedConfig.feedLine.feed_position.compare(real_position):
# self.feedStatus = FeedStatus.FBroken
# self.sendTargPosition(self.feedConfig.feedLine.broken1_position)
# pass #破袋
elif self.feedStatus==FeedStatus.FBroken:
log.log_message(logging.INFO, Constant.str_feed_broken)
if self.feedConfig.feedLine.broken1_position.compare(real_position):
self.sendTargPosition(self.feedConfig.feedLine.broken2_position)
if self.feedConfig.feedLine.broken2_position.compare(real_position):
current_position = Real_Position().init_position(0,0,0,0,0,0)
current_position.X = self.robotClient.status_model.axis_0
current_position.Y = self.robotClient.status_model.axis_1
current_position.Z = self.robotClient.status_model.axis_2
current_position.U = self.robotClient.status_model.axis_3
current_position.V = self.robotClient.status_model.axis_4
current_position.W = self.robotClient.status_model.axis_5
for i in range(8):
if (i % 2 == 0) ^ (i >= 3): ## 过一半取消
c1 = current_position.U - 5
self.sendTargPosition(c1, MoveType.AXIS, speed=Constant.shake_speed)
else:
c1 = current_position.U + 5
self.sendTargPosition(c1, MoveType.AXIS, speed=Constant.shake_speed)
if True: # 延迟判断，如果最后点位延迟1s，则认为阶段完成
self.sendTargPosition(self.feedConfig.feedLine.shake_position)
if self.feedConfig.feedLine.shake_position.compare(real_position): # 延迟判断，如果最后点位延迟1s，则认为阶段完成
# TODO 震动方案
time.sleep(2)
self.feedStatus = FeedStatus.FDropBag
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
elif self.feedStatus == FeedStatus.FDropBag:
log.log_message(logging.INFO, Constant.str_feed_drop)
if self.feedConfig.feedLine.safe_position.compare(real_position):
if self.feedConfig.num - 1 != 0:
self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position)
else:
self.sendTargPosition(self.feedConfig.feedLine.zip_bag_position)
self.sendTargPosition(self.feedConfig.feedLine.drop_bag_position)
pass
if self.feedConfig.feedLine.drop_bag_position.compare(real_position):
# TODO 松开吸嘴
time.sleep(2)
self.feedConfig.num = self.feedConfig.num - 1
log.log_message(logging.INFO, f'{Constant.str_feed_feed_num}{self.feedConfig.num}')
self.feedStatus = FeedStatus.FSafeP
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
if self.feedConfig.feedLine.zip_bag_position.compare(real_position):
log.log_message(logging.INFO, Constant.str_feed_zip_bag)
# TODO 松开吸嘴 是否需要判断放没放开不
self.feedStatus = FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.init_detection_image()
log.log_message(logging.INFO, Constant.str_feed_finish)
pass
if self.feedConfig.num == 0: # 投料次数为0，直接返回
log.log_message(logging.INFO, Constant.str_feed_finish)
self.feedStatus = FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
self.init_detection_image()
else:
self.feedStatus = FeedStatus.FSafeP
self.sendTargPosition(self.feedConfig.feedLine.safe_position)