diff --git a/COM/COM_TCP.py b/COM/COM_TCP.py index c0e3b30..361e1b0 100644 --- a/COM/COM_TCP.py +++ b/COM/COM_TCP.py @@ -9,6 +9,7 @@ class TCPClient: self.IPAddress = ip self.port = port self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.client_socket.settimeout(5) def CreatConnect(self): diff --git a/CU/Command.py b/CU/Command.py index 0c732db..1a9ea95 100644 --- a/CU/Command.py +++ b/CU/Command.py @@ -10,3 +10,7 @@ class Command: self.status = Status.Prepareing pass +class FeedCommand(Command): + def __init__(self, feedingConfig): + super().__init__() + self.feed_config = feedingConfig \ No newline at end of file diff --git a/CU/Feeding.py b/CU/Feeding.py index 2330b3b..7d2f6c7 100644 --- a/CU/Feeding.py +++ b/CU/Feeding.py @@ -1,11 +1,22 @@ from Model.Position import Real_Position +from enum import Enum + +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: @@ -19,5 +30,78 @@ class FeedingConfig: class Feeding(): - def __init__(self,feedingConfig): + def __init__(self,robotClient:RobotClient): + 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]); + + if self.feedStatus==FeedStatus.FNone: + + pass + elif self.feedStatus==FeedStatus.FStart: + if self.feedConfig.num != 0: + self.feedStatus = FeedStatus.FSafeP + + 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 + sendTargPosition(self.feedConfig.photo_position) + + elif self.feedStatus == FeedStatus.FPhoto: + if self.feedConfig.photo_position.compare(real_position): + self.feedStatus = FeedStatus.FTake + 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 + 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 + sendTargPosition(self.feedConfig.safe_position) + pass + + def sendTargPosition(self,real_position): + 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 + diff --git a/Expection.py b/Expection.py index 8b81e10..efdbc5e 100644 --- a/Expection.py +++ b/Expection.py @@ -7,9 +7,6 @@ class Error_Code(Enum): class VisionError_Code(Enum): CAMERA_SUCCESS = 200 -class TraceError_Code(Enum): - Trace_SUCCESS = 300 - diff --git a/Model/Position.py b/Model/Position.py index 7336cba..41a379e 100644 --- a/Model/Position.py +++ b/Model/Position.py @@ -1,9 +1,5 @@ - -""" -摄像头识别位置和角度 -""" -class Detection_Position: +class Position: def __init__(self): self.X = 0.0 self.Y = 0.0 @@ -11,6 +7,23 @@ class Detection_Position: self.U = 0.0 self.V = 0.0 self.W = 0.0 + def compare(self,position): + if self.X-position.X\n" + self.textEdit_log.setHtml(QCoreApplication.translate("MainWindow", u"\n" "