python3 EtherCAT pysoem库 sdo电机控制示例
·
python3 EtherCAT pysoem库 sdo电机控制示例
1.ubuntu 普通用户:
sudo setcap 'cap_net_raw,cap_net_admin+ep' /usr/bin/python3.10
2.回零模式、位置模式 示例代码:
import pysoem
import ctypes
import time
import logging
logging.basicConfig(
level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s"
)
class EtherCATController:
"""EtherCAT控制器类,封装了与从站设备的交互"""
class HomingConfig:
"""CiA402 Homing模式参数容器"""
def __init__(self):
self.homing_method = 18
self.high_speed = 5000
self.low_speed = 500
self.accel = 5000
self.home_offset = 0
self.lead_pitch = 10
self.encoder_res = 10000
def mm_to_pulses(self, mm):
return int(mm * self.encoder_res / self.lead_pitch)
class PPConfig:
"""PP模式参数容器(需结合丝杆导程参数)"""
def __init__(self, lead=10):
self.mode = 1
self.target_pos = 0
self.profile_vel = 5000
self.profile_acc = 20000
self.profile_dec = 20000
self.lead = lead
self.pulses_per_rev = 10000
@property
def mm_to_pulses(self):
return int(self.target_pos * self.pulses_per_rev / self.lead)
def __init__(self, interface):
self.master = pysoem.Master()
self.interface = interface
self.slaves = []
def __enter__(self):
self.master.open(self.interface)
if self.master.config_init() > 0:
self.slaves = self.master.slaves
for slave in self.slaves:
logging.info(f"发现从站设备:{slave.name}")
self.master.state = pysoem.OP_STATE
self.master.write_state()
return self
raise RuntimeError("未检测到从站设备")
def __exit__(self, exc_type, exc_val, exc_tb):
self.master.close()
logging.info("EtherCAT主站已关闭")
def _sdo_operation(self, slave, index, subindex, data=None):
"""封装SDO读写操作"""
try:
if data is not None:
slave.sdo_write(index, subindex, data)
else:
return slave.sdo_read(index, subindex)
except pysoem.SdoError as e:
action = "写入" if data is not None else "读取"
logging.error(
f"SDO{action}失败(从站:{slave.name},索引:{index},子索引:{subindex}):{e}"
)
raise
def set_hm_mode(self, slave, config):
"""设置回零模式参数"""
self._sdo_operation(slave, 0x6060, 0, bytes(ctypes.c_int8(6)))
self._sdo_operation(
slave, 0x6098, 0, bytes(ctypes.c_int8(config.homing_method))
)
self._sdo_operation(slave, 0x6099, 1, bytes(ctypes.c_uint32(config.high_speed)))
self._sdo_operation(slave, 0x6099, 2, bytes(ctypes.c_uint32(config.low_speed)))
self._sdo_operation(slave, 0x609A, 0, bytes(ctypes.c_uint32(config.accel)))
self._sdo_operation(slave, 0x607C, 0, bytes(ctypes.c_int32(config.home_offset)))
def trigger_homing_sequence(self, slave):
"""触发回零操作"""
self._sdo_operation(slave, 0x6040, 0, bytes(ctypes.c_uint16(0x000F)))
time.sleep(0.1)
self._sdo_operation(slave, 0x6040, 0, bytes(ctypes.c_uint16(0x001F)))
def is_limit_triggered(self, slave, homing_method):
"""检查限位开关状态"""
status_word = int.from_bytes(self._sdo_operation(slave, 0x60FD, 0), "little")
positive_limit = (status_word >> 1) & 0x1
negative_limit = status_word & 0x1
logging.info(
f"从站 {slave.name} 状态字: {status_word}, 正限位: {positive_limit}, 负限位: {negative_limit}"
)
return (positive_limit == 1) if homing_method == 1 else (negative_limit == 1)
def check_and_reset_errors(self, slave):
"""错误状态检查与复位"""
status_word = int.from_bytes(self._sdo_operation(slave, 0x6041, 0), "little")
if status_word & (1 << 3): # 检查错误位
error_code = int.from_bytes(self._sdo_operation(slave, 0x603F, 0), "little")
logging.warning(
f"从站 {slave.name} 检测到设备错误 0x{error_code:04X},尝试复位..."
)
for cmd in [0x0080, 0x0006, 0x000F]:
self._sdo_operation(slave, 0x6040, 0, bytes(ctypes.c_uint16(cmd)))
time.sleep(0.1)
def get_actual_position(self, slave):
"""通过SDO读取实际位置(0x6064)"""
data = self._sdo_operation(slave, 0x6064, 0)
return ctypes.c_int32.from_buffer_copy(data).value
def get_target_position(self, slave):
"""通过SDO读取目标位置(0x607A)"""
data = self._sdo_operation(slave, 0x607A, 0)
return ctypes.c_int32.from_buffer_copy(data).value
def monitor_homing_status(self, slave, timeout=30.0):
"""监控回零状态"""
start_time = time.time()
while time.time() - start_time < timeout:
status_word = int.from_bytes(
self._sdo_operation(slave, 0x6041, 0), "little"
)
home_status = (status_word >> 12) & 0x1
actual_pos = self.get_actual_position(slave)
target_pos = self.get_target_position(slave)
logging.info(
f"从站 {slave.name} 实际位置: {actual_pos}, 目标位置: {target_pos}"
)
if home_status:
logging.info(f"从站 {slave.name} 回零成功,状态字: {status_word}")
return True
if status_word & (1 << 13):
error_code = int.from_bytes(
self._sdo_operation(slave, 0x603F, 0), "little"
)
raise RuntimeError(f"从站 {slave.name} 回零错误 0x{error_code:04X}")
time.sleep(0.1)
raise TimeoutError(f"从站 {slave.name} 回零操作超时")
def set_pp_mode(self, slave, config):
"""设置PP模式参数"""
self._sdo_operation(slave, 0x6060, 0, bytes(ctypes.c_int8(config.mode)))
self._sdo_operation(
slave, 0x6081, 0, bytes(ctypes.c_uint32(config.profile_vel))
)
self._sdo_operation(
slave, 0x6083, 0, bytes(ctypes.c_uint32(config.profile_acc))
)
self._sdo_operation(
slave, 0x6084, 0, bytes(ctypes.c_uint32(config.profile_dec))
)
target_pulses = config.mm_to_pulses
self._sdo_operation(slave, 0x607A, 0, bytes(ctypes.c_int32(target_pulses)))
def trigger_pp_movement(self, slave):
"""触发PP运动"""
self._sdo_operation(slave, 0x6040, 0, bytes(ctypes.c_uint16(0x000F)))
self._sdo_operation(slave, 0x6040, 0, bytes(ctypes.c_uint16(0x001F)))
time.sleep(0.1)
self._sdo_operation(slave, 0x6040, 0, bytes(ctypes.c_uint16(0x003F)))
def monitor_movement(self, slave, timeout=30.0):
"""通用运动状态监控"""
start_time = time.time()
while time.time() - start_time < timeout:
status_word = int.from_bytes(
self._sdo_operation(slave, 0x6041, 0), "little"
)
actual_pos = self.get_actual_position(slave)
target_pos = self.get_target_position(slave)
logging.info(
f"从站 {slave.name} 实际位置: {actual_pos} pulses | 目标位置: {target_pos} pulses"
)
if status_word & (1 << 10):
logging.info(f"从站 {slave.name} 位置到达确认")
return True
if status_word & (1 << 3):
error_code = int.from_bytes(
self._sdo_operation(slave, 0x603F, 0), "little"
)
raise RuntimeError(f"从站 {slave.name} 运动错误 0x{error_code:04X}")
time.sleep(0.1)
raise TimeoutError(f"从站 {slave.name} 运动超时")
def main(self):
for slave in self.slaves:
config = self.HomingConfig()
self.set_hm_mode(slave, config)
max_retries = 3
for attempt in range(1, max_retries + 1):
try:
logging.info(f"开始从站 {slave.name} 第 {attempt} 次回零尝试")
self.check_and_reset_errors(slave)
self.trigger_homing_sequence(slave)
self.monitor_homing_status(slave)
logging.info(f"从站 {slave.name} 回零成功完成")
pp_config = self.PPConfig(lead=10)
pp_config.target_pos = -205 # 设置目标位置(单位:毫米)
pp_config.profile_vel = 60000 # 设置速度
self.set_pp_mode(slave, pp_config)
self.trigger_pp_movement(slave)
self.monitor_movement(slave) # 监控运动状态
break
except TimeoutError:
if self.is_limit_triggered(slave, config.homing_method):
logging.error(
f"从站 {slave.name} 超时但限位已触发,可能存在机械问题"
)
break
logging.warning(f"从站 {slave.name} 超时且限位未触发,准备重试...")
if attempt < max_retries:
time.sleep(1)
else:
logging.error(f"从站 {slave.name} 达到最大重试次数,终止回零")
raise
except Exception as e:
logging.error(f"从站 {slave.name} 回零失败:{str(e)}")
raise
if __name__ == "__main__":
interface_name = "eth0" # 替换为实际网卡名称
with EtherCATController(interface_name) as controller:
controller.main()
链接: https://github.com/bnjmnp/pysoem/tree/master
链接: https://pysoem.readthedocs.io/en/latest
魔乐社区(Modelers.cn) 是一个中立、公益的人工智能社区,提供人工智能工具、模型、数据的托管、展示与应用协同服务,为人工智能开发及爱好者搭建开放的学习交流平台。社区通过理事会方式运作,由全产业链共同建设、共同运营、共同享有,推动国产AI生态繁荣发展。
更多推荐


所有评论(0)