CiA 402 Motion Control Example For MAXPOS Drive - Simple Robot

9/27/23, 2:32 PM CiA 402 Motion control example for MAXPOS drive | Simple Robot
Simple Robot ...build yourself robot simply with… search
Sidebar Home RASPBERRY-PI1&0 RASPBERRY-PI2&3 RPI-EtherCAT FACEBOOK
PiCAT4 - The real… 28

CiA 402 Motion control example for MAXPOS drive
Teardown and Recovery…
Github link https://github.com/thanhtam-h/soem-w5500-rpi
[https://github.com/thanhtam-h/soem-w5500-rpi]
PiCAT4 - Realtim… 14
24
Motion control may be the most interested application of EtherCAT. In
PiCAT Xenomai 3…
this post I will explain how to use PiCAT to control a standard CoE
DS402 motor driver
Xenomai for Ras… 67
Raspberry pi 4 su… 17
CiA 402 Motion c… 29
PiCAT schematic… 4
Distributed Clock … 43
EtherCAT Master … 39
2-DOF Robot con… 14
Raspberry Pi Eth… 57
EtherCAT
Raspberry pi 2,3 … 27
Raspberry pi 0,1 - Deplo…

CANopen, CoE and CiA DS402 profile
CANopen is high layer communication protocol, it is a standard for

embedded network built on CAN bus. CANopen is managed by CAN
in Automation [https://www.can-cia.org/] organization (CiA). In
CANopen, from basic CiA 301 specification there are several specific
profiles for different types of CANopen devices like CiA 401 for IO
Powered by Blogger.
www.simplerobot.net/2019/02/cia-402-motion-control-example-for.html 1/13
device, CiA 402 for motion devices (include servo drives, inverters,
Simple Robot stepper motor drivers,...).
...build
CoE
yourself
-
robot simply
CANopen over
with …
EtherCAT is the way to
implement CANopen profiles specification for EtherCAT. By CoE, we
Sidebar can utilize RASPBERRY-PI2&3
Home RASPBERRY-PI1&0 all well standardized CANopen protocols
RPI-EtherCAT FACEBOOKon high speed ,
high density EtherCAT network. With CoE, limitations of CAN bus like
PiCAT4 - The real… 28 low bit rate, short frame length are avoided.
CiA DS402 is a CANopen profile for motion devices or motor drivers.
In this motion profile, a set of standard objects, operation modes and
Teardown and Recovery… finite state machine (FSM) are predefined. And since everything is
standardized, Master control code for one driver type can be utilized to
PiCAT4 - Realtim… 14 other types with minimal modifications.
Several common predefined objects with indices in CiA 402 profile:

PiCAT Xenomai 3… 24
Controlword (object index 0x6040): control commands sent from
Master to slave (driver) to ask driver (actually FSM in driver)
change to ON state (servo-on), OFF state (servo-off), or for other
specific purposes
Raspberry pi 4 su… 17 Statusword (object index 0x6041): indicates current state of
slave device
Modes of operation (object index 0x6060): modes (torque,
velocity, position,...) that Master want slave device to work at
Target torque (object index 0x6071): requested torque from
PiCAT schematic… 4 Master when slave work in torque mode
Target velocity (object index 0x60ff): requested velocity from
Master when slave work in velocity mode
Target position (object index 0x607a): requested position from
Master when slave work in position mode
EtherCAT Master … 39 Torque actual value (object index 0x6077): actual torque
measured and sent by slave to Master
Velocity actual value (object index 0x606c): actual velocity
measured and sent by slave to Master
Position actual value (object index 0x6064): actual position
Raspberry Pi Eth… 57 measured and sent by slave to Master
EtherCAT Master … 11 Several operation modes of driver with CiA 402 profile compatible. The
value follow by mode name is code which Master would assign for
'Mode of operation' object:
EtherCAT
Profile position mode: 1
Velocity mode (frequency converter): 2
Raspberry pi 2,3 … 27 Profile velocity mode: 3
Torque profile mode: 4
Homing mode: 6
Interpolated position mode: 7
Cyclic synchronous position mode (CSP): 8
Raspberry pi 2,3 … 8 Cyclic synchronous velocity mode (CSV): 9
Cyclic synchronous torque mode (CST): 10
Any EtherCAT CiA 402 compatible driver should support at least one
mode from 8 to 10. Among these operation modes, CSP is most
popular found in EtherCAT drivers, this mode is useful for positioning
Powered by Blogger.
applications like CNC machine. CST is mostly used in robotics

Simple Robot ...build yourself robot simply with…
applications.
And here is standard FSM of CiA 402 driver

[https://1.bp.blogspot.com/-
M3lIylaXfaw/XF1EdbYPQiI/AAAAAAAAA70/io6Za9gehb
QgJQHejM9a-Renn1p8A0gGgCLcBGAs/s1600/CiA402-
EtherCAT
StateMachine.PNG]
In this figure, rectangle blocks are driver states and yellow highlighted
numbers are state transition. Driver state is indicated by Statusword
object, in order to change from one state to another state, Master
Raspberry pi 0,1 - Deplo… should send a corresponded Controlword value.
Powered by Blogger.

aAva7vuhve8/XF1F8B9GUVI/AAAAAAAAA8A/Hb8zQ15
XbqkKBweSixF0wjWxTj5GWwFWACLcBGAs/s1600/FS
M-in-out.PNG]
SERVO-ON: is procedure that Master should read Statusword from
driver and send corresponded Controlword (command) to driver to
Distributed Clock … 43 make it change to final state, Operation Enable. The common SERVO-
ON path in CiA 402 state machine is: (1) -> (2) -> (3) -> (4) or (13) ->
39
(14) -> (15) -> (2) -> (3) -> (4)
EtherCAT Master …
SERVO-OFF: is procedure to switch off servo drive, that is moving its

2-DOF Robot con… 14 state to 'Switch On Disabled' state. The shorted path is transition (9) in
which Master send Controlword of 0 to an operating driver.
For more detailed description about meaning of each bit in
Controlword, Statusword you should read CiA 402 document from
EtherCAT Master … 11 CAN in Automation website [https://www.can-cia.org/can-
knowledge/canopen/cia402/] , or CiA402 Implementation Directive from
EtherCAT group
EtherCAT
[https://www.ethercat.org/en/downloads/downloads_733ABB98E11545EA901
D80D9A4CA7F80.htm] , or directly from driver's manual.
Raspberry pi 0,1 - Deplo… CiA 402 motion control example for MAXPOS
drive
I have added a new motion control example

at https://github.com/thanhtam-h/soem-w5500-rpi/tree/rpi23-xenomai-
3/test/MAXPOS_CSP [https://github.com/thanhtam-h/soem-w5500-
rpi/tree/rpi23-xenomai-3/test/MAXPOS_CSP] [https://www.blogger.com/] .
This example control two MAXPOS drives in CSP mode. MAXPOS is
name of an EtherCAT
Powered byservo
Blogger. drive from Maxon, a well known motor
and driver manufacturer. For demonstration purpose, two motors are

Simple Robot first run independently and later they are connected together. They are
...build yourself robot simply with…
driven in synchronized position mode with same target positions but in
opposite direction.
[https://2.bp.blogspot.com/-rWTuQ_ppnS0/XF17etYj7GI/AAAAAAAAA8U/-
NgOrjuS7XAGg25HOOMdOGGrVT3nNN5_gCLcBGAs/s1600/20190206_234
EtherCAT Master … 39 301.jpg]
For controlling a CiA 402 drive, the most important work is

implementation of state machine to turn on servo (SERVO-ON). This
new control code is found in file
Raspberry Pi Eth… 57 servo.c. Function ServoOn_GetCtrlWrd compute target Controlword
according to current Statusword to turn on servo drive.
EtherCAT
Powered by Blogger.

Distributed Clock … 43 [https://2.bp.blogspot.com/-

aWdgwJ0MmCs/XF19A_ArH1I/AAAAAAAAA8g/kjd79xyXc88PHhO-
9A0HiJHQ76wT8gIMACLcBGAs/s1600/CiA402-FSM.PNG]
In ecat_init function of main.c file, after initialization we should do
2-DOF Robot con… 14 PDO remapping for slave device. Many drivers allow user to select
and remap PDO objects to match their control requirements. For
example MAXPOS drive has several reconfiguration PDO set (check
Raspberry Pi Eth… 57 its xml file), 0x1600 RxPDO and 0x1A00 TxPDO contain objects
suitable for CSP mode.
EtherCAT
Powered by Blogger.

EtherCAT Master … 39 vc6EY0aoXGI/XF2AofGORoI/AAAAAAAAA88/YSd2Q_vQulkoJ_M6CHyInG
Xl1pCDRjs5QCLcBGAs/s1600/PDO-remapping.PNG]
EtherCAT
DFqthehyRSk/XF2AVc6DqcI/AAAAAAAAA8w/vadYw9UDHhcBjIArfEKHV
VGYYYP8_ZXdQCLcBGAs/s1600/MAXPOS-default-PDO.PNG]
Powered by Blogger.
For other operation mode (e.g CST, CSV) user have to select other
Simple Robot suitable objects and do PDO remapping. In some cases, user should
...build yourself robot simply with…
do mapping for each individual entry.
Sidebar Most servoRASPBERRY-PI2&3

