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

Logo

腾讯云面向开发者汇聚海量精品云计算使用和开发经验,营造开放的云计算技术生态圈。

更多推荐