CiA402 EtherCAT Motor Drive configuration
The generic plugin EcCiA402Drive
is a particular case of the GenericEcSlave
enabling features compliant with the CiA402 (“CANopen device profile for drives and motion control”) norm. Therefore, the configuration of such a drive uses the same formalism for the slave_config
file as in the case of the GenericEcSlave
.
Plugin features
Drive State transitions: Management of the motor drive states and their transitions.
Drive Fault reset: Management of the motor drive fault reset using
command_interface
“reset_fault”.Mode of Operation: Management of multiple cyclic modes of operation : position (8), velocity (9), effort (10) and homing (6) with the possibility of switch between them.
Default position: Management of the target position when not controlled.
Configuration options
In addition to the configuration options given by the GenericEcSlave
, the EcCiA402Drive
allows to configure the following options in the slave_config
file:
Configuration flag |
Description |
---|---|
|
if set to |
Behavior
Here are some remarks about the implemented motor drive behavior logic.
After launching the well-configured drive, by default and without fault, motor drive module is brought automatically into the state OPERATION_ENABLED
making it ready for use. Automatic transition is only enabled when the control_word
command interface is either missing or set to NaN
making it possible for the user to take control over the motor drive’s state machine by sending corresponding state transition values using the control_word
command interface.
The default mode of operation of the motor drive can be set either in the configuration yaml file as the default value of the corresponding PDO channel or in urdf using the mode_of_operation
parameter of the the EcCiA402Drive
. If both are set, the urdf parameter value overrides the default one from the configuration yaml file.
In order to prevent unwanted movements of the motor, if uncontrolled, the default target position that is send to the drive in all modes of operation is always the last read position. That is why, it is important to send NaN
in the position command interface when not controlling the motor position. This applies especially for cases when switching between modes.
Usage
Example configuration for the Maxon EPOS3 motor dive:
1# Configuration file for Maxon EPOS3 drive
2vendor_id: 0x000000fb
3product_id: 0x64400000
4assign_activate: 0x0300 # DC Synch register
5auto_fault_reset: false # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
6sdo: # sdo data to be transferred at drive startup
7 - {index: 0x60C2, sub_index: 1, type: int8, value: 10} # Set interpolation time for cyclic modes to 10 ms
8 - {index: 0x60C2, sub_index: 2, type: int8, value: -3} # Set base 10-3s
9rpdo: # RxPDO = receive PDO Mapping
10 - index: 0x1603
11 channels:
12 - {index: 0x6040, sub_index: 0, type: uint16, default: 0} # Control word
13 - {
14 index: 0x607a,
15 sub_index: 0,
16 type: int32,
17 command_interface: position,
18 default: .nan} # Target position
19 - {index: 0x60ff, sub_index: 0, type: int32, default: 0} # Target velocity
20 - {index: 0x6071, sub_index: 0, type: int16, default: 0} # Target torque
21 - {index: 0x60b0, sub_index: 0, type: int32, default: 0} # Offset position
22 - {index: 0x60b1, sub_index: 0, type: int32, default: 0} # Offset velocity
23 - {index: 0x60b2, sub_index: 0, type: int16, default: 0} # Offset torque
24 - {index: 0x6060, sub_index: 0, type: int8, default: 8} # Mode of operation
25 - {index: 0x2078, sub_index: 1, type: uint16, default: 0} # Digital Output Functionalities
26 - {index: 0x60b8, sub_index: 0, type: uint16, default: 0} # Touch Probe Function
27tpdo: # TxPDO = transmit PDO Mapping
28 - index: 0x1a03
29 channels:
30 - {index: 0x6041, sub_index: 0, type: uint16} # Status word
31 - {
32 index: 0x6064,
33 sub_index: 0,
34 type: int32,
35 state_interface: position
36 } # Position actual value
37 - {
38 index: 0x606c,
39 sub_index: 0,
40 type: int32,
41 state_interface: velocity
42 } # Velocity actual value
43 - {
44 index: 0x6077,
45 sub_index: 0,
46 type: int16,
47 state_interface: effort
48 } # Torque actual value
49 - {index: 0x6061, sub_index: 0, type: int8} # Mode of operation display
50 - {index: 0x2071, sub_index: 1, type: int16} # Digital Input Functionalities State
51 - {index: 0x60b9, sub_index: 0, type: int16} # Touch Probe Status
52 - {index: 0x60ba, sub_index: 0, type: int32} # Touch Probe Position 1 Positive Value
53 - {index: 0x60bb, sub_index: 0, type: int32} # Touch Probe Position 1 Negative Value
This configuration can be used for controlling a joint
component. Here is an example urdf for ros2_control
using this configuration together with the EcCiA402Drive
plugin:
<ros2_control name="ec_single_axis" type="system">
<hardware>
<plugin>ethercat_driver/EthercatDriver</plugin>
<param name="master_id">0</param>
<param name="control_frequency">100</param>
</hardware>
<joint name="joint_0">
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
<command_interface name="position"/>
<command_interface name="reset_fault"/>
<ec_module name="MAXON">
<plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
<param name="alias">0</param>
<param name="position">0</param>
<param name="mode_of_operation">8</param>
<param name="slave_config">/path/to/maxon.yaml</param>
</ec_module>
</joint>
</ros2_control>