Home RASPBERRY-PI1&0 drivers support distributed clock feature,
RPI-EtherCAT FACEBOOKand some drivers
even don't work in position mode if Master don't enable DC control
PiCAT4 - The real… 28 (like Sanyo Denki servo driver). For MAXPOS, it will work with or
without of DC enabled.
PiCAT4 - Realtim… 14 Posted 8th February 2019 by Ho Tam

Labels: CiA 402, ethercat, motion, PiCAT, raspberry pi
29 View comments

GhostRider February 8, 2019 at 11:55 PM
Raspberry pi 4 su… 17 Was eagerly waiting for this post from you.Its been a long time.
Nevertheless, Thank you. Hope to get some details on using ROS service
for the same.
CiA 402 Motion c… 29 Reply
Anonymous February 18, 2019 at 8:46 PM
배터리 케이스에 한글..? 설마 한국인이셨나요. 정말 잘 보고있습니다. 좋은 정보

항상 감사합니다.
Reply
2-DOF Robot con… 14 Anonymous March 11, 2019 at 8:59 PM

Hi, thank you for the nice example. Are you connected to the RPi over a
WiFi SSH connection?
Reply
Replies
Ho Tam March 12, 2019 at 7:34 AM

EtherCAT
This case, Yes. However you still can use Ethernet port if you
wish.
Reply
Raspberry pi 2,3 … 8 gpfrank March 19, 2019 at 3:13 PM

