572 lines
20 KiB
Python
572 lines
20 KiB
Python
import time
|
||
import socket
|
||
import struct
|
||
import numpy as np
|
||
import atexit
|
||
import threading
|
||
from typing import Optional, Tuple
|
||
from matplotlib import pyplot as plt
|
||
import csv
|
||
from datetime import datetime
|
||
|
||
class XjcSensor:
|
||
def __init__(self, host: str, port: int, rate: int = 250):
|
||
"""
|
||
初始化 TCP 连接的力传感器
|
||
:param host: 设备 IP 地址（如 "192.168.1.100"）
|
||
:param port: 设备端口（如 502）
|
||
:param rate: 数据采样率（100/250/500Hz）
|
||
"""
|
||
self.host = host
|
||
self.port = port
|
||
self.rate = rate
|
||
self.slave_address = 0x01 # 默认从机地址
|
||
self.sock = None # TCP socket 对象
|
||
self.crc16_table = self.generate_crc16_table()
|
||
# 后台读取相关的属性
|
||
self._background_thread = None
|
||
self._stop_background = False
|
||
self._last_reading = None
|
||
self._reading_lock = threading.Lock()
|
||
# 建立 TCP 连接
|
||
self.connect()
|
||
|
||
# 注册退出时的清理函数
|
||
atexit.register(self.disconnect)
|
||
|
||
def __new__(cls, *args, **kwargs):
|
||
"""单例模式"""
|
||
if not hasattr(cls, 'instance'):
|
||
cls.instance = super(XjcSensor, cls).__new__(cls)
|
||
return cls.instance
|
||
|
||
def connect(self):
|
||
"""建立 TCP 连接"""
|
||
try:
|
||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||
self.sock.settimeout(1.0) # 设置超时时间
|
||
self.sock.connect((self.host, self.port))
|
||
print(f"Connected to {self.host}:{self.port}")
|
||
except Exception as e:
|
||
print(f"TCP connection failed: {e}")
|
||
raise
|
||
|
||
def send_and_receive(self, data: bytes, expected_response_len: int) -> Optional[bytes]:
|
||
"""
|
||
发送数据并接收响应
|
||
:param data: 要发送的字节数据
|
||
:param expected_response_len: 预期响应的长度
|
||
:return: 收到的字节数据（失败返回 None）
|
||
"""
|
||
try:
|
||
self.sock.sendall(data)
|
||
response = self.sock.recv(expected_response_len)
|
||
return response
|
||
except socket.timeout:
|
||
print("Timeout while waiting for response")
|
||
return None
|
||
except Exception as e:
|
||
print(f"TCP communication error: {e}")
|
||
return None
|
||
|
||
def set_zero(self) -> int:
|
||
"""传感器置零（TCP 版本）"""
|
||
if self.slave_address == 0x01:
|
||
zero_cmd = bytes.fromhex('01 10 46 1C 00 02 04 00 00 00 00 E8 95')
|
||
elif self.slave_address == 0x09:
|
||
zero_cmd = bytes.fromhex('09 10 46 1C 00 02 04 00 00 00 00 C2 F5')
|
||
|
||
response = self.send_and_receive(zero_cmd, 8)
|
||
if not response:
|
||
print("set zero fail")
|
||
return -1
|
||
|
||
# CRC 校验（假设协议要求）
|
||
hi_check, lo_check = self.crc16(response[:-2])
|
||
if (response[0] != self.slave_address or
|
||
response[1] != 0x10 or
|
||
response[-2] != lo_check or
|
||
response[-1] != hi_check):
|
||
print("Set zero failed: CRC check error")
|
||
return -1
|
||
|
||
print("Set zero success!")
|
||
return 0
|
||
|
||
def read_data_f32(self) -> np.ndarray:
|
||
"""
|
||
TCP版本 - 读取六维力传感器数据 (float32格式)
|
||
返回:
|
||
np.ndarray(6,) - 六维力数据 [Fx,Fy,Fz,Mx,My,Mz]
|
||
读取失败返回 -1
|
||
"""
|
||
# 构造读取命令
|
||
|
||
if self.slave_address == 0x01:
|
||
command = bytes.fromhex('01 04 00 00 00 0C F0 0F')
|
||
elif self.slave_address == 0x09:
|
||
command = bytes.fromhex('09 04 00 54 00 0C B0 97')
|
||
|
||
try:
|
||
# 清空接收缓冲区
|
||
self._clear_tcp_buffer()
|
||
|
||
# 接收完整响应
|
||
response = self.send_and_receive(command, 29)
|
||
|
||
# CRC校验
|
||
Hi_check, Lo_check = self.crc16(response[:-2])
|
||
if (response[0] != self.slave_address or
|
||
response[1] != 0x04 or
|
||
response[2] != 24 or # 数据长度字节
|
||
response[-2] != Lo_check or
|
||
response[-1] != Hi_check):
|
||
print(f"Protocol error! Received: {response.hex(' ')}")
|
||
print(f"Expected slave:{self.slave_address}, "
|
||
f"func:0x04, len:24, "
|
||
f"CRC:{Lo_check:02X}{Hi_check:02X}")
|
||
return -1
|
||
|
||
# 解析6个float32数据 (大端序)
|
||
sensor_data = struct.unpack('>ffffff', response[3:27])
|
||
return np.array(sensor_data)
|
||
|
||
except socket.timeout:
|
||
print("Timeout while waiting for sensor response")
|
||
return -1
|
||
except ConnectionError as e:
|
||
print(f"TCP connection error: {e}")
|
||
return -1
|
||
except Exception as e:
|
||
print(f"Unexpected error in read_data_f32: {str(e)}")
|
||
return -1
|
||
|
||
def _clear_tcp_buffer(self):
|
||
"""清空TCP接收缓冲区"""
|
||
self.sock.settimeout(0.1) # 短暂超时，避免阻塞
|
||
try:
|
||
while True:
|
||
data = self.sock.recv(1024)
|
||
if not data: # 连接关闭或无数据
|
||
break
|
||
print(f"Cleared TCP buffer: {data.hex(' ')}") # 调试输出
|
||
except socket.timeout:
|
||
pass # 预期内的超时，表示缓冲区已空
|
||
finally:
|
||
self.sock.settimeout(2.0) # 恢复默认超时
|
||
|
||
def enable_active_transmission(self) -> int:
|
||
"""启用主动传输模式（TCP 版本）"""
|
||
if self.rate == 100:
|
||
cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 00 AB 6A') if self.slave_address == 0x01 else \
|
||
bytes.fromhex('09 10 01 9A 00 01 02 00 00 CC AA')
|
||
elif self.rate == 250:
|
||
cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 01 6A AA') if self.slave_address == 0x01 else \
|
||
bytes.fromhex('09 10 01 9A 00 01 02 00 01 0D 6A')
|
||
elif self.rate == 500:
|
||
cmd = bytes.fromhex('01 10 01 9A 00 01 02 00 02 2A AB') if self.slave_address == 0x01 else \
|
||
bytes.fromhex('09 10 01 9A 00 01 02 00 02 4D 6B')
|
||
else:
|
||
print("Unsupported rate")
|
||
return -1
|
||
|
||
response = self.send_and_receive(cmd, 8)
|
||
if response and response[1] == 0x10: # 检查响应功能码
|
||
print(f"Active transmission enabled at {self.rate}Hz")
|
||
return 0
|
||
else:
|
||
print("Failed to enable active transmission")
|
||
return -1
|
||
|
||
def disconnect(self):
|
||
"""关闭 TCP 连接"""
|
||
if self.sock:
|
||
try:
|
||
self.sock.close()
|
||
print("TCP connection closed")
|
||
except Exception as e:
|
||
print(f"Error while closing socket: {e}")
|
||
self.sock = None
|
||
def start_background_reading(self):
|
||
"""
|
||
启动后台读取任务，每秒读取一次传感器数据
|
||
"""
|
||
if self._background_thread is not None and self._background_thread.is_alive():
|
||
print("Background reading is already running")
|
||
return False
|
||
|
||
self._stop_background = False
|
||
self._background_thread = threading.Thread(target=self._background_reading_task)
|
||
self._background_thread.daemon = True # 设置为守护线程
|
||
self._background_thread.start()
|
||
return True
|
||
|
||
def stop_background_reading(self):
|
||
"""
|
||
停止后台读取任务
|
||
"""
|
||
if self._background_thread is None or not self._background_thread.is_alive():
|
||
print("No background reading is running")
|
||
return False
|
||
|
||
self._stop_background = True
|
||
self._background_thread.join(timeout=2.0) # 等待线程结束，最多等待2秒
|
||
self._background_thread = None
|
||
return True
|
||
|
||
def _background_reading_task(self):
|
||
"""
|
||
后台读取任务的实现
|
||
"""
|
||
while not self._stop_background:
|
||
try:
|
||
data = self.read_data_f32()
|
||
if isinstance(data, np.ndarray):
|
||
with self._reading_lock:
|
||
self._last_reading = data
|
||
else:
|
||
self.connect()
|
||
except Exception as e:
|
||
print(f"Error in background reading: {e}")
|
||
time.sleep(1.0) # 每秒读取一次
|
||
|
||
def get_last_reading(self):
|
||
"""
|
||
获取最近一次的读数
|
||
"""
|
||
with self._reading_lock:
|
||
return self._last_reading
|
||
|
||
# ------------------------- 以下方法保持不变 -------------------------
|
||
@staticmethod
|
||
def generate_crc16_table():
|
||
"""CRC16 表生成（与原代码相同）"""
|
||
table = []
|
||
polynomial = 0xA001
|
||
for i in range(256):
|
||
crc = i
|
||
for _ in range(8):
|
||
if crc & 0x0001:
|
||
crc = (crc >> 1) ^ polynomial
|
||
else:
|
||
crc >>= 1
|
||
table.append(crc)
|
||
return table
|
||
|
||
def crc16(self, data: bytes):
|
||
"""CRC16 计算（与原代码相同）"""
|
||
crc = 0xFFFF
|
||
for byte in data:
|
||
crc = (crc >> 8) ^ self.crc16_table[(crc ^ byte) & 0xFF]
|
||
return (crc >> 8) & 0xFF, crc & 0xFF
|
||
|
||
def __del__(self):
|
||
"""析构时自动断开连接"""
|
||
self.disconnect()
|
||
|
||
def disable_active_transmission(self) -> int:
|
||
"""
|
||
禁用传感器的主动传输模式（TCP 版本）
|
||
Returns:
|
||
0 表示成功，-1 表示失败
|
||
"""
|
||
try:
|
||
# 构造禁用命令（原代码中的 FF x11 字节流）
|
||
disable_cmd = bytes.fromhex('FF FF FF FF FF FF FF FF FF FF FF')
|
||
print("Disabling active transmission mode...")
|
||
|
||
# 检查 TCP 连接是否有效
|
||
if not self.sock:
|
||
raise ConnectionError("TCP connection is not established")
|
||
|
||
# 发送禁用命令
|
||
self.sock.sendall(disable_cmd)
|
||
print("Active transmission disabled successfully")
|
||
return 0
|
||
|
||
except ConnectionError as e:
|
||
print(f"Connection error: {e}")
|
||
return -1
|
||
except socket.timeout:
|
||
print("Timeout while sending disable command")
|
||
return -1
|
||
except Exception as e:
|
||
print(f"Unexpected error: {e}")
|
||
return -1
|
||
|
||
def read(self):
|
||
"""
|
||
TCP Version - Read the sensor's data in passive mode.
|
||
Returns:
|
||
np.ndarray(6,) - Six-axis force/torque data [Fx, Fy, Fz, Mx, My, Mz]
|
||
None if reading fails
|
||
"""
|
||
try:
|
||
# Clear any existing data in the TCP buffer first
|
||
# self._clear_tcp_buffer()
|
||
|
||
# Search for frame header (0x20 0x4E)
|
||
header_found = False
|
||
start_time = time.time()
|
||
timeout = 1.0 # 1 second timeout
|
||
|
||
while not header_found and (time.time() - start_time < timeout):
|
||
# Read one byte at a time looking for header
|
||
byte1 = self.sock.recv(1)
|
||
if not byte1:
|
||
continue # No data available yet
|
||
|
||
if byte1 == b'\x20':
|
||
byte2 = self.sock.recv(1)
|
||
if byte2 == b'\x4E':
|
||
header_found = True
|
||
|
||
if not header_found:
|
||
print("Frame header not found within timeout period")
|
||
return None
|
||
|
||
# Now read the remaining 14 bytes of the frame
|
||
response = bytearray([0x20, 0x4E]) # Start with the header we found
|
||
remaining_bytes = 14
|
||
bytes_received = 0
|
||
start_time = time.time()
|
||
|
||
while bytes_received < remaining_bytes and (time.time() - start_time < timeout):
|
||
chunk = self.sock.recv(remaining_bytes - bytes_received)
|
||
if chunk:
|
||
response.extend(chunk)
|
||
bytes_received += len(chunk)
|
||
|
||
if bytes_received < remaining_bytes:
|
||
print(f"Incomplete frame received. Got {len(response)} bytes, expected 16")
|
||
return None
|
||
|
||
# Verify CRC checksum
|
||
Hi_check, Lo_check = self.crc16(response[:-2])
|
||
if response[-1] != Hi_check or response[-2] != Lo_check:
|
||
print("CRC check failed!")
|
||
print(f"Received CRC: {response[-2]:02X}{response[-1]:02X}")
|
||
print(f"Calculated CRC: {Lo_check:02X}{Hi_check:02X}")
|
||
return None
|
||
|
||
# Parse the sensor data
|
||
sensor_data = self.parse_data_passive(response)
|
||
return sensor_data
|
||
|
||
except socket.timeout:
|
||
print("Timeout while waiting for sensor data")
|
||
return None
|
||
except ConnectionError as e:
|
||
print(f"TCP connection error: {e}")
|
||
return None
|
||
except Exception as e:
|
||
print(f"Unexpected error in TCP read(): {str(e)}")
|
||
return None
|
||
|
||
def parse_data_passive(self, buffer):
|
||
values = [
|
||
int.from_bytes(buffer[i:i+2], byteorder='little', signed=True)
|
||
for i in range(2, 14, 2)
|
||
]
|
||
Fx, Fy, Fz = np.array(values[:3]) / 10.0
|
||
Mx, My, Mz = np.array(values[3:]) / 1000.0
|
||
return np.array([Fx, Fy, Fz, Mx, My, Mz])
|
||
|
||
|
||
# 外部
|
||
def test_sensor_frequency(sensor, mode='active', duration=5.0):
|
||
"""
|
||
测试传感器在不同模式下的实际数据获取频率
|
||
|
||
参数:
|
||
sensor: XjcSensor实例
|
||
mode: 'active'(主动模式) 或 'polling'(查询模式)
|
||
duration: 测试持续时间(秒)
|
||
|
||
返回:
|
||
dict: 包含测试结果的字典
|
||
"""
|
||
# 准备测试环境
|
||
if mode == 'active':
|
||
print("\n=== 测试主动传输模式 ===")
|
||
sensor.enable_active_transmission()
|
||
read_func = sensor.read
|
||
else:
|
||
print("\n=== 测试查询模式 ===")
|
||
sensor.disable_active_transmission() # 确保不在主动模式
|
||
time.sleep(0.5)
|
||
sensor.disable_active_transmission() # 确保不在主动模式
|
||
time.sleep(0.5)
|
||
sensor.disable_active_transmission() # 确保不在主动模式
|
||
time.sleep(0.5)
|
||
read_func = sensor.read_data_f32
|
||
|
||
# 初始化测试变量
|
||
timestamps = []
|
||
data_count = 0
|
||
start_time = time.perf_counter()
|
||
end_time = start_time + duration
|
||
|
||
print(f"开始测试，持续时间 {duration} 秒...")
|
||
|
||
# 主测试循环
|
||
while time.perf_counter() < end_time:
|
||
data = read_func()
|
||
print(data)
|
||
if data is not None:
|
||
timestamps.append(time.perf_counter())
|
||
data_count += 1
|
||
# print(timestamps)
|
||
# 计算统计结果
|
||
if data_count < 2:
|
||
print("警告: 采集到的数据点不足")
|
||
return None
|
||
|
||
intervals = np.diff(timestamps)
|
||
save_intervals_to_csv(timestamps)
|
||
avg_interval = np.mean(intervals)
|
||
min_interval = np.min(intervals)
|
||
max_interval = np.max(intervals)
|
||
std_interval = np.std(intervals)
|
||
avg_freq = 1.0 / avg_interval
|
||
|
||
# 打印结果
|
||
print("\n测试结果:")
|
||
print(f"总数据点数: {data_count}")
|
||
print(f"平均频率: {avg_freq:.2f} Hz")
|
||
print(f"最小间隔: {min_interval*1000:.3f} ms")
|
||
print(f"最大间隔: {max_interval*1000:.3f} ms")
|
||
print(f"间隔标准差: {std_interval*1000:.3f} ms")
|
||
|
||
# 返回结果字典
|
||
return {
|
||
'mode': mode,
|
||
'duration': duration,
|
||
'data_count': data_count,
|
||
'avg_freq': avg_freq,
|
||
'min_interval': min_interval,
|
||
'max_interval': max_interval,
|
||
'std_interval': std_interval,
|
||
'timestamps': timestamps,
|
||
'intervals': intervals
|
||
}
|
||
|
||
def plot_test_results(active_results, polling_results):
|
||
"""绘制两种模式的测试结果对比图"""
|
||
plt.figure(figsize=(12, 8))
|
||
|
||
# 间隔时间分布图
|
||
plt.subplot(2, 1, 1)
|
||
plt.hist(active_results['intervals']*1000, bins=50, alpha=0.7, label='主动模式')
|
||
plt.hist(polling_results['intervals']*1000, bins=50, alpha=0.7, label='查询模式')
|
||
plt.xlabel('间隔时间 (ms)')
|
||
plt.ylabel('出现次数')
|
||
plt.title('数据间隔时间分布')
|
||
plt.legend()
|
||
plt.grid(True)
|
||
|
||
# 间隔时间序列图
|
||
plt.subplot(2, 1, 2)
|
||
plt.plot(np.arange(len(active_results['intervals'])),
|
||
active_results['intervals']*1000, 'b.', label='主动模式')
|
||
plt.plot(np.arange(len(polling_results['intervals'])),
|
||
polling_results['intervals']*1000, 'r.', label='查询模式')
|
||
plt.xlabel('数据点序号')
|
||
plt.ylabel('间隔时间 (ms)')
|
||
plt.title('数据间隔时间序列')
|
||
plt.legend()
|
||
plt.grid(True)
|
||
|
||
plt.tight_layout()
|
||
plt.show()
|
||
|
||
def compare_modes(sensor, duration=5.0):
|
||
"""比较两种工作模式的性能"""
|
||
# 测试主动模式
|
||
active_results = test_sensor_frequency(sensor, 'active', duration)
|
||
time.sleep(1) # 模式切换间隔
|
||
|
||
# 测试查询模式
|
||
polling_results = test_sensor_frequency(sensor, 'polling', duration)
|
||
time.sleep(1)
|
||
|
||
# 打印对比报告
|
||
print("\n=== 模式对比报告 ===")
|
||
print(f"{'指标':<15} {'主动模式':>15} {'查询模式':>15}")
|
||
print(f"{'平均频率(Hz)':<15} {active_results['avg_freq']:>15.2f} {polling_results['avg_freq']:>15.2f}")
|
||
print(f"{'最小间隔(ms)':<15} {active_results['min_interval']*1000:>15.3f} {polling_results['min_interval']*1000:>15.3f}")
|
||
print(f"{'最大间隔(ms)':<15} {active_results['max_interval']*1000:>15.3f} {polling_results['max_interval']*1000:>15.3f}")
|
||
print(f"{'间隔标准差(ms)':<15} {active_results['std_interval']*1000:>15.3f} {polling_results['std_interval']*1000:>15.3f}")
|
||
|
||
# 绘制对比图表
|
||
plot_test_results(active_results, polling_results)
|
||
|
||
return active_results, polling_results
|
||
|
||
def save_intervals_to_csv(timestamps, filename="sensor_intervals.csv"):
|
||
"""
|
||
Save timestamp intervals to CSV file with metadata
|
||
|
||
Args:
|
||
timestamps: List of timestamp values (from time.perf_counter())
|
||
filename: Output CSV filename
|
||
"""
|
||
if len(timestamps) < 2:
|
||
print("Not enough timestamps to calculate intervals")
|
||
return
|
||
|
||
# Calculate intervals in milliseconds
|
||
intervals_ms = np.diff(timestamps) * 1000
|
||
|
||
# Prepare data rows
|
||
rows = []
|
||
for i, interval in enumerate(intervals_ms):
|
||
rows.append({
|
||
"reading_number": i+1,
|
||
"interval_ms": f"{interval:.4f}",
|
||
"timestamp": timestamps[i],
|
||
"expected_interval_ms": (1000/250) if i==0 else "", # For 250Hz
|
||
"deviation_ms": f"{interval - (1000/250):.4f}" if i>0 else ""
|
||
})
|
||
|
||
# CSV header
|
||
fieldnames = ["reading_number", "interval_ms", "timestamp",
|
||
"expected_interval_ms", "deviation_ms"]
|
||
|
||
# Write to file
|
||
with open(filename, 'w', newline='') as csvfile:
|
||
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
|
||
writer.writeheader()
|
||
writer.writerows(rows)
|
||
|
||
print(f"Saved {len(intervals_ms)} intervals to {filename}")
|
||
|
||
if __name__ == "__main__":
|
||
# 替换为你的传感器 IP 和端口
|
||
sensor = XjcSensor("192.168.5.1", 60000)
|
||
|
||
# 示例操作
|
||
|
||
sensor.disable_active_transmission()
|
||
time.sleep(0.5)
|
||
sensor.disable_active_transmission()
|
||
time.sleep(0.5)
|
||
sensor.disable_active_transmission()
|
||
time.sleep(0.5)
|
||
sensor.set_zero()
|
||
|
||
# sensor.set_zero()
|
||
# sensor.disable_active_transmission()
|
||
|
||
# while True:
|
||
# data = sensor.read_data_f32()
|
||
# if data is not None:
|
||
# print(f"Force data: {data}")
|
||
# sensor.enable_active_transmission()
|
||
# while True:
|
||
# sensor_data = sensor.read()
|
||
# if sensor_data is None:
|
||
# print('failed to get force sensor data!')
|
||
# print(sensor_data)
|
||
test_sensor_frequency(sensor)
|
||
# compare_modes(sensor) |