Motion control #135
Comments
|
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:
Note that the PDO content (and therefore the Codefrom 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() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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 ?
Output of above code :
The text was updated successfully, but these errors were encountered: