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

添加中转点和破袋点

Browse Source
This commit is contained in:
hjw
2024-09-12 15:41:42 +08:00
parent 90f7ff4625
commit 3452cc0b54
5 changed files with 279 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
CU/Feeding.py
View File

@ -1,5 +1,6 @@
import Constant
import time
import Constant
from Model.Position import Real_Position, Detection_Position
from enum import Enum
from COM.COM_Robot import RobotClient
@ -15,13 +16,16 @@ class FeedStatus(Enum):
FTake = 4
FSafeF = 5
FFeedP = 6
FFinished = 7
FBroken = 7
FFinished = 8
class FeedLine:
def __init__(self,name,safe_position:Real_Position,photo_position:Real_Position,feed_position:Real_Position):
def __init__(self,name,safe_position:Real_Position,photo_position:Real_Position,mid_position:Real_Position,broken_position:Real_Position,feed_position:Real_Position):
self.safe_position = safe_position
self.photo_position = photo_position
self.feed_position = feed_position
self.mid_position = mid_position
self.broken_position = broken_position
self.take_position = None
self.name = name
@ -87,7 +91,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-5,
self.robotClient.status_model.world_2-10,
self.robotClient.status_model.world_3,
self.robotClient.status_model.world_4,
self.robotClient.status_model.world_5)
@ -103,22 +107,28 @@ class Feeding():
elif self.feedStatus == FeedStatus.FTake:
if self.feedConfig.feedLine.take_position != None and self.feedConfig.feedLine.take_position.compare(real_position):
self.feedStatus = FeedStatus.FSafeF
time.sleep(2)
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
time.sleep(1)
self.sendTargPosition(self.feedConfig.feedLine.mid_position)
pass #打开吸嘴并返回
elif self.feedStatus == FeedStatus.FSafeF:
if self.feedConfig.feedLine.safe_position.compare(real_position):
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:
if self.feedConfig.feed_position.compare(real_position):
if self.feedConfig.feedLine.feed_position.compare(real_position):
self.feedStatus = FeedStatus.FBroken
self.sendTargPosition(self.feedConfig.feedLine.broken_position)
pass #破袋
elif self.feedStatus==FeedStatus.FBroken:
if self.feedConfig.feedLine.broken_position.compare(real_position):
self.feedConfig.num = self.feedConfig.num-1
if self.feedConfig.num == 0:
self.feedStatus=FeedStatus.FNone
self.sendTargPosition(self.feedConfig.feedLine.safe_position)
else:
self.feedStatus = FeedStatus.FSafeP
self.sendTargPosition(self.feedConfig.feedLine.safe_position)