diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index d3d2b84..6021271 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -78,102 +78,78 @@ class dobot_nova5:
self.feedbackData = feedbackItem() # 自定义反馈数据结构体
self.logger = CustomLogger('Dobot_nova5')
- # try:
- # self.dashboard = DobotApiDashboard(self.ip, self.dashboardPort)
- # if not self.dashboard.connect():
- # print(f"[ERROR] 无法连接 Dashboard {self.ip}:{self.dashboardPort}")
- # self.dashboard = None
- # else:
- # print("[INFO] Dashboard 连接成功")
-
- # except Exception as e:
- # print(f"[ERROR] Dashboard 初始化失败: {e}")
- # self.dashboard = None
-
- # 尝试连接 Dashboard 控制接口（默认 29999 端口）
- try:
- self.dashboard = DobotApiDashboard(self.ip, self.dashboardPort)
- if self.dashboard.is_connected():
- print("[INFO] Dashboard 连接成功")
- else:
- print("[ERROR] Dashboard 未连接成功")
- self.dashboard = None
- except Exception as e:
- print(f"[ERROR] Dashboard 连接失败: {e}")
- self.dashboard = None
-
- # 尝试连接 Feedback 接口（默认 30003 端口）
- try:
- self.feedFour = DobotApiFeedBack(self.ip, self.feedFourPort)
- if self.feedFour.is_connected():
- print("[INFO] Feedback 连接成功")
- else:
- print("[ERROR] Feedback 未连接成功")
- self.feedFour = None
- except Exception as e:
- print(f"[ERROR] Feedback 连接失败: {e}")
- self.feedFour = None
-
-
-
- # self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort)
- # self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort)
-
self.stop_feedback = threading.Event()
self.stop_feedback.clear()
# 状态反馈线程
self.feedback_thread = threading.Thread(
target=self.GetFeedback # 机器状态反馈线程
)
- 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:
- if self.feedbackData.robotMode != -1:
- self.dashboard.EmergencyStop(mode=0) # 松开急停
- 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()
- if self.feedbackData.robotMode == 3:
- self.dashboard.PowerOn() # 复位
- # self.dashboard.Stop() # 停止
- if self.start_up(): # 启动
- self.logger.log_info(f"连接机械臂成功")
- break
+
+ try:
+ self.dashboard = DobotApiDashboard(self.ip, self.dashboardPort)
+ self.feedFour = DobotApiFeedBack(self.ip, self.feedFourPort)
+ if self.dashboard.is_connected() and self.feedFour.is_connected():
+ print("连接成功")
+ 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:
+ if self.feedbackData.robotMode != -1:
+ self.dashboard.EmergencyStop(mode=0) # 松开急停
+ 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()
+ if self.feedbackData.robotMode == 3:
+ self.dashboard.PowerOn() # 复位
+ # self.dashboard.Stop() # 停止
+ if self.start_up(): # 启动
+ self.logger.log_info(f"连接机械臂成功")
+ break
+ else:
+ self.logger.log_error(f"启动失败，尝试第{cnt_num+1}次连接")
+ cnt_num += 1
+ time.sleep(1)
+ else:
+ if self.feedbackData.robotMode == 9:# and self.parseResultId(stopId)[0] == -3:
+ print(robot_mode_types.get(self.feedbackData.robotMode, "未知模式"))
+
+ requests.post(
+ "http://127.0.0.1:5000/on_message", data={"message": "机械臂处于锁定状态
请解除锁定后再使用"}
+ )
+ instruction = {
+ "is_massaging": False,
+ "massage_service_started": False
+ }
+ requests.post(
+ "http://127.0.0.1:5000/update_massage_status", data=instruction
+ )
+ sys.exit(0)
+ self.disableRobot()
+ self.clearError() # 清除报警
+ cnt_num += 1
+ time.sleep(1)
else:
- self.logger.log_error(f"启动失败，尝试第{cnt_num+1}次连接")
+ self.logger.log_error(f"连接机械臂失败，尝试第{cnt_num+1}次连接")
cnt_num += 1
time.sleep(1)
- else:
- if self.feedbackData.robotMode == 9:# and self.parseResultId(stopId)[0] == -3:
- print(robot_mode_types.get(self.feedbackData.robotMode, "未知模式"))
-
- requests.post(
- "http://127.0.0.1:5000/on_message", data={"message": "机械臂处于锁定状态
请解除锁定后再使用"}
- )
- instruction = {
- "is_massaging": False,
- "massage_service_started": False
- }
- requests.post(
- "http://127.0.0.1:5000/update_massage_status", data=instruction
- )
- sys.exit(0)
- self.disableRobot()
- self.clearError() # 清除报警
- cnt_num += 1
- time.sleep(1)
-
else:
- self.logger.log_error(f"连接机械臂失败，尝试第{cnt_num+1}次连接")
- cnt_num += 1
- time.sleep(1)
+ print("[ERROR]未连接成功")
+ self.dashboard = None
+ self.feedFour = None
+ except Exception as e:
+ print(f"[ERROR] Dashboard 连接失败: {e}")
+ self.dashboard = None
+ self.feedFour = None
+
+
+
+
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]