From 54a673386134bdb4e4dabcde0ce30fa4ef79b071 Mon Sep 17 00:00:00 2001
From: "Ziwei.He" <hellohezw@gmail.com>
Date: Mon, 12 May 2025 09:49:26 +0800
Subject: [PATCH] =?UTF-8?q?ServoJ=E4=BC=BA=E6=9C=8D=E6=8E=A5=E5=8F=A3?=
 =?UTF-8?q?=E6=94=B9=E8=BF=9B=E5=AE=8C=E6=88=90?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../__pycache__/dobot_api.cpython-39.pyc      | Bin 136445 -> 136445 bytes
 .../MassageControl/hardware/dobot_nova5.py    |  44 +++++++-----------
 2 files changed, 18 insertions(+), 26 deletions(-)

diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc
index f2d025b3be1d47968cbc186e954c8e69c650fcae..2f713281ba47f06fc11e63e66b8ebfca04fc4ac3 100644
GIT binary patch
delta 36
qcmeynlH>164(>!=UM>b8@DEmO<Zk6=+{(?ABg}YfdxbDlD<c5GlL@l`

delta 36
qcmeynlH>164(>!=UM>b8xYZ!j$lc1#xRskJN0>2cdxbDlD<c5Rp9$sw

diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index 574db20..05624cb 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -164,28 +164,20 @@ class dobot_nova5:
                 break
             sleep(0.1)
 
-    def ServoJoint(self,joints_list,t=-1,aheadtime=-1,gain=-1):
-        # 关节空间伺服
-        recvmovemess = self.dashboard.ServoJ(*joints_list,t,aheadtime,gain)
-        print("ServoJ:",recvmovemess)
-        print(self.parseResultId(recvmovemess))
-        currentCommandID = self.parseResultId(recvmovemess)[1]
-        print("指令 ID", currentCommandID)
-        while True:
-            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
-            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
-                self.dashboard.Stop()
-                self.dashboard.EmergencyStop()
-                print("当前工作停止，机械臂处于急停状态")
-            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
-                print("机械臂为暂停状态")
-                res = self.dashboard.Continue()
-                code = int(res[0])
-                if code == 0:
-                    print("机械臂继续已暂停的运动指令")
-            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
-                print("完成一次伺服动作")
-                break
+    def ServoJoint(self,joints_list):
+        tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]},t=0.008,aheadtime=20,gain=200)"
+        self.dashboard.sendRecvMsg(tcp_command)
+        print("ServoJ:",tcp_command)
+        if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
+            self.dashboard.Stop()
+            self.dashboard.EmergencyStop()
+            print("当前工作停止，机械臂处于急停状态")
+        if self.feedbackData.robotMode == 10: # 10 当前工程暂停
+            print("机械臂为暂停状态")
+            res = self.dashboard.Continue()
+            code = int(res[0])
+            if code == 0:
+                print("机械臂继续已暂停的运动指令")
             
     
     def ServoPose(self,poses_list,t=-1,aheadtime=-1,gain=-1):
@@ -405,9 +397,9 @@ if __name__ == "__main__":
     dobot.RunPoint_P_inJoint(posJ)
     sleep(1)
     
-    # for i in range(200):
-    #     posJ[2] += 0.04
-    #     dobot.ServoJoint(joints_list=posJ,t=0.008,aheadtime=20,gain=200) 
-    #     sleep(0.008)
+    for i in range(400):
+        posJ[2] += 0.04
+        dobot.ServoJoint(posJ)
+        sleep(0.007)
     
     dobot.disableRobot()
\ No newline at end of file