add emergency

Massage/MassageControl/MassageRobot_nova5.py

@@ -456,6 +456,7 @@ class MassageRobot:
                         self.logger.log_error(f"传感器线程数据读取失败-{self.sensor_fail_count}")
                         if self.sensor_fail_count > 10:
                             self.logger.log_error("传感器线程数据读取失败超过10次,程序终止")
+                            self.stop_thread()
                             self.stop()
                             break
                     else:
@@ -612,10 +613,11 @@ class MassageRobot:
                     # # 自定义添加所需反馈数据
 
                     # print("self111222:",self.arm.feedbackData.robotMode,tcp_command)
-                    if self.arm.feedbackData.robotMode in [3, 4, 6, 8, 9, 11] or not self.arm.feedbackData.EnableState: 
-                        self.logger.log_error(f"机械臂异常:{self.arm.feedbackData.robotMode}")
+                    if self.arm.feedbackData.robotMode in [3, 4, 6, 8, 9, 11] or not self.arm.feedbackData.EnableState or self.arm.feedbackData.ErrorStatus == True: 
+                        self.logger.log_error(f"机械臂异常:{self.arm.feedbackData.robotMode},{self.arm.feedbackData.EnableState},{self.arm.feedbackData.ErrorStatus}")
                         self.arm.dashboard.Stop()
                         self.arm.dashboard.EmergencyStop(mode=1)
+                        self.stop_thread()
                         self.stop()
                         return -1 
                     if self.arm.feedbackData.EnableState and self.arm.feedbackData.robotMode == 10:

Massage/MassageControl/hardware/dobot_nova5.py

@@ -72,6 +72,7 @@ class dobot_nova5:
                 self.QActual = -1
                 self.ActualQuaternion = -1
                 self.ToolVectorActual = -1
+                self.ErrorStatus = False
                 # ... 自定义反馈数据
                 '''
                 self.UserValue = -1
@@ -118,7 +119,7 @@ class dobot_nova5:
                                 cnt_num += 1
                                 time.sleep(1)
                         else:
-                            if self.feedbackData.robotMode == 9:# and self.parseResultprocess(stopId)[0] == -3:
+                            if self.feedbackData.robotMode == 9 or self.feedbackData.ErrorStatus == True:
                                 print(robot_mode_types.get(self.feedbackData.robotMode, "未知模式"))
 
                                 requests.post(
@@ -263,6 +264,7 @@ class dobot_nova5:
                         self.feedbackData.QActual = feedInfo['QActual'][0]
                         self.feedbackData.ActualQuaternion = feedInfo['ActualQuaternion'][0]
                         self.feedbackData.ToolVectorActual = feedInfo['ToolVectorActual'][0]
+                        self.feedbackData.ErrorState = feedInfo['ErrorStatus'][0]
                         # ⬇️ 将 ToolVectorActual 写入 txt 文件
                         # try:
                         #     with open("tool_vector_log.txt", "a") as f:
@@ -290,7 +292,7 @@ class dobot_nova5:
             code = self.parseResultprocess(self.dashboard.EnableRobot())[0]
             if code == 0:
                 while 1:
-                    if self.feedbackData.robotMode in [3, 6, 7, 8, 9, 10, 11]:
+                    if self.feedbackData.robotMode in [3, 6, 7, 8, 9, 10, 11] or self.feedbackData.ErrorStatus == True:
                         self.logger.log_error(f"使能失败，机械臂异常:{self.feedbackData.robotMode}")
                         return False
                     elif self.feedbackData.robotMode == 5:
@@ -312,7 +314,7 @@ class dobot_nova5:
         # print("self.feedbackData.robotMode6:",self.feedbackData.robotMode)
         # print("self.parseResultprocess(command)",self.parseResultprocess(command))
         while True:
-            if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11]: 
+            if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11] or not self.feedbackData.EnableState or self.feedbackData.ErrorStatus == True: 
                 self.logger.log_error(f"机械臂异常:{self.feedbackData.robotMode}")
                 print(robot_mode_types.get(self.feedbackData.robotMode, "未知模式"))
                 self.dashboard.Stop()