First thank you for you share your knowledge! they give me a lot of help!
2 Can you do me a favor? do you know the limitation or not good points for
Raspberry pi 0,1 …
SOEM master stack? thanks a lot!
Reply
Replies
Powered by Blogger.
Simple Robot ...build yourselfHo Tam March 19, 2019 at 10:35 PM

robot simply with…
It is kind of personal point of view, so for this question FIY!

gpfrank April 2, 2019 at 11:35 AM
PiCAT4 - The real… 28 thanks a lot!
Teardown and Recovery… Reply
Anonymous April 5, 2019 at 3:14 PM
24 Hi Ho Tam,
PiCAT Xenomai 3…
I followed all of your tutorials and there are really great to get knowledge
67 about the Ethercat working.
Xenomai for Ras…
Currently I have few questions regarding SOEM.
In my project, I am using TMS570LC4357 Launchpad which has inbuilt
EMAC module and would like to use it as an etherCAT master. As an
29 etherCAT slave, I am using LAN9252 from Microchip.
CiA 402 Motion c…
Since, I am completely new to etherCAT, I do not know where to start i.e.
4 how to use the ethercat SOEM library and what do i need to edit in the code
PiCAT schematic…
etc.
43 Could you point me in the right direction to get it start working? Like what
Distributed Clock …
are the steps do i need to do initially?
Reply
Replies
Ho Tam April 7, 2019 at 3:48 PM
This will be a hard work, in order to port SOEM to bare metal
Raspberry Pi Eth… 57 platform you should do 2 important jobs:
- Network driver, mostly you have to implement 3 functions:
initialization, raw sending and raw receiving
EtherCAT Master … 11 - Time management, you have to provide system time and
modify osal.c file, replace linux time functions (like clock_gettime
) by your functions.
EtherCAT If you are new to SOEM, I suggest you first get familiar with it in
Linux environment before porting to new system.
Anonymous July 26, 2019 at 1:28 PM

