Motion control about pysoem HOT 1 OPEN

from typing import Optional
from time import sleep, time
import pysoem
import ctypes
import struct


class PDOInput(ctypes.Structure):
    """Process data object from the slave to the master.

    Byte and bit layouts are copied from TwinCAT project.
    """
    _pack_ = 1
    _fields_ = [
        # 0x1A00 (6 bytes):
        ("", ctypes.c_uint16, 1),  # bit0
        ("latch_extern_valid", ctypes.c_uint16, 1),  # bit1
        ("set_counter_done", ctypes.c_uint16, 1),  # bit2
        ("counter_underflow", ctypes.c_uint16, 1),  # bit3
        ("counter_overflow", ctypes.c_uint16, 1),  # bit4
        ("", ctypes.c_uint16, 3 + 4),  # bits 5-7 and 0-3
        ("status_extern_latch", ctypes.c_uint16, 1),  # bit4
        ("sync_error", ctypes.c_uint16, 1),  # bit5
        ("", ctypes.c_uint16, 1),  # bit6
        ("tx_pdo_toggle", ctypes.c_uint16, 1),  # bit7
        ("counter_value", ctypes.c_uint16),
        ("latch_value", ctypes.c_uint16),

        # 0x1A03 (2 bytes):
        ("ready_to_enable", ctypes.c_uint16, 1),  # bit0
        ("ready", ctypes.c_uint16, 1),  # bit1
        ("warning", ctypes.c_uint16, 1),  # bit2
        ("error", ctypes.c_uint16, 1),  # bit3
        ("moving_positive", ctypes.c_uint16, 1),  # bit4
        ("moving_negative", ctypes.c_uint16, 1),  # bit5
        ("torque_reduced", ctypes.c_uint16, 1),  # bit6
        ("", ctypes.c_uint16, 1 + 3),  # bits 7 and 0-2
        ("digital_input1", ctypes.c_uint16, 1),  # bit3
        ("digital_input2", ctypes.c_uint16, 1),  # bit4
        ("sync_error_stm", ctypes.c_uint16, 1),  # bit5
        ("", ctypes.c_uint16, 1),  # bit7
        ("tx_pdo_toggle_stm", ctypes.c_uint16, 1),  # bit7
    ]


class PDOOutput(ctypes.Structure):
    """Process data object from the master to the slave.

    Byte and bit layouts are copied from TwinCAT project.
    """
    _pack_ = 1
    _fields_ = [
        # 0x1600 (4 bytes):
        ("", ctypes.c_uint16, 1),  # bit0
        ("latch_enable_positive", ctypes.c_uint16, 1),  # bit1
        ("set_counter", ctypes.c_uint16, 1),  # bit2
        ("latch_enable_negative", ctypes.c_uint16, 1),  # bit3
        ("set_counter_value", ctypes.c_uint16),

        # 0x1602 (2 bytes):
        ("enable", ctypes.c_uint16, 1),  # bit0
        ("reset", ctypes.c_uint16, 1),  # bit1
        ("reduce_torque", ctypes.c_uint16, 1),  # bit2

        # 0x1604 (2 bytes):
        ("velocity", ctypes.c_int16),
    ]


class MotorNode:

    def __init__(self):
        self.adapter_name = "\\Device\\NPF_{2D594793-2E69-4C58-90F4-3164E860643B}"

        self._master: Optional[pysoem.Master] = pysoem.Master()
        self._slave_motor = None

        self.start_time = 0
        self.velocity = 0.0  # In degrees per second

        self._output_pdo = PDOOutput()
        self._output_pdo.reduce_torque = 1  # Always use low-torque mode

        self._input_pdo: Optional[PDOInput] = None

    def __del__(self):
        self.close()

    def run(self):
        try:
            self.setup()
            while True:
                self.loop()
        finally:
            self.close()

    def on_config_el7031(self, _slave_idx):
        # Process data (inputs and outputs) for velocity control:
        map_pdo_rx = struct.pack("<HHHH", 3, 0x1600, 0x1602, 0x1604)
        map_pdo_tx = struct.pack("<HHH", 2, 0x1a00, 0x1a03)
        self._slave_motor.sdo_write(0x1c12, 0x00, map_pdo_rx, ca=True)  # Master output
        self._slave_motor.sdo_write(0x1c13, 0x00, map_pdo_tx, ca=True)  # Master input

    def setup(self):
        self._master = pysoem.Master()
        self._master.open(self.adapter_name)

        dev_count = self._master.config_init()
        if dev_count <= 0:
            raise RuntimeError("Could not find any EtherCAT devices")

        for slave in self._master.slaves:
            if "EL7031" in slave.name:
                self._slave_motor = slave

        if self._slave_motor:
            self._slave_motor.config_func = self.on_config_el7031

        # Move from PREOP to SAFEOP state - each slave's config_func is called:
        self._master.config_map()
        self._assert_state(pysoem.SAFEOP_STATE)

        self._master.state = pysoem.OP_STATE
        self._master.write_state()
        self._assert_state(pysoem.OP_STATE, 1_000_000)

        print("All devices in 'OP' state")

        # Trigger first update to initialize:
        self._master.send_processdata()
        self._master.receive_processdata()

        self.start_time = time()

    def loop(self):
        self._master.receive_processdata()
        self._input_pdo = PDOInput.from_buffer_copy(self._slave_motor.input)

        print(
            "TxPDO:", self._input_pdo.tx_pdo_toggle,
            "\tReady:", self._input_pdo.ready,
            "\tCounter:", self._input_pdo.counter_value,
        )

        self._output_pdo.enable = 1

        if time() - self.start_time > 1.0:
            self.start_time = time()

            # Toggle speed high and low
            self.velocity = 0.0 if self.velocity > 0.0 else 90.0

        self._output_pdo.velocity = self.velocity_setpoint(self.velocity)
        self._slave_motor.output = bytes(self._output_pdo)
        self._master.send_processdata()

        sleep(0.01)

    def close(self):
        if self._master:
            self._master.close()
            self._master = None

    def _assert_state(self, state, timeout: Optional[int] = None):
        """Check master state and give verbose error if state is not met."""
        args = (state,)
        if timeout is not None:
            args += (timeout,)
        if self._master.state_check(*args) != state:
            self._master.read_state()
            msg = ""
            for slave in self._master.slaves:
                if slave.state != state:
                    msg += slave.name + " - " + hex(slave.state) + ","
                    desc = pysoem.al_status_code_to_string(slave.al_status)
                    print(f"{slave.name} al status: {hex(slave.al_status)} ({desc})")

            raise RuntimeError(f"Not all slaves reached state {hex(state)}: {msg}")

    @staticmethod
    def velocity_setpoint(velocity: float) -> int:
        """Get PDO velocity setpoint from velocity in deg/sec."""
        # For a motor with 200 steps and configured at 1000 fullsteps/sec
        # The factor of 2 cannot be explained, but the results check out
        return int(round((2**16 - 1) / 1000.0 * 200.0 / 360.0 / 2.0 * velocity))


def main():
    node = MotorNode()
    try:
        node.run()
    except KeyboardInterrupt:
        pass  # Ignore


if __name__ == "__main__":
    main()