135 lines
5.5 KiB
Python
135 lines
5.5 KiB
Python
from Model.Position import Real_Position, Detection_Position
|
|
from enum import Enum
|
|
from COM.COM_Robot import RobotClient
|
|
from Model.RobotModel import CMDInstructRequest
|
|
from Vision.camera_coordinate_dete import Detection
|
|
|
|
|
|
class FeedStatus(Enum):
|
|
FNone = 0
|
|
FStart = 1
|
|
FSafeP = 2
|
|
FPhoto = 3
|
|
FTake = 4
|
|
FSafeF = 5
|
|
FFeedP = 6
|
|
FFinished = 7
|
|
|
|
class FeedLine:
|
|
def __init__(self,safe_position:Real_Position,photo_position:Real_Position,feed_position:Real_Position):
|
|
self.safe_position = safe_position
|
|
self.photo_position = photo_position
|
|
self.feed_position = feed_position
|
|
self.take_position = None
|
|
|
|
|
|
class FeedingConfig:
|
|
def __init__(self, num:int, feedLine:FeedLine):
|
|
self.num = num
|
|
self.feedLine = feedLine
|
|
|
|
|
|
def get_line_info(self):
|
|
pass
|
|
|
|
|
|
class Feeding():
|
|
def __init__(self,robotClient:RobotClient,detection: Detection):
|
|
self.feedConfig = None
|
|
self.feedStatus = FeedStatus.FNone
|
|
self.robotClient = robotClient
|
|
pass
|
|
|
|
def run(self):
|
|
# 获取事件坐标
|
|
real_position = Real_Position()
|
|
# real_position.init_position(self.robotClient.status_model.world_n[0],
|
|
# self.robotClient.status_model.world_n[1],
|
|
# self.robotClient.status_model.world_n[2],
|
|
# self.robotClient.status_model.world_n[3],
|
|
# self.robotClient.status_model.world_n[4],
|
|
# self.robotClient.status_model.world_n[5],
|
|
# self.robotClient.status_model.world_n[6]);
|
|
real_position.init_position(0,
|
|
0,
|
|
0,
|
|
0,
|
|
0,
|
|
0);
|
|
|
|
|
|
if self.feedStatus==FeedStatus.FNone:
|
|
pass
|
|
elif self.feedStatus==FeedStatus.FStart:
|
|
if self.feedConfig.num != 0:
|
|
self.feedStatus = FeedStatus.FSafeP
|
|
|
|
self.sendTargPosition(self.feedConfig.safe_position)
|
|
#print(request_command)
|
|
|
|
pass
|
|
elif self.feedStatus==FeedStatus.FSafeP:
|
|
|
|
if self.feedConfig.safe_position.compare(real_position):
|
|
self.feedStatus = FeedStatus.FPhoto
|
|
self.sendTargPosition(self.feedConfig.photo_position)
|
|
|
|
elif self.feedStatus == FeedStatus.FPhoto:
|
|
if self.feedConfig.photo_position.compare(real_position):
|
|
_, _, xyz, uvw = self.detection.get_position() #检测结果
|
|
if xyz!=None:
|
|
self.feedStatus = FeedStatus.FTake
|
|
#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,
|
|
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)
|
|
self.feedConfig.take_position = Real_Position().init_position(*target_position,*noraml_base)
|
|
else:
|
|
print("打印日志，保存失败图像")
|
|
pass # 发送拍照获取坐标 并 开始移动
|
|
|
|
elif self.feedStatus == FeedStatus.FTake:
|
|
if self.feedConfig.take_position != None and self.feedConfig.take_position.compare(real_position):
|
|
self.feedStatus = FeedStatus.FSafeF
|
|
pass #打开吸嘴并返回
|
|
|
|
elif self.feedStatus == FeedStatus.FSafeF:
|
|
if self.feedConfig.safe_position.compare(real_position):
|
|
self.feedStatus = FeedStatus.FFeedP
|
|
self.sendTargPosition(self.feedConfig.feed_position)
|
|
pass #吸嘴开始
|
|
|
|
|
|
elif self.feedStatus==FeedStatus.FFeedP:
|
|
if self.feedConfig.feed_position.compare(real_position):
|
|
self.feedConfig.num = self.feedConfig.num-1
|
|
if self.feedConfig.num == 0:
|
|
self.feedStatus=FeedStatus.FNone
|
|
else:
|
|
self.feedStatus = FeedStatus.FSafeP
|
|
self.sendTargPosition(self.feedConfig.safe_position)
|
|
pass
|
|
|
|
def sendTargPosition(self,real_position):
|
|
from Model.RobotModel import Instruction
|
|
position_instruction = Instruction()
|
|
position_instruction.m0 = self.feedConfig.safe_position.X
|
|
position_instruction.m1 = self.feedConfig.safe_position.Y
|
|
position_instruction.m2 = self.feedConfig.safe_position.Z
|
|
position_instruction.m3 = self.feedConfig.safe_position.U
|
|
position_instruction.m4 = self.feedConfig.safe_position.V
|
|
position_instruction.m5 = self.feedConfig.safe_position.W
|
|
instruction_command = CMDInstructRequest()
|
|
instruction_command.instructions.append(vars(position_instruction))
|
|
request_command = vars(instruction_command)
|
|
self.robotClient.add_sendQuene(request_command)
|
|
pass
|
|
|