Raspberry pi 0,1 - Deplo… Hi Poorna, Ho Tam,
First of all great work Ho Tam! Really like how you explain things
Raspberry pi 2,3 … 8 and use affordable hardware and opensource software to get
industry like performance.
Raspberry pi 0,1 … 2 @Poorna: I have quit some experience building EtherCAT

slaves using the LAN9252. I mostly use the SOES stack and
combine the LAN9252 with a microcontroller of my onw choice
(mostly ST Microelectronics or TI).
Powered by Blogger.
Reply
Sidebar KSolberg
Home RASPBERRY-PI1&0 July 7, 2019 at 4:16
RASPBERRY-PI2&3 PM
RPI-EtherCAT FACEBOOK
Are you working with G-Code? My 3d printer has ethercat servos in all axis,
PiCAT4 - The real… 28 including the extruder.(Copley plus and Elmo gold) I'm using step/direction
now, but would love to use ethercat. Unfortunately existing solutions are
expensive.
Teardown and Recovery… Reply
PiCAT4 - Realtim… 14 Replies
Ho Tam July 8, 2019 at 11:03 AM

I have tried to develop a g-code parser and motion planning, it
worked but not enough for releasing as a project yet. I think you
should consider porting available projects, it may not be too
difficult.
Burak ÖZLER November 16, 2019 at 4:08 PM
Hi Ho TAM,
I'm following your work it's great.. For CNC you don't need to
write complete G_code interpreter. You can use Linux CNC and
write a HAL file for it.
Linux CNC already has a Ethercat HAL from rt-labs.. Bu they
don't have SOEM HAL, I think SOEM is much more lighter and
can be use much more widely with Linux CNC..
Have a nice day..

2-DOF Robot con… 14 Ho Tam November 17, 2019 at 7:46 AM

Hi Burak ÖZLER,
Thanks for your suggestion, actually I have quite good
experience with LinuxCNC and HAL driver, however there is an
issue leading me to another solution. After patching Xenomai for
rpi, OpenGL doesn't work and therefore LinuxCNC become very
slow on rpi.
EtherCAT Reply
Raspberry pi 0,1 - Deplo… K Solberg,
Did you check Codesys? The provide an EtherCAT master for a raspberry
Raspberry pi 2,3 … 8 pi or beaglebone for ~50EUR. You can add CNC module as well to control
the 3d printer.
2 Reply
Raspberry pi 0,1 …

Multicore RPi: https://store.codesys.com/softplc/codesys-control-for-
raspberry-pi-mc-sl.html#Product%20Description
Powered by Blogger.
Standard RPi: https://store.codesys.com/softplc/codesys-control-for-

raspberry-pi-sl.html
Reply
28 Davin Seol February 21, 2020 at 4:59 AM

PiCAT4 - The real…
So this example uses MAXPOS drives and essentially controls the motors
via the raspberry pi. Do you think this same method would be viable with
Teardown and Recovery… another motor and driver from Sanyo Denki that uses ethercat to
communicate
14 Reply
PiCAT4 - Realtim…
Replies
Ho Tam February 21, 2020 at 8:10 AM
67 I am quite sure that it will work for any CiA402 compatible drive.
Xenomai for Ras…
At least I have tested with Maxon, Elmo, Servotronix, Yaskawa
and Neuromeka Drive
Reply
PiCAT schematic… 4 Anonymous April 2, 2020 at 9:12 PM

Hi Ho Tam,
is the implementation of the XML-File (ESI) necessary to run an motor
Distributed Clock … 43 controller with CIA 402? If I run the slaveninfo -sdo, it doesn't display the
CoE object dictionary. have you already made experiences with such
controllers?
Reply
2-DOF Robot con… 14 Replies
Ho Tam April 7, 2020 at 11:37 AM

