蓝点触控-六维力传感器数据读取程序(Python)

蓝点触控有WRIST系列六维力传感器，支持RS422，EtherCAT，UDP以及Modbus TCP通信。力传感器最重要的功能就是读取力的数据，本文采用UDP读取力的数据。

根据数据手册可知道其请求包的结构以及响应包的结构，以下内容为数据手册中截取

【请求包的结构】

请求数据包结构说明
typedef struct _req_struct
{
        unsigned short magic;// 固定为 0x1234
        unsigned short command; // 要执行的命令，如上所述
        unsigned int count;// 数据计数，为 0 时表示持续输出
} reqStruct;
其中:
magic 占 2 字节，固定为 0x1234，用于识别请求命令。
command 占 2 字节，命令字，指定功能。具体定义见表 1-1。
count 占 4 字节, 数据计数，用来指定该命令要输出的响应包个数。特别地，0 表示不
限制个数，力传感器将持续输出响应包，直到收到停止命令。
注意：请求数据包中的数据字节要求是网络序。同样地，相应数据包中的数据字节也是网络
序，需要先转换为本机字节序使用。
上位机通过 UDP 协议往 192.168.1.20 的 49152 端口发送如上所述的请求数据包，然后接
收（或持续接收）力传感器返回的响应包数据。

【响应包的结构】

typedef struct _resp_struct
{
        unsigned int rdt_sequence;// 响应数据包计数
        unsigned int ft_sequence;// 采集数据包计数
        unsigned int status;// 力传感器状态字
        int Fx;// x 方向的力，除以 1000000 后单位为 N
        int Fy;// y 方向的力，除以 1000000 后单位为 N
        int Fz;// z 方向的力，除以 1000000 后单位为 N
        int Tx;// x 向轴的力距，除以 1000000 后单位为 N·m
        int Ty; // y 向轴的力距，除以 1000000 后单位为 N·m
        int Tz;// z 向轴的力距，除以 1000000 后单位为 N·m
} respStruct;

其中:
rdt_sequence 占 4 字节，低字节在前，表示响应包数据序号。力传感器每发出一个响应
包，rdt_sequence 都加 1。当力传感器收到新的请求包时，会将 rdt_sequence 重新设置为 1。
也就是说，rdt_sequence 表示一次数据请求时响应数据包的序号，每次递增。上位机可以据此判断是否有数据包丢失。
ft_sequence 占 4 字节，低字节在前，表示数据采集包序号。力传感器每采集一组数据，
ft_sequence 都加 1。当 ft_sequence 的值为 0xFFFFFFFF 时，加 1 后会变为 0，然后继续累加。
Fx,Fy,Fz 各占 4 字节，低字节在前，分别表示 x，y，z 方向上除以 1000000 后以 N 为单
位的力的数值。
Tx,Ty,Tz 各占 4 字节，低字节在前，分别表示以 x，y，z 方向为轴除以 1000000 后以 N·m
为单位的力矩值。

【Python代码】

# 读取蓝点触控的数据
import socket
import struct
# 常量定义
PORT = 49152  # UDP端口
COMMAND = 2  # 命令代码
NUM_SAMPLES = 1  # 响应包个数
SERVER_IP = "192.168.1.20"  # 服务器IP地址

AXES = ["Fx", "Fy", "Fz", "Tx", "Ty", "Tz"]
        
def main():
    # 创建一个UDP套接字
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    server_address = (SERVER_IP, PORT)

    while True:
        # 构建请求包数据
        request = struct.pack('!HHI', 0x1234, COMMAND, NUM_SAMPLES)

        try:
            # 发送数据
            print("Sending request...")
            sent = sock.sendto(request, server_address)

            # 接收响应数据
            print("Waiting for response...")
            data, _ = sock.recvfrom(36)  # 接收36字节的响应包

            if len(data) >= 36:
                # 解析响应包数据
                rdt_sequence, ft_sequence, status = struct.unpack('!III', data[:12])
                FTData = struct.unpack('!6i', data[12:])

                print(f"rdt, ft, status: 0x{rdt_sequence:08x}, 0x{ft_sequence:08x}, 0x{status:08x}")
                for i, value in enumerate(FTData):
                    print(f"{AXES[i]}: {value / 1000000.0:.5f}")
        
        except Exception as e:
            print(f"Error: {e}")

if __name__ == '__main__':
    main()