From 7a59ee56932206dc4a60a35aa1a63cfec9f35b5c Mon Sep 17 00:00:00 2001
From: "Ziwei.He" <hellohezw@gmail.com>
Date: Thu, 29 May 2025 22:45:43 +0800
Subject: [PATCH] stash

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

diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index cec089b..e15e343 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -89,6 +89,7 @@ class dobot_nova5:
         self.feedback_thread.daemon = True
         self.feedback_thread.start()
         time.sleep(1)
+        self.dashboard.RequestControl()
         max_retry_num = 5  #尝试连接次数
         cnt_num = 0
         while cnt_num < max_retry_num:
@@ -97,11 +98,12 @@ class dobot_nova5:
                 self.clearError() # 清除报警
                 # stopId = self.dashboard.Stop()
                 time.sleep(0.5)
+                print("self.self.feedbackData.robotMode:",self.feedbackData.robotMode)
                 if (self.feedbackData.robotMode == 3 or self.feedbackData.robotMode == 4): #and self.parseResultId(stopId)[0] == 0:
-                    self.dashboard.RequestControl()
+                    # self.dashboard.RequestControl()
                     if self.feedbackData.robotMode == 3:
                         self.dashboard.PowerOn() # 复位
-                    self.dashboard.Stop() # 停止
+                    # self.dashboard.Stop() # 停止
                     if self.start_up(): # 启动
                         self.logger.log_info(f"连接机械臂成功")
                         break
@@ -187,19 +189,19 @@ class dobot_nova5:
         while cnt < max_try:
             cnt += 1
             # 关闭碰撞检测
-            if self.parseResultId(self.dashboard.SetCollisionLevel(level=0))[0] == 0:
-                time.sleep(0.1)
-                self.logger.log_info("关闭碰撞检测成功")
-                code = self.parseResultId(self.dashboard.EnableRobot())[0]
-                if code == 0:
-                    while 1:
-                        if self.feedbackData.robotMode in [3, 6, 7, 8, 9, 10, 11]:
-                            self.logger.log_error(f"使能失败，机械臂异常:{self.feedbackData.robotMode}")
-                            return False
-                        elif self.feedbackData.robotMode == 5:
-                            self.logger.log_info("机械臂使能成功")
-                            return True
-                        time.sleep(0.1)
+            # if self.parseResultId(self.dashboard.SetCollisionLevel(level=0))[0] == 0:
+            time.sleep(0.1)
+            self.logger.log_info("关闭碰撞检测成功")
+            code = self.parseResultId(self.dashboard.EnableRobot())[0]
+            if code == 0:
+                while 1:
+                    if self.feedbackData.robotMode in [3, 6, 7, 8, 9, 10, 11]:
+                        self.logger.log_error(f"使能失败，机械臂异常:{self.feedbackData.robotMode}")
+                        return False
+                    elif self.feedbackData.robotMode == 5:
+                        self.logger.log_info("机械臂使能成功")
+                        return True
+                    time.sleep(0.1)
     
     # def disable_servo(self):
     #     pass