Unfortunately not all drives support sdo access, it is not
surprised if you see nothing with -sdo option. Slave ESI is
EtherCAT Master … 11 standard for every EtherCAT slave device, although some
slaves have already had default fixed PDO configuration, some
additional information may be only found in ESI file.
EtherCAT
Reply
Raspberry pi 0,1 - Deplo… Steven June 27, 2020 at 12:11 AM

Is there a way you can give a brief example on how to map the indivial entry
for both the rxpdo and the txpdo?
Reply
Minmin July 29, 2020 at 11:11 PM
Hello!
I've tried your Xenomai 3 with Raspberry pi 4 and tested the soem codes
with XMC4800 that shows to work fine. Now, I modified your
Powered by Blogger.
MAXPOS_CSP code for my EPOS4 controller and tried to control one
motor (one controller only, unlike your application).
Now, I am facing some interesting results.
The codes seem to function without re-mapping, but the actual position and
Sidebar Home RASPBERRY-PI1&0 RASPBERRY-PI2&3
target position is extremelyRPI-EtherCAT
off. There seems FACEBOOK
to be a large difference
between the commands (with a ratio of approximately 60). I am now setting
the sine_amp = 11500000000, but still not seeing my motor moving.
The slave seems to be in state 0x02 with no AL error. but the status word
runs crazy with various numbers (for example, one sample shows 0xB024
and the next shows 0xA334).
On the other hand, if I try to remap PDO, things just doesn't work (with
RxPDO assignment error).
Got any idea what is happening? I've checked the Index with EPOS studio
and they should be identical to your code. If I change the subindex to 00, it
seems to work, but
Reply
Replies
but later shows expectedWKC = 0
4 Ho Tam July 30, 2020 at 9:54 AM

PiCAT schematic…
Seems you have problem with PDO mapping or some special
settings (DC for example). I am afraid that I can't help in this
Distributed Clock … 43 case. I don't have EPOS4 or any information about that device.
I see. Is there any parameter in the codes that adjusts the output
2-DOF Robot con… 14 positions unit? I don't seem to find any, but please correct me if
I'm wrong.
Thank you again for your feedback and nice work.
One more thing. I see in the object directory that the index are
EtherCAT identical to your settings. What would happen if I don't remap?
Isn't the index identical, and I don't need to remap?

Raspberry pi 0,1 - Deplo… I had several rounds of check, and this is what I see:
I can force the controller and actuator into specific mode, for
exampe the CSV mode with a specific speed.
Raspberry pi 2,3 … 8 But with the soem code, it seems that it was able to switch
controller state (for example from Pre_op to Safe_op) but not
the position or speed.
Raspberry pi 0,1 … 2 With ec_statecheck, I can see that the state does change and it
also reflect with the controller LED light. But once it return to OP
state, it would just operate the previous etting from the Epos
studio. For example, if I previously set the command to CSV,
once the controller reach OP state it just start runing.
Powered by Blogger.
Ho Tam July 31, 2020 at 8:07 AM
Simple Robot ...build yourselfTalk about MAXPOS (the one I had), Maxon design several
robot simply with…
PDO groups like CSP CSV CST,...Depending on your desired
operation mode you should select and map proper groups. The
default PDO mapping of MAXPOS is for CSP mode only. When
you try to remap, you must change pdo information in pdo_def.h
and PDO index in ecat_init function.
PiCAT4 - The real… 28 One important note, you should check your driver firmware
revision and then select matched pdo information in ESI file.
Different firmware revision will have different pdo information.
Teardown and Recovery… Good luck!
Minmin August 4, 2020 at 4:55 PM
An update from my side. I've checked the mapping, and it
PiCAT Xenomai 3… 24 should be correct. For Epos4, we don't need to remap but
command directly the specific object for different modes (such
as CSP...etc). Therefore, I can use ec_SDOwrite to command
Xenomai for Ras… 67 the controller to operate in each different mode and move the
motor. But I'm not sure did the rest of the codes work fine. For
example, the functions such as ec_statecheck works fine, and
Raspberry pi 4 su… 17 the ec_slave[i].state are able to map to the state. Also, your
code ServoOn_GetCtrlWrd is able to work and command the
controller to operation mode. However, I cannot use
CiA 402 Motion c… 29 maxpos_drove_pt[i].ptOutParam->TargetPosition to command
the position, or to use maxpos_drove_pt[i].ptOutParam-
>ModeOfOperation to modify the mode. How should I explain
PiCAT schematic… 4 this result?
Reply
EtherCAT Master … 39 Enter Comment
EtherCAT
Raspberry pi 0,1 - Deplo… Load more
Powered by Blogger.

