1. 大寰夹爪
import serial
import time
COM_PORT = 'COM3'
BAUD_RATE = 115200
PARITY = 'N'
STOP_BITS = 1
DATA_BITS = 8
ser = serial.Serial(
port=COM_PORT,
baudrate=BAUD_RATE,
bytesize=DATA_BITS,
parity=PARITY,
stopbits=STOP_BITS,
timeout=1
)
if not ser.is_open:
ser.open()
def calculate_crc(data):
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_read_request(slave_address, register_address):
request = bytes([slave_address, 0x03, (register_address >> 8) & 0xFF, register_address & 0xFF, 0x00, 0x01])
crc = calculate_crc(request)
request += crc.to_bytes(2, byteorder='little')
ser.write(request)
time.sleep(0.1)
response = ser.read(ser.in_waiting)
return response
def send_write_request(slave_address, register_address, value):
request = bytes([slave_address, 0x06, (register_address >> 8) & 0xFF, register_address & 0xFF, (value >> 8) & 0xFF, value & 0xFF])
crc = calculate_crc(request)
request += crc.to_bytes(2, byteorder='little')
ser.write(request)
time.sleep(0.1)
response = ser.read(ser.in_waiting)
return response
response = send_read_request(1, 0)
if len(response) >= 5:
status = response[3]
print(f"保持寄存器的值：{status}")
else:
print("读取失败")
response = send_write_request(1, 0x0101, 0x0014)
if len(response) >= 5:
print("写入成功")
else:
print("写入失败")
response = send_read_request(1, 0x0101)
if len(response) >= 5:
status = response[4]
print(f"读取当前设定力大小：{status}")
else:
print("读取失败")
response = send_write_request(1, 0x0103, 0x0000)
if len(response) >= 5:
print("写入成功")
else:
print("写入失败")
ser.close()
2. Realsense 相机
import pyrealsense2 as rs
import numpy as np
import cv2
pipeline = rs.pipeline()
config = rs.config()
config.enable_device('218622279681')
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale)
align_to = rs.stream.color
align = rs.align(align_to)
depth_stream = profile.get_stream(rs.stream.depth)
intrinsics = depth_stream.as_video_stream_profile().get_intrinsics()
print("Width:", intrinsics.width)
print("Height:", intrinsics.height)
print("PPX:", intrinsics.ppx)
print("PPY:", intrinsics.ppy)
print("FX:", intrinsics.fx)
print("FY:", intrinsics.fy)
print("Distortion Model:", intrinsics.model)
print("Coefficients:", intrinsics.coeffs)
try:
while True:
frames = pipeline.wait_for_frames()
aligned_frames = align.process(frames)
aligned_depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
if not aligned_depth_frame or not color_frame:
continue
depth_image = np.asanyarray(aligned_depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
cv2.imshow('RGB Image', color_image)
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
cv2.imshow('Depth Image', depth_colormap)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
finally:
pipeline.stop()
cv2.destroyAllWindows()
import pyrealsense2 as rs
ctx = rs.context()
if len(ctx.devices) > 0:
for d in ctx.devices:
print('Found device: ', d.get_info(rs.camera_info.name), ' ', d.get_info(rs.camera_info.serial_number))
else:
print("No Intel Device connected")
3. 坤维六维力
import serial
import time
import struct
COM_PORT = 'COM4'
BAUD_RATE = 460800
PARITY = 'N'
STOP_BITS = 1
DATA_BITS = 8
ser = serial.Serial(
port=COM_PORT,
baudrate=BAUD_RATE,
bytesize=DATA_BITS,
parity=PARITY,
stopbits=STOP_BITS,
timeout=1
)
if not ser.is_open:
ser.open()
def calculate_crc(data):
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_read_request(slave_address, register_address):
request = bytes([slave_address, 0x03, (register_address >> 8) & 0xFF, register_address & 0xFF, 0x00, 0x06])
crc = calculate_crc(request)
request += crc.to_bytes(2, byteorder='little')
ser.write(request)
time.sleep(0.1)
response = ser.read(ser.in_waiting)
return response
def send_write_request(payload, loop=True):
crc = calculate_crc(bytes(payload))
request = bytes(payload) + crc.to_bytes(2, byteorder='little')
ser.write(request)
time.sleep(0.1)
response = bytearray()
if loop:
while ser.in_waiting > 0:
tmp = ser.read(ser.in_waiting)
response += tmp
tmp2 = bytes([0xb0,0x75,0x1c,0xc1])
print(struct.unpack('<f', tmp2)[0])
time.sleep(0.01)
hex_data = ' '.join(f'{b:02x}' for b in tmp2)
hex_data1 = ' '.join(f'{b:02x}' for b in tmp)
print(f'16进制响应数据: {hex_data}')
print(f'16进制响应数据: {hex_data1}')
return bytes(response)
try:
time.sleep(0.01)
send_write_request([0x43, 0xAA, 0x0D, 0x0A], False)
time.sleep(0.01)
response = send_write_request([0x49, 0xAA, 0x0D, 0x0A])
if len(response) >= 0:
print(f"保持寄存器的值：{response}")
else:
print("读取失败")
time.sleep(0.05)
except KeyboardInterrupt:
time.sleep(0.01)
response = send_write_request([0x43, 0xAA, 0x0D, 0x0A], False)
print(f"停止的结果是：{response}")
time.sleep(0.01)
send_write_request([0x43, 0xAA, 0x0D, 0x0A], False)
print(f"停止的结果是：{response}")
time.sleep(0.01)
send_write_request([0x43, 0xAA, 0x0D, 0x0A], False)
print(f"停止的结果是：{response}")
time.sleep(0.01)
send_write_request([0x43, 0xAA, 0x0D, 0x0A], False)
print(f"停止的结果是：{response}")
time.sleep(0.01)
print("\n程序已停止")
ser.close()
ser.close()
4. 帕西尼触觉传感器
import serial
import time
import serial.tools.list_ports
import logging
from typing import List, Optional, Dict, Any
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(levelname)s - %(message)s',
handlers=[
logging.StreamHandler()
]
)
logger = logging.getLogger(__name__)
class SensorCommunication:
"""传感器通信类，封装与力传感器设备的串口通信功能"""
def __init__(self, port: str = None, baudrate: int = 460800, timeout: float = 0.2):
"""
初始化传感器通信类
参数:
port: 串口号，如'COM8'
baudrate: 波特率
timeout: 串口超时时间
"""
self.port = port
self.baudrate = baudrate
self.timeout = timeout
self.ser = None
self.connected = False
self.current_port = None
def list_available_ports(self) -> List[Dict[str, str]]:
"""获取所有可用串口信息"""
ports = []
for port in serial.tools.list_ports.comports():
ports.append({
'device': port.device,
'description': port.description,
'hwid': port.hwid
})
logger.info(f"检测到可用串口: {port.device} - {port.description}")
return ports
def connect(self, port: str = None) -> bool:
"""
连接到指定串口
参数:
port: 串口号，若为None则使用初始化时的port
返回:
连接是否成功
"""
try:
if port:
self.port = port
if not self.port:
logger.error("未指定串口号")
return False
self.ser = serial.Serial(self.port, self.baudrate, timeout=self.timeout)
if self.ser.is_open:
self.connected = True
logger.info(f"成功连接到串口: {self.port}")
return True
else:
logger.error(f"无法打开串口: {self.port}")
return False
except serial.SerialException as e:
logger.error(f"串口连接异常: {str(e)}")
return False
except Exception as e:
logger.error(f"连接过程中发生未知错误: {str(e)}")
return False
def disconnect(self):
"""断开串口连接"""
if self.connected and self.ser and self.ser.is_open:
self.ser.close()
self.connected = False
self.current_port = None
logger.info("已断开串口连接")
def send_hex_data(self, hex_data: str) -> bool:
"""
发送16进制数据到串口
参数:
hex_data: 16进制字符串，如"55 AA 7B 7B"
返回:
发送是否成功
"""
if not self.connected:
logger.error("未连接到串口，无法发送数据")
return False
try:
clean_hex = hex_data.replace(' ', '').upper()
data_bytes = bytes.fromhex(clean_hex)
self.ser.write(data_bytes)
logger.debug(f"已发送16进制数据: {hex_data}")
return True
except Exception as e:
logger.error(f"发送数据时出错: {str(e)}")
return False
def read_serial_response(self, timeout: float = 0.3) -> Optional[bytes]:
"""
从串口读取回复数据
参数:
timeout: 读取超时时间
返回:
读取到的数据字节，若无数据则返回None
"""
if not self.connected:
logger.error("未连接到串口，无法读取数据")
return None
try:
time.sleep(timeout)
response = self.ser.read_all()
if response:
logger.debug(f"收到回复数据: {response.hex(' ')}")
return response
return None
except Exception as e:
logger.error(f"读取数据时出错: {str(e)}")
return None
@staticmethod
def calculate_lrc(data: bytes) -> int:
"""
计算LRC校验码
参数:
data: 要计算校验码的数据字节
返回:
LRC校验码
"""
lrc = 0
for byte in data:
lrc = (lrc + byte) & 0xFF
lrc = ((~lrc) + 1) & 0xFF
logger.debug(f"LRC校验码计算结果: {lrc:02X}")
return lrc
def select_port(self, port_id: int) -> bool:
"""
选择指定的物理端口
参数:
port_id: 端口ID，1或2
返回:
端口选择是否成功
"""
if not self.connected:
logger.error("未连接到串口，无法选择端口")
return False
if port_id not in [1, 2]:
logger.error(f"无效的端口ID: {port_id}，支持1或2")
return False
if self.current_port == port_id:
logger.info(f"已选择端口{port_id}，无需重复选择")
return True
port_commands = {
1: {
"command": "choose_port",
"body": "0E 00 70 B1 0A 01 00 01",
"sleep": 1
},
2: {
"command": "choose_port2",
"body": "0E 00 70 B1 0A 01 00 04",
"sleep": 1
}
}
cmd = port_commands[port_id]
head = "55 AA 7B 7B"
body = cmd["body"]
sleep_time = cmd["sleep"]
data_bytes = bytes.fromhex(body.replace(' ', ''))
lrc = self.calculate_lrc(data_bytes)
tail = "55 AA 7D 7D"
full_hex_data = f"{head} {body} {lrc:02X} {tail}"
logger.info(f"选择端口{port_id}: {full_hex_data}")
if not self.send_hex_data(full_hex_data):
return False
time.sleep(sleep_time)
response = self.read_serial_response()
if response:
if len(response) >= 16 and response[9] == 0x00:
self.current_port = port_id
logger.info(f"成功选择端口{port_id}")
return True
else:
logger.warning(f"选择端口{port_id}可能失败，响应: {response.hex(' ')}")
self.current_port = port_id
logger.info(f"已发送端口{port_id}选择命令，等待确认")
return True
def get_ser_response(self, fun: str, length: int = 0) -> Optional[str]:
"""
向传感器发送命令并获取响应
参数:
fun: 命令功能标识，如"get_version"
length: 数据长度，用于get_data等需要指定长度的命令
返回:
解析后的数据字符串，若失败则返回None
"""
if not self.connected:
logger.error("未连接到串口，无法执行命令")
return None
commands = {
"get_version": {
"body": "0E 00 60 A0 01 00 00",
"sleep": 1,
"parse": "ascii",
"description": "获取版本号"
},
"recalibration": {
"body": "0E 00 70 B0 02 02 00 03 01",
"sleep": 1,
"parse": "text",
"description": "重新校准"
},
"set_mode": {
"body": "0E 00 70 C0 0C 01 00 05",
"sleep": 2,
"parse": "",
"description": "设置模式"
},
"get_mode": {
"body": "0E 00 70 C0 0D 00 00 B5",
"sleep": 1,
"parse": "text",
"description": "获取模式"
},
"get_data": {
"body": "0E 00 70 C0 06 05 00 7B 0E 04",
"sleep": 0.05,
"parse": "hex",
"description": "获取CN1数据"
},
"get_data2": {
"body": "0E 00 70 C0 06 05 00 7B F0 03",
"sleep": 0.05,
"parse": "hex",
"description": "获取CN2数据"
}
}
if fun not in commands:
logger.error(f"不支持的命令: {fun}")
return None
cmd = commands[fun]
head = "55 AA 7B 7B"
body = cmd["body"]
sleep_time = cmd["sleep"]
parse_type = cmd["parse"]
logger.info(f"执行命令: {fun} - {cmd['description']}")
if fun in ["get_data", "get_data2"] and length > 0:
length_bytes = length.to_bytes(2, byteorder='little')
body = f"{body} {length_bytes.hex()}"
logger.debug(f"添加长度参数: {length} 字节, 十六进制: {length_bytes.hex()}")
data_bytes = bytes.fromhex(body.replace(' ', ''))
lrc = self.calculate_lrc(data_bytes)
tail = "55 AA 7D 7D"
full_hex_data = f"{head} {body} {lrc:02X} {tail}"
logger.debug(f"完整发送数据: {full_hex_data}")
if not self.send_hex_data(full_hex_data):
return None
time.sleep(sleep_time)
response = self.read_serial_response()
if not response:
logger.warning("未收到设备响应")
return None
try:
if len(response) >= 16:
if response[0:4] == bytes.fromhex(head.replace(' ', '')) and response[-4:] == bytes.fromhex(tail.replace(' ', '')):
error = response[9]
if error == 0x00:
length_bytes = response[10:12]
data_length = int.from_bytes(length_bytes, byteorder='little')
data = response[12:12+data_length]
if parse_type == "ascii":
result = data.decode('ascii', errors='replace')
logger.info(f"{fun}: {result}")
return result
elif parse_type == "text":
result = data.hex()
logger.debug(f"{fun}: {result}")
return result
elif parse_type == "hex":
result = data.hex()
logger.info(f"0x7b 获取数据: {result}")
return result
else:
logger.info(f"{fun}: 执行成功")
return "success"
else:
logger.error(f"命令执行错误，错误码: {error:02X}")
return None
else:
logger.error("响应数据格式错误，头或尾不匹配")
return None
else:
logger.warning(f"响应数据长度不足，无法解析，实际长度: {len(response)}")
return None
except Exception as e:
logger.error(f"解析响应数据时出错: {str(e)}")
return None
def init_box(self) -> bool:
"""初始化控制盒，执行必要的启动命令"""
if not self.connected:
logger.error("未连接到控制盒，无法初始化")
return False
try:
version = self.get_ser_response("get_version")
if not version:
logger.error("获取版本号失败")
return False
if self.get_ser_response("set_mode") != "success":
logger.error("设置模式失败")
return False
logger.info("控制盒初始化成功")
return True
except Exception as e:
logger.error(f"初始化过程中出错: {str(e)}")
return False
def get_port_data(self, port_id: int, request_length: int, parse_type: str = 'byte') -> Optional[List[int]]:
"""
从指定端口获取传感器数据
参数:
port_id: 端口ID，1或2
request_length: 请求的数据长度（字节）
parse_type: 解析类型，支持 'int16' 等
返回:
解析后的数据列表，若失败则返回None
"""
if not self.connected:
logger.error("未连接到传感器，无法获取数据")
return None
if not self.select_port(port_id):
logger.error(f"选择端口{port_id}失败")
return None
command = "get_data" if port_id == 1 else "get_data2"
logger.info(f"从端口{port_id}获取{request_length}字节数据")
data = self.get_ser_response(command, request_length)
if not data:
logger.error(f"从端口{port_id}获取原始数据失败")
return None
if len(data) < 12:
logger.error(f"端口{port_id}数据长度不足，无法裁切，实际长度: {len(data)}")
return None
valid_data = data[12:]
logger.debug(f"端口{port_id}裁切后数据: {valid_data}")
try:
byte_values = list(bytes.fromhex(valid_data))
logger.info(f"成功从端口{port_id}解析{len(byte_values)}个单字节值")
return byte_values
except ValueError as e:
logger.error(f"端口{port_id}十六进制数据解析失败: {str(e)}, 数据: {valid_data}")
return None
def main():
"""主函数，程序入口"""
logger.info("力传感器数据采集程序启动")
sensor = SensorCommunication()
try:
ports = sensor.list_available_ports()
if not ports:
logger.error("未检测到可用串口，请检查设备连接")
return
chosen_port = ports[0]['device']
logger.info(f"选择串口: {chosen_port}")
if not sensor.connect(chosen_port):
logger.error("串口连接失败，程序退出")
return
if not sensor.init_box():
logger.error("传感器初始化失败，程序退出")
return
try:
while True:
logger.info("读取CN1数据...")
data1 = sensor.get_port_data(1, 127, 'byte')
if data1:
logger.info(f"CN1数据: {data1}...")
time.sleep(1)
logger.info("读取CN2数据...")
data2 = sensor.get_port_data(2, 127, 'byte')
if data2:
logger.info(f"CN2数据: {data2}...")
time.sleep(1)
except KeyboardInterrupt:
logger.info("用户中断程序")
except Exception as e:
logger.error(f"主循环出错: {str(e)}")
finally:
sensor.disconnect()
logger.info("程序结束")
if __name__ == "__main__":
main()
5. elite 六轴
import socket
import json
import time
def connectETController(ip, port=8055):
"""
连接ET控制器
:param ip: 控制器的IP地址
:param port: 控制器的端口号，默认为8055
:return: 连接成功返回(True, socket对象)，连接失败返回(False,)
"""
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
sock.connect((ip, port))
return (True, sock)
except Exception as e:
sock.close()
return (False,)
def disconnectETController(sock):
"""
断开与ET控制器的连接
:param sock: socket对象
"""
if sock:
sock.close()
sock = None
else:
sock = None
def sendCMD(sock, cmd, params=None, id=1):
"""
向ET控制器发送命令
:param sock: socket对象
:param cmd: 命令名称
:param params: 命令参数，默认为None
:param id: 请求的ID，默认为1
:return: 返回命令执行结果，包括是否成功、结果数据和请求ID
"""
if not params:
params = []
else:
params = json.dumps(params)
sendStr = '{{"method":"{0}","params":{1},"jsonrpc":"2.0","id":{2}}}'.format(cmd, params, id) + "\n"
try:
sock.sendall(bytes(sendStr, "utf-8"))
ret = sock.recv(1024)
jdata = json.loads(str(ret, "utf-8"))
print("kkkk",jdata)
if "result" in jdata.keys():
return (True, json.loads(jdata["result"]), jdata["id"])
elif "error" in jdata.keys():
return (False, jdata["error"], jdata["id"])
else:
return (False, None, None)
except Exception as e:
print(e)
return (False, None, None)
if __name__ == "__main__":
robot_ip = "192.168.1.200"
conSuc, sock = connectETController(robot_ip, 8055)
jbi_filename = "t1"
print("conSuc", conSuc)
if conSuc:
suc, result , id = sendCMD(sock, "getServoStatus")
print (suc, result , id)
time. sleep (0.5)
if result == 0:
ret, result, id=sendCMD(sock, "set_servo_status", {"status": 1})
print ( result )
time. sleep (1)
suc, result , id=sendCMD(sock, "calibrate_encoder_zero_position")
print ("1---1", suc, result , id)
suc, result, id = sendCMD(sock, "checkJbiExist", {"filename": jbi_filename })
print("2---2",suc,result,id)
time. sleep (10)
if (suc and result ==1):
suc1, result1 , id=sendCMD(sock,"setSpeed",{"value": 50 })
if (suc1 and result1 ==1):
suc2, result2 , id=sendCMD(sock,"runJbi",{"filename": jbi_filename })
disconnectETController(sock)
else:
print("连接机器人失败")
扫一扫
5325
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
