213 lines
6.7 KiB
Python
213 lines
6.7 KiB
Python
#!/usr/bin/python3
|
||
"""
|
||
# @Time : 2025/12/12 11:05
|
||
# @Author : reenrr
|
||
# @File : RobotModel.py
|
||
# @Desc :
|
||
"""
|
||
from enum import Enum
|
||
from Position import Real_Position
|
||
|
||
|
||
class MoveType(Enum):
|
||
AXIS = 4 # 关节轴运动模式
|
||
WORLD = 10 # 世界坐标系运动模式
|
||
Cure = 17 # 曲线运动模式
|
||
|
||
class DATARequest:
|
||
def __init__(self):
|
||
"""
|
||
初始化查询请求的核心协议参数，符合机械臂通信协议要求
|
||
"""
|
||
self.dsID = 'www.hc-system.com.RemoteMonitor' # 设备服务ID
|
||
self.reqType = 'query' # 请求类型
|
||
self.queryAddr = [] # 查询地址列表
|
||
|
||
def toString(self):
|
||
"""
|
||
将DATARequest对象转换为符合JSON格式的字符串（符合机械臂通信协议）
|
||
"""
|
||
model_str = '{'+f'"dsID":"{self.dsID}","reqType":"{self.reqType}","queryAddr":[' \
|
||
f'{self.queryAddr[0].toString()}]'+'}'
|
||
return model_str
|
||
|
||
|
||
|
||
class DataAddress:
|
||
def __init__(self):
|
||
# 基础配置属性
|
||
self.version = ''
|
||
self.curMold = ''
|
||
self.counterList = ''
|
||
self.counter_n = ''
|
||
self.curMode = 0
|
||
self.boardIONum = ''
|
||
|
||
# IO口状态属性
|
||
self.input_n = ''
|
||
self.output_n = 0
|
||
|
||
# 运动轴配置属性
|
||
self.axisNum = '6'
|
||
|
||
self.axis_n = ''
|
||
self.world_0 = 0 # 世界坐标系X轴坐标
|
||
self.world_1 = 0
|
||
self.world_2 = 0
|
||
self.world_3 = 0
|
||
self.world_4 = 0
|
||
self.world_5 = 0
|
||
self.axis_0 = 0
|
||
self.axis_1 = 0 # 1轴关节角度
|
||
self.axis_2 = 0
|
||
self.axis_3 = 0
|
||
self.axis_4 = 0
|
||
self.axis_5 = 0
|
||
|
||
self.curAlarm = 0
|
||
self.curCycle = ''
|
||
self.lastCycle = ''
|
||
self.machineName = ''
|
||
self.curTorque_n = ''
|
||
self.curSpeed_n = ''
|
||
self.curAccount = ''
|
||
self.origin = ''
|
||
self.moldList = ''
|
||
self.isMoving = False
|
||
self.M_n = ''
|
||
self.toolCoord=0
|
||
self.RemoteCmdLen = 0
|
||
|
||
def setPosition(self,w0,w1,w2,w3,w4,w5,a0,a1,a2,a3,a4,a5):
|
||
"""
|
||
位置坐标设置方法：批量更新世界坐标系和关节轴坐标系的位置数据，封装位置更新逻辑
|
||
"""
|
||
self.world_0 = float(w0)
|
||
self.world_1 = float(w1)
|
||
self.world_2 = float(w2)
|
||
self.world_3 = float(w3)
|
||
self.world_4 = float(w4)
|
||
self.world_5 = float(w5)
|
||
|
||
self.axis_0 = float(a0)
|
||
self.axis_1 = float(a1)
|
||
self.axis_2 = float(a2)
|
||
self.axis_3 = float(a3)
|
||
self.axis_4 = float(a4)
|
||
self.axis_5 = float(a5)
|
||
|
||
def getRealPosition(self):
|
||
"""
|
||
提取世界坐标系位置数据，封装成Real_Position对象
|
||
:return: Real_Position对象
|
||
"""
|
||
real_position = Real_Position().init_position(self.world_0,self.world_1,self.world_2,self.world_3,self.world_4,self.world_5)
|
||
return real_position
|
||
|
||
def getAnglePosition(self):
|
||
"""
|
||
提取关节轴位置数据，封装成Real_Position对象
|
||
:return: Real_Position对象
|
||
"""
|
||
real_position = Real_Position().init_position(self.axis_0,self.axis_1,self.axis_2,self.axis_3,self.axis_4,self.axis_5)
|
||
return real_position
|
||
|
||
def get_IO_bits(self):
|
||
"""
|
||
提取IO口状态数据，封装成IO_bits数组
|
||
"""
|
||
io_bits_str = format(self.output_n, '032b')[::-1] # 将output_n转换为32位二进制字符串（不足32位补0），然后反转字符串（低位对应低编号IO口）
|
||
io_bits_arry = [bit == '1' for bit in io_bits_str] # 转换为布尔数组，标识每个IO口的通断状态
|
||
return io_bits_arry
|
||
|
||
def setAngle(self,a0,a1,a2,a3,a4,a5):
|
||
pass
|
||
|
||
def toString(self):
|
||
model_str = f'"curMode",' \
|
||
f'"world-0","world-1","world-2","world-3","world-4","world-5","axis-0","axis-1","axis-2","axis-3","axis-4","axis-5",' \
|
||
f'"curAlarm","isMoving","RemoteCmdLen","toolCoord","input-n","output-n","curSpeed-n"'
|
||
return model_str
|
||
|
||
|
||
|
||
|
||
|
||
class DATAReply:
|
||
def __init__(self):
|
||
self.dsID = ''
|
||
self.reqType = ''
|
||
self.queryData = []
|
||
|
||
|
||
|
||
def JsonToObject(self):
|
||
return
|
||
|
||
class CMDReply:
|
||
def __init__(self):
|
||
self.dsID = 'www.hc-system.com.RemoteMonitor'
|
||
self.reqType = 'command'
|
||
self.cmdData = []
|
||
return
|
||
|
||
|
||
class Instruction:
|
||
def __init__(self):
|
||
self.oneshot = 1
|
||
self.action = 4 #4 自由路径 10 姿势直线 17 姿势曲线
|
||
self.m0 = 0.0
|
||
self.m1 = 0.0
|
||
self.m2 = 0.0
|
||
self.m3 = 0.0
|
||
self.m0_p = 0.0
|
||
self.m1_p = 0.0
|
||
self.m2_p = 0.0
|
||
self.m3_p = 0.0
|
||
self.m4_p = 0.0
|
||
self.m5_p = 0.0
|
||
self.ckStatus = '0x3F'
|
||
self.speed = 10
|
||
self.smooth = 0
|
||
self.tool=2
|
||
self.IO = False
|
||
self.type = 0
|
||
self.io_status=1
|
||
self.point = 0
|
||
self.delay = 0
|
||
|
||
def toString(self):
|
||
if not self.IO :
|
||
model_str = f'"oneshot":"{self.oneshot}","action":"{self.action}","m0":"{self.m0}","m1":"{self.m1}","m2":"{self.m2}",' \
|
||
f'"m3":"{self.m3}","ckStatus":"{self.ckStatus}","speed":"{self.speed}",' \
|
||
f'"delay":"{self.delay}","smooth":"{self.smooth}","tool":"{self.tool}"'
|
||
if self.action == 17:
|
||
model_str = f'"oneshot":"{self.oneshot}","action":"{self.action}","m0":"{self.m0}","m1":"{self.m1}","m2":"{self.m2}",' \
|
||
f'"m3":"{self.m3}","m0_p":"{self.m0_p}","m1_p":"{self.m1_p}","m2_p":"{self.m2_p}",' \
|
||
f'"m3_p":"{self.m3_p}","ckStatus":"{self.ckStatus}","speed":"{self.speed}",' \
|
||
f'"delay":"{self.delay}","smooth":"{self.smooth}","tool":"{self.tool}"'
|
||
else:
|
||
model_str = f'"oneshot":"{self.oneshot}","action":"{200}","type":"{self.type}","io_status":"{self.io_status}"' \
|
||
f',"point":"{self.point}","delay":"{self.delay}"'
|
||
return model_str
|
||
|
||
|
||
|
||
|
||
class CMDInstructRequest:
|
||
def __init__(self):
|
||
self.dsID = 'www.hc-system.com.HCRemoteCommand'
|
||
self.reqType = "AddRCC"
|
||
self.emptyList = '0' #清空机械臂的远程命令数据
|
||
self.instructions = []
|
||
|
||
def toString(self):
|
||
model_str = '{'+f'"dsID":"{self.dsID}","reqType":"{self.reqType}","emptyList":"{self.emptyList}"'
|
||
if len(self.instructions) != 0:
|
||
model_str = model_str+',"instructions":'+"[{"+self.instructions[0].toString()+"}]"+"}"
|
||
else:
|
||
model_str = model_str+',"instructions":'+"[]"+"}" #model_str+"}"
|
||
return model_str
|
||
|
||
|