From 7a262382192fbe5fe0c4e53a34516039dea52cca Mon Sep 17 00:00:00 2001
From: Gogs <gogs@fake.local>
Date: Thu, 12 Dec 2024 10:37:25 +0800
Subject: [PATCH] =?UTF-8?q?update=20=E8=A1=A5=E5=85=85=E9=83=A8=E5=88=86?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 CU/Catch.py | 47 ++++++++++++++++++++++++++++++++++++++++++-----
 main.py     | 12 +++++++-----
 2 files changed, 49 insertions(+), 10 deletions(-)

diff --git a/CU/Catch.py b/CU/Catch.py
index 25016bb..3893074 100644
--- a/CU/Catch.py
+++ b/CU/Catch.py
@@ -1,20 +1,57 @@
 from enum import Enum
 
+from COM.COM_Robot import RobotClient
+from Util.util_time import CClockPulse, CTon
+
 
 class CatchStatus(Enum):
     CNone = 0
     CTake = 1
-    CPress = 2
+    CDrop = 2
     CShake = 3
+    COk = 4
 
 class catch:
-    def __init__(self, x, y, width, height, angle, color):
-        pass
-
+    def __init__(self, robotClient: RobotClient):
+        self.robotClient = robotClient
+        self.catch_status = CatchStatus.CNone
+        self.shake_continue = CTon()
+        self.shakePulse = CClockPulse()
+        self.shake_count = 5
     def run(self):
+        if self.catch_status == CatchStatus.CNone:
+            return
 
-        pass
+        if self.catch_status == CatchStatus.CTake:
+            self.robotClient.sendIOControl(self.robotClient.con_ios[0],1)
+            if self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
+                self.catch_status = CatchStatus.COk
+
+        if self.catch_status == CatchStatus.CDrop:
+            self.robotClient.sendIOControl(self.robotClient.con_ios[0],0)
+            self.robotClient.sendIOControl(self.robotClient.con_ios[1],1)
+            if self.robotClient.check_outputQ(self.robotClient.con_ios[1]) and not self.robotClient.check_outputQ(self.robotClient.con_ios[0]):
+                self.catch_status = CatchStatus.COk
+        if self.catch_status == CatchStatus.CShake:
+            if not self.shake_continue.Q(True, 3000):
+                shakeQ = self.shakePulse.Q(True, 1000, 1000)
+                self.robotClient.sendIOControl(self.robotClient.con_ios[2], shakeQ)
+            else:
+                self.shake_continue.SetReset()
+                self.catch_status = CatchStatus.COk
+                if self.robotClient.check_outputQ(self.robotClient.con_ios[2]):
+                    self.robotClient.sendIOControl(self.robotClient.con_ios[2], False)
+
+        if self.catch_status == CatchStatus.COk:
+
+            pass
 
 
     def take_bag(self):
+        return True
+
+    def press_bag(self):
+        return True
+
+    def shake_bag(self):
         return True
\ No newline at end of file
diff --git a/main.py b/main.py
index e3bd17a..96e6291 100644
--- a/main.py
+++ b/main.py
@@ -192,6 +192,8 @@ class MainWindow(QMainWindow, Ui_MainWindow):
 
         self.pushButton_savePosition.clicked.connect(self.send_save_lines_data)
 
+    def send_table_line_position_cell_changed(self):
+        self.table_position_changed = True
     def send_table_lines_cell_changed(self):
         self.table_line_changed = True
     def send_save_lines_data(self):
@@ -335,7 +337,7 @@ class MainWindow(QMainWindow, Ui_MainWindow):
             header.setSectionResizeMode(i, PySide6.QtWidgets.QHeaderView.ResizeMode.Interactive)
         self.tableWidget_line_positions.setContextMenuPolicy(PySide6.QtCore.Qt.ContextMenuPolicy.CustomContextMenu)
         self.tableWidget_line_positions.customContextMenuRequested.connect(self.show_positions_context_menu)
-
+        self.tableWidget_line_positions.cellChanged.connect(self.send_table_line_position_cell_changed)
     def init_table_positions_data(self,position_list):
         self.tableWidget_line_positions.setRowCount(0)
         for index,position_model in enumerate(position_list):
@@ -602,15 +604,15 @@ class MainWindow(QMainWindow, Ui_MainWindow):
         debug_speed = int(self.configReader.get('Speed', 'debug_speed'))
         feed_speed = int(self.configReader.get('Speed', 'feed_speed'))
         reset_speed = int(self.configReader.get('Speed', 'reset_speed'))
-        solenoid_valve1_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve1_addr'))
-        solenoid_valve2_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve2_addr'))
-        solenoid_valve3_addr = int(self.configReader.get('Robot_Feed', 'solenoid_valve3_addr'))
+        io_take_addr = int(self.configReader.get('Robot_Feed', 'io_take_addr'))
+        io_zip_addr = int(self.configReader.get('Robot_Feed', 'io_zip_addr'))
+        io_shake_addr = int(self.configReader.get('Robot_Feed', 'io_shake_addr'))
         time_delay_take = float(self.configReader.get('Robot_Feed', 'takeDelay'))
         time_delay_put = float(self.configReader.get('Robot_Feed', 'putDelay'))
         time_delay_shake = float(self.configReader.get('Robot_Feed', 'shakeDelay'))
         #TODO
         #dropDelay_time = int(self.configReader.get('Robot_Feed', 'dropDelay_time'))
-        self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address,[solenoid_valve1_addr, solenoid_valve2_addr, solenoid_valve3_addr],time_delay_take,time_delay_put,time_delay_shake,origin_position)
+        self.robotClient = RobotClient(ip, port, photo_locs, self.command_position_quene, self.status_address,[io_take_addr, io_zip_addr, io_shake_addr],time_delay_take,time_delay_put,time_delay_shake,origin_position)
         self.robotClient.debug_speed = debug_speed
         self.robotClient.feed_speed = feed_speed
         self.robotClient.reset_speed = reset_speed