Motion control

[developermastermind07]

I am trying to control motor via synaptic drive (node 1000).

I am trying to run following code but motor did,t move . Can you help me ?

import pysoem
import time
import ctypes
from customSlave import customSlave

synapt = None

class InputPdo(ctypes.Structure):
    _pack_ = 1
    _fields_ = [
        ('statusword', ctypes.c_uint16),
        ('modes_of_operation_display', ctypes.c_int8),
        ('position_actual_value', ctypes.c_int32),
        ('velocity_actual_value', ctypes.c_int32),
        ('torque_actual_value', ctypes.c_int32),
        ('analog_input_1', ctypes.c_uint16),
        ('analog_input_2', ctypes.c_uint16),
        ('analog_input_3', ctypes.c_uint16),
        ('analog_input_4', ctypes.c_uint16),
        ('tuning_status', ctypes.c_uint32),
        ('digital_inputs', ctypes.c_uint32),
        ('user_miso', ctypes.c_uint32),
        ('timestamp', ctypes.c_uint32),
        ('position_demand_internal_value', ctypes.c_int32),
        ('velocity_demand_value', ctypes.c_int32),
        ('torque_demand', ctypes.c_int16),
    ]

class OutputPdo(ctypes.Structure):
    _pack_ = 1
    _fields_ = [
        ('controlword', ctypes.c_uint16),
        ('modes_of_operation', ctypes.c_int8),
        ('target_torque', ctypes.c_int32),
        ('target_position', ctypes.c_int32),
        ('target_velocity', ctypes.c_int32),
        ('torque_offset', ctypes.c_int16),
        ('tuning_command', ctypes.c_uint32),
        ('physical_outputs', ctypes.c_uint32),
        ('bit_mask', ctypes.c_uint32),
        ('user_mosi', ctypes.c_uint32),
        ('velocity_offset', ctypes.c_int32),
    ]

modes_of_operation = {
    'No mode': 0,
    'Profile position mode': 1,
    'Profile velocity mode': 3,
    'Homing mode': 6,
    'Cyclic synchronous position mode': 8,
    'Cyclic synchronous velocity mode': 9,
    'Cyclic synchronous torque mode': 10,
}

def convert_input_data(data):
    return InputPdo.from_buffer_copy(data)

def main():
    global synapt
    master = pysoem.Master()
    master.open("\\Device\\NPF_{D167ADA2-C6E1-41BF-BF07-E54DEA36CB1C}")
    #master.open('enp1s0')  # someting like '\\Device\\NPF_{B4B7A38F-7DEB-43AF-B787B7-EABADE43978EA}' under Windows
    if master.config_init() > 0:
        synapt = master.slaves[1]
        slave1 = customSlave(synapt)
        master.config_map()
        if master.state_check(pysoem.SAFEOP_STATE, 50_000) == pysoem.SAFEOP_STATE:
            master.state = pysoem.OP_STATE
            master.write_state()
            master.state_check(pysoem.OP_STATE, 5_000_000)
            if master.state == pysoem.OP_STATE:
                output_data = OutputPdo()
                output_data.modes_of_operation = modes_of_operation['Cyclic synchronous velocity mode']
                output_data.target_velocity = 20  # RPM
                #slave1.writeSDO(0x6060,0,9)
                print(hex(slave1.readSDO(0x6060,0)))
                print(hex(slave1.readSDO(0x6061,0)))
                slave1.writeSDO(0x60ff,0,20)
                time.sleep(0.01)
                print(hex(slave1.readSDO(0x60ff,0)))
                for control_cmd in [6, 7, 15]:
                    output_data.controlword = control_cmd
                    synapt.output = bytes(output_data)  # that is the actual change of the PDO output data
                    master.send_processdata()
                    master.receive_processdata(1_000)
                    #print(hex(slave1.readSDO(0x6040,0)))
                    #slave1.writeSDO(0x6040,0,control_cmd)
                    time.sleep(0.01)
                    print(hex(slave1.readSDO(0x6060,0)))
                    print(hex(slave1.readSDO(0x60ff,0)))
                    print(hex(slave1.readSDO(0x6040,0)))
                    print(hex(slave1.readSDO(0x6041,0)))
                try:
                    while 1:
                        #print(hex(slave1.readSDO(0x6060,0)))
                        #print(hex(slave1.readSDO(0x60ff,0)))
                        #print(hex(slave1.readSDO(0x6040,0)))
                        #print(hex(slave1.readSDO(0x6041,0)))
                        master.send_processdata()
                        master.receive_processdata(1_000)
                        time.sleep(0.01)
                except KeyboardInterrupt:
                    print('stopped')
                # zero everything
                synapt.output = bytes(len(synapt.output))
                master.send_processdata()
                master.receive_processdata(1_000)
            else:
                print('failed to got to op state')
        else:
            print('failed to got to safeop state')
        master.state = pysoem.PREOP_STATE
        master.write_state()
    else:
        print('no device found')
    master.close()

if __name__ == '__main__':
    main()

Output of above code :

OD Found
SOMANETCreating OD
SOMANETOD Created
0x0
0x0
SDO (24831,0) wrote 20 successfuly
0x14
0x9
0x140000
0x6
0x1031
0x9
0x140000
0x7
0x1033
0x9
0x140000
0xf
0x1037

[RobertoRoos]

Somewhat relevant, but just now I finally managed to control a stepper motor using the Beckhoff EL7031-0030 module.

The full code is below.

In a nutshell:

1. Connect to your ethercat master
2. Setup the chain and set all slaves in 'OP' mode
3. Configure the drive, selecting the registers to include the in- and output PDOs
4. Every loop, receive and transmit the PDO and unpack / pack them according to a ctypes.Structure

Note that the PDO content (and therefore the Structure structs that we make) are dependent on the exact PDOs selected during config.

Code

```python 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(" 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() ```