Control Motor Drive using pySOEM

[jmartinhoj]

Hello, I am trying to control my motor with a Somanet Node 1000 drive using pySOEM. I have already tried both with SDO and PDO. With SDO I managed to make it rotate but it rarely worked (with no pattern I could realize). Now, I am trying to implement a PDO solution, but it is not working. I have used this comment as a code base. Here is my code:

import pysoem
import time
import ctypes

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),
    ]

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),
    ]

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('enp1s0')  # someting like '\\Device\\NPF_{B4B7A38F-7DEB-43AF-B787B7-EABADE43978EA}' under Windows
    if master.config_init() > 0:
        synapt = master.slaves[0]
        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 = 100  # RPM
                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)
                    time.sleep(0.01)
                try:
                    while 1:
                        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()

and here is the Rx and Tx sections of the ESI file:

<RxPdo Sm="2">
  <Index>#x1600</Index>
  <Name>RxPDO Mapping 1</Name>
  <Entry>
    <Index>#x6040</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Controlword</Name>
    <DataType>UINT</DataType>
  </Entry>
  <Entry>
    <Index>#x6060</Index>
    <SubIndex>0</SubIndex>
    <BitLen>8</BitLen>
    <Name>Modes of operation</Name>
    <DataType>SINT</DataType>
  </Entry>
  <Entry>
    <Index>#x6071</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Target Torque</Name>
    <DataType>INT</DataType>
  </Entry>
  <Entry>
    <Index>#x607A</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Target position</Name>
    <DataType>DINT</DataType>
  </Entry>
  <Entry>
    <Index>#x60FF</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Target velocity</Name>
    <DataType>DINT</DataType>
  </Entry>
  <Entry>
    <Index>#x60B2</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Torque offset</Name>
    <DataType>INT</DataType>
  </Entry>
  <Entry>
    <Index>#x2701</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Tuning command</Name>
    <DataType>UDINT</DataType>
  </Entry>
</RxPdo>
<RxPdo Sm="2">
  <Index>#x1601</Index>
  <Name>RxPDO Mapping 2</Name>
  <Entry>
    <Index>#x60FE</Index>
    <SubIndex>1</SubIndex>
    <BitLen>32</BitLen>
    <Name>Physical outputs</Name>
    <DataType>UDINT</DataType>
  </Entry>
  <Entry>
    <Index>#x60FE</Index>
    <SubIndex>2</SubIndex>
    <BitLen>32</BitLen>
    <Name>Bit mask</Name>
    <DataType>UDINT</DataType>
  </Entry>
</RxPdo>
<RxPdo Sm="2">
  <Index>#x1602</Index>
  <Name>RxPDO Mapping 3</Name>
  <Entry>
    <Index>#x2703</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>User MOSI</Name>
    <DataType>UDINT</DataType>
  </Entry>
  <Entry>
    <Index>#x60B1</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Velocity offset</Name>
    <DataType>DINT</DataType>
  </Entry>
</RxPdo>

<TxPdo Sm="3">
  <Index>#x1a00</Index>
  <Name>TxPDO Mapping 1</Name>
  <Entry>
    <Index>#x6041</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Statusword</Name>
    <DataType>UINT</DataType>
  </Entry>
  <Entry>
    <Index>#x6061</Index>
    <SubIndex>0</SubIndex>
    <BitLen>8</BitLen>
    <Name>Modes of operation display</Name>
    <DataType>SINT</DataType>
  </Entry>
  <Entry>
    <Index>#x6064</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Position actual value</Name>
    <DataType>DINT</DataType>
  </Entry>
  <Entry>
    <Index>#x606C</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Velocity actual value</Name>
    <DataType>DINT</DataType>
  </Entry>
  <Entry>
    <Index>#x6077</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Torque actual value</Name>
    <DataType>INT</DataType>
  </Entry>
</TxPdo>
<TxPdo Sm="3">
  <Index>#x1a01</Index>
  <Name>TxPDO Mapping 2</Name>
  <Entry>
    <Index>#x2401</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Analog input 1</Name>
    <DataType>UINT</DataType>
  </Entry>
  <Entry>
    <Index>#x2402</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Analog input 2</Name>
    <DataType>UINT</DataType>
  </Entry>
  <Entry>
    <Index>#x2403</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Analog input 3</Name>
    <DataType>UINT</DataType>
  </Entry>
  <Entry>
    <Index>#x2404</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Analog input 4</Name>
    <DataType>UINT</DataType>
  </Entry>
  <Entry>
    <Index>#x2702</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Tuning status</Name>
    <DataType>UDINT</DataType>
  </Entry>
</TxPdo>
<TxPdo Sm="3">
  <Index>#x1a02</Index>
  <Name>TxPDO Mapping 3</Name>
  <Entry>
    <Index>#x60FD</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Digital inputs</Name>
    <DataType>UDINT</DataType>
  </Entry>
</TxPdo>
<TxPdo Sm="3">
  <Index>#x1a03</Index>
  <Name>TxPDO Mapping 4</Name>
  <Entry>
    <Index>#x2704</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>User MISO</Name>
    <DataType>UDINT</DataType>
  </Entry>
  <Entry>
    <Index>#x20F0</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Timestamp</Name>
    <DataType>UDINT</DataType>
  </Entry>
  <Entry>
    <Index>#x60FC</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Position demand internal value</Name>
    <DataType>DINT</DataType>
  </Entry>
  <Entry>
    <Index>#x606B</Index>
    <SubIndex>0</SubIndex>
    <BitLen>32</BitLen>
    <Name>Velocity demand value</Name>
    <DataType>DINT</DataType>
  </Entry>
  <Entry>
    <Index>#x6074</Index>
    <SubIndex>0</SubIndex>
    <BitLen>16</BitLen>
    <Name>Torque demand</Name>
    <DataType>INT</DataType>
  </Entry>
</TxPdo>

My main mistake could be the fact that I do not specify the PDO mapping that I want to use (as you can see by this XML, there are several ones), because I don't know how to do it.

Do you have any suggestion?

Thanks in advance.

[bnjmnp]

Hi.

I think by default, all RxPdos and TxPdos are selected.

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),
    ]

.. not sure.

If you are able to control the drive via SDO, does it help to not call send_processdata() and receive_processdata() to make it more stable?

[bnjmnp]

Hi @jmartinhoj, can we actually close this issue?