CiA 402 Motion Control Example For MAXPOS Drive - Simple Robot

Uploaded by

Copyright:

Available Formats

CiA 402 Motion Control Example For MAXPOS Drive - Simple Robot

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

CiA 402 Motion Control Example For MAXPOS Drive - Simple Robot

Uploaded by

Copyright:

Available Formats

9/27/23, 2:32 PM CiA 402 Motion control example for MAXPOS drive | Simple Robot

Simple Robot ...build yourself robot simply with… search

Sidebar Home RASPBERRY-PI1&0 RASPBERRY-PI2&3 RPI-EtherCAT FACEBOOK

PiCAT4 - The real… 28

CiA 402 Motion c… 29

2-DOF Robot con… 14

Raspberry pi 0,1 - Deplo…

CANopen is high layer communication protocol, it is a standard for

Several common predefined objects with indices in CiA 402 profile:

applications like CNC machine. CST is mostly used in robotics

And here is standard FSM of CiA 402 driver

PiCAT4 - The real… 28

Teardown and Recovery…

Xenomai for Ras… 67

CiA 402 Motion c… 29

2-DOF Robot con… 14

Simple Robot ...build yourself robot simply with…

PiCAT4 - The real… 28

Teardown and Recovery…

Xenomai for Ras… 67

SERVO-OFF: is procedure to switch off servo drive, that is moving its

I have added a new motion control example

and driver manufacturer. For demonstration purpose, two motors are

PiCAT4 - The real… 28

Teardown and Recovery…

Xenomai for Ras… 67

CiA 402 Motion c… 29

For controlling a CiA 402 drive, the most important work is

Raspberry pi 0,1 - Deplo…

Simple Robot ...build yourself robot simply with…

PiCAT4 - The real… 28

Teardown and Recovery…

Xenomai for Ras… 67

CiA 402 Motion c… 29

Distributed Clock … 43 [https://2.bp.blogspot.com/-

Raspberry pi 0,1 - Deplo…

Simple Robot ...build yourself robot simply with…

PiCAT4 - The real… 28

Teardown and Recovery…

Xenomai for Ras… 67

CiA 402 Motion c… 29

Raspberry pi 0,1 - Deplo…

Sidebar Most servoRASPBERRY-PI2&3

PiCAT4 - Realtim… 14 Posted 8th February 2019 by Ho Tam

Xenomai for Ras… 67

배터리 케이스에 한글..? 설마 한국인이셨나요. 정말 잘 보고있습니다. 좋은 정보

2-DOF Robot con… 14 Anonymous March 11, 2019 at 8:59 PM

Ho Tam March 12, 2019 at 7:34 AM

Raspberry pi 2,3 … 8 gpfrank March 19, 2019 at 3:13 PM

Simple Robot ...build yourselfHo Tam March 19, 2019 at 10:35 PM

Sidebar Home RASPBERRY-PI1&0 RASPBERRY-PI2&3 RPI-EtherCAT FACEBOOK

Teardown and Recovery… Reply

Anonymous July 26, 2019 at 1:28 PM

Raspberry pi 0,1 … 2 @Poorna: I have quit some experience building EtherCAT

PiCAT4 - Realtim… 14 Replies

Ho Tam July 8, 2019 at 11:03 AM

Have a nice day..

2-DOF Robot con… 14 Ho Tam November 17, 2019 at 7:46 AM