DevelopmentTeam/MassageRobot_Dobot
12
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加了部分异常处理，验证了底层的伺服运动频率

Browse Source
This commit is contained in:
Ziwei.He 2025-05-09 18:56:50 +08:00
parent 2dfb5f8447
commit 286b606f5a
5 changed files with 30709 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Massage/MassageControl/MassageRobot_nova5 → Massage/MassageControl/MassageRobot_nova5.py
View File

@ -119,6 +119,14 @@ class MassageRobot:
self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
self.arm_measure_thread.start()
position,quat_rot = self.arm.get_arm_position()
# 初始值
self.arm_state.desired_position = position
self.arm_state.arm_position_command = position
self.arm_state.desired_orientation = quat_rot
self.arm_state.arm_orientation_command = quat_rot
time.sleep(0.5)
def stop(self):

BIN
Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-39.pyc
View File

Binary file not shown.

101
Massage/MassageControl/hardware/dobot_nova5.py
View File

@ -1,8 +1,9 @@
from dobot_api import DobotApiFeedBack,DobotApiDashboard
import threading
from time import sleep
from time import sleep,time
import re
import numpy as np
from scipy.spatial.transform import Rotation as R
class dobot_nova5:
def __init__(self,ip):
self.ip = ip
@ -53,6 +54,7 @@ class dobot_nova5:
def start(self):
self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort)
self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort)
self.clearError() # 清除报警
if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
print("使能失败:检查29999端口是否被占用")
return
@ -100,7 +102,8 @@ class dobot_nova5:
print(self.parseResultId(recvmovemess))
currentCommandID = self.parseResultId(recvmovemess)[1]
print("指令 ID:", currentCommandID)
# sleep(0.02)
sleep(0.02)
start_time = time() # 记录开始时间
while True: # 完成判断循环
########## 调试注释 #############
# print("robot mode",self.feedbackData.robotMode)
@ -109,10 +112,21 @@ class dobot_nova5:
# print("30004",self.dashboard.GetCurrentCommandID())
########## 调试注释 #############
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
sleep(0.1)
sleep(0.1) # 避免 CPU 占用过高
def RunPoint_L_inPose(self,poses_list):
'''
@ -159,9 +173,20 @@ class dobot_nova5:
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 ServoPose(self,poses_list,t=-1,aheadtime=-1,gain=-1):
# 位姿伺服
@ -189,7 +214,8 @@ class dobot_nova5:
print("工具坐标系序号超出范围")
return
code = self.dashboard.SetTool(i,tool_i)
res = self.dashboard.SetTool(i,tool_i)
code = int(res[0])
if code == 0:
print(f"设置工具坐标系{i}的偏移量成功")
else:
@ -203,7 +229,8 @@ class dobot_nova5:
if i not in {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}:
print("工具坐标系序号超出范围")
return
code = self.dashboard.Tool(i)
res = self.dashboard.Tool(i)
code = int(res[0])
if code == 0:
print(f"切换为工具坐标系{i}成功")
else:
@ -223,7 +250,8 @@ class dobot_nova5:
print("用户坐标系序号超出范围")
return
code = self.dashboard.SetUser(i,base_i)
res = self.dashboard.SetUser(i,base_i)
code = int(res[0])
if code == 0:
print(f"设置用户坐标系{i}成功")
else:
@ -237,7 +265,8 @@ class dobot_nova5:
if i not in {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}:
print("工具坐标系序号超出范围")
return
code = self.dashboard.User(i)
res = self.dashboard.User(i)
code = int(res[0])
if code == 0:
print(f"切换为用户基坐标系{i}成功")
else:
@ -317,7 +346,8 @@ class dobot_nova5:
return joint
def disableRobot(self):
code = self.dashboard.DisableRobot()
res = self.dashboard.DisableRobot()
code = int(res[0])
if code == 0:
print("下使能机械臂成功")
else:
@ -325,30 +355,59 @@ class dobot_nova5:
return
def clearError(self):
code = self.dashboard.ClearError()
res = self.dashboard.ClearError()
code = int(res[0])
if code == 0:
print("清楚报警成功")
else:
print("清楚报警失败")
return
def start_drag(self):
res = self.dashboard.StartDrag()
code = int(res[0])
if code == 0:
print("拖拽模式开启成功")
else:
print("拖拽模式开启失败")
return
def stop_drag(self):
res = self.dashboard.StopDrag()
code = int(res[0])
if code == 0:
print("拖拽模式关闭成功")
else:
print("拖拽模式关闭失败")
return
def __del__(self):
del self.dashboard
del self.feedFour
#########
def get_arm_position(self):
pose = self.getPose()
x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
pos = np.array([x,y,z])
quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat()
return pos,quat_rot
#########
if __name__ == "__main__":
dobot = dobot_nova5("192.168.5.1")
dobot.start()
posJ = [0,30,-120,0,90,0]
sleep(1)
dobot.RunPoint_P_inJoint(posJ)
############### 测试 #################
# curAngle = dobot.getAngel()
# curPose = dobot.getPose()
# res1 = dobot.getForwardKin(curAngle)
# res2 = dobot.getInverseKin(curPose)
# print(res1)
# print(res2)
# dobot.disableRobot()
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)
dobot.disableRobot()

117
Massage/MassageControl/hardware/servoj_8ms.py Normal file
View File

@ -0,0 +1,117 @@
import socket
from time import sleep
import time
import threading
import logging
from logging.handlers import RotatingFileHandler
def setup_logger(log_file='app.log'):
# 创建日志记录器
logger = logging.getLogger('my_app')
logger.setLevel(logging.DEBUG)
# 创建格式化器
formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
# 创建旋转文件处理器
# maxBytes=50MB (50 * 1024 * 1024), backupCount=5
handler = RotatingFileHandler(
log_file,
maxBytes=50 * 1024 * 1024, # 50MB
backupCount=5, # 保留5个备份文件
encoding='utf-8'
)
handler.setFormatter(formatter)
# 添加处理器到日志记录器
logger.addHandler(handler)
return logger
def ServoP(client, pt_floats):
tcp_command = f"ServoP({pt_floats[0]},{pt_floats[1]},{pt_floats[2]},{pt_floats[3]},{pt_floats[4]},{pt_floats[5]}, t=0.004, aheadtime=80, gain=500)"
tcp_command = tcp_command.encode()
client.send(tcp_command)
def ServoJ(client, J1_angle):
tcp_command = "servoj(0,%f,%f,0,0,0,t=0.008,aheadtime=20, gain=200)" % (J1_angle,J1_angle/2)
tcp_command = tcp_command.encode()
client.sendall(tcp_command)
# buf = client.recv(200).decode() # 接收反馈信息的长度
# print(buf)
def servoj_2(client, J1_angle):
tcp_command = "servoj(%f,0,0,0,0,0,t=2,aheadtime=50, gain=500)" % (J1_angle)
tcp_command = tcp_command.encode()
print(tcp_command)
client.send(tcp_command)
def EnableRobot(client):
tcp_command = "EnableRobot()"
tcp_command = tcp_command.encode()
client.send(tcp_command)
buf = client.recv(200).decode() # 接收反馈信息的长度
index_str = buf[buf.find('{') + 1:buf.find('}')]
if (index_str == ''):
return -1
return buf[buf.find('{') + 1:buf.find('}')]
stop = True
def enableStatus(client):
global stop
logger = setup_logger()
servoj_2(client, 0)
sleep(3)
J1 = 0
bias = 0.04
pl = bias
while stop:
time_home = time.time()
if J1 > 60 and bias == pl:
bias = -pl
elif J1 < -1 and bias == -pl:
bias = pl
J1 = J1 + bias
ServoJ(client, J1)
time.sleep(0.007)
tie = time.time()-time_home
logger.info(f"time: {tie}")
print(tie)
SERVER3_ADDRESS = '192.168.5.1'
SERVER3_PORT = 29999
client_socket1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# 设置TCP_NODELAY禁用Nagle算法
client_socket1.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
client_socket1.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 100000)
client_socket1.connect((SERVER3_ADDRESS, SERVER3_PORT))
#client_socket1.setblocking(False)
print('成功连接到第1个服务端')
#enableStatus(client_socket1)
def worker(): #子线程接受模式
global stop
while True:
try:
data = client_socket1.recv(1024).decode()
if data:
print(data)
index_str = data[data.find('{') + 1:data.find('}')]
if (index_str == ''):
stop = False
print(11111111111)
else:
stop = True
except BlockingIOError:
time.sleep(0.001)
time_h = time.time()
try:
EnableRobot(client_socket1)
feed_thread = threading.Thread(
target=worker) # 机器状态反馈线程
feed_thread.daemon = True
feed_thread.start()
enableStatus(client_socket1)
except KeyboardInterrupt:
print("用户中断操作。")
print((time.time()-time_h)/60)
except Exception as e:
print("发生异常：", e)
print((time.time() - time_h) / 60)
print((time.time()-time_h)/60)

30504
app.log Normal file
View File

File diff suppressed because it is too large Load Diff