From 15eed49134325597c8360c9a27fc02b744311882 Mon Sep 17 00:00:00 2001
From: swayneleong <15723182159@163.com>
Date: Thu, 29 May 2025 22:50:13 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9init?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../MassageControl/hardware/dobot_nova5.py    | 82 +++++++++++++++----
 1 file changed, 65 insertions(+), 17 deletions(-)

diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index e15e343..118c7d9 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -137,6 +137,54 @@ class dobot_nova5:
                 time.sleep(1)
         atexit.register(self.exit_task)
 
+        self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
+
+    # 为状态管理而封装的初始化函数
+    def init(self):
+        self.is_exit = False
+        self.traj_list = None
+        self.last_pose_command = np.zeros(6)
+        self.last_input_command = None
+        self.tcp_offset = None
+
+
+        self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
+        self.filter_matrix = np.zeros((1,6)) # 角度伺服用
+        sleep(1)
+        self.is_standby = False
+        code = self.move_joint(self.init_pos)
+
+        if code == 0:
+            if self.parseResultId(self.dashboard.SetCollisionLevel(level=1))[0] == 0:
+                self.logger.log_info("机械臂初始化")
+            else:
+                time.sleep(0.2)
+                max_retries = 5
+                retry_count = 0
+                while retry_count < max_retries:
+                    if self.parseResultId(self.dashboard.SetCollisionLevel(level=1))[0] == 0:
+                        print("retry_count",retry_count)
+                        break
+                    else:
+                        time.sleep(0.1)
+
+                if retry_count == max_retries:
+                    self.logger.log_error("机械臂初始化失败")
+                    requests.post(
+                        "http://127.0.0.1:5000/on_message", data={"message": "机械臂初始化失败"}
+                    )
+                    self.is_exit = True
+                    sys.exit(0)
+        else:
+            self.logger.log_error("机械臂初始化失败")
+            requests.post(
+                "http://127.0.0.1:5000/on_message", data={"message": "机械臂初始化失败"}
+            )
+            self.is_exit = True
+            sys.exit(0)
+        self.last_print_time=0
+        self.last_reocrd_time =0
+
     def exit_task(self):
         self.logger.log_yellow("退出任务")
         # self.disable()
@@ -587,23 +635,23 @@ class dobot_nova5:
     
     ## 适配中间层的再封装接口
     
-    # 为状态管理而封装的初始化函数
-    def init(self):
-        self.is_exit = False
-        self.traj_list = None
-        self.last_pose_command = np.zeros(6)
-        self.last_input_command = None
-        self.tcp_offset = None
-        self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449]
-        self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
-        self.filter_matrix = np.zeros((1,6)) # 角度伺服用
-        sleep(1)
-        self.is_standby = False
-        code = self.RunPoint_P_inJoint(self.init_J)
-        if code == 0:
-            print("机械臂初始化成功且到达初始位置")
-        else:
-            print("机械臂初始化失败")
+    # # 为状态管理而封装的初始化函数
+    # def init(self):
+    #     self.is_exit = False
+    #     self.traj_list = None
+    #     self.last_pose_command = np.zeros(6)
+    #     self.last_input_command = None
+    #     self.tcp_offset = None
+    #     self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449]
+    #     self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
+    #     self.filter_matrix = np.zeros((1,6)) # 角度伺服用
+    #     sleep(1)
+    #     self.is_standby = False
+    #     code = self.RunPoint_P_inJoint(self.init_J)
+    #     if code == 0:
+    #         print("机械臂初始化成功且到达初始位置")
+    #     else:
+    #         print("机械臂初始化失败")
 
     def get_pose_at_flange(self):
         '''