ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Control Trinamic motor using ros_canopen

asked 2016-06-06 02:53:47 -0500

JuliusGel gravatar image

I am trying to control Trinamic PD-1160 motor using ros_canopen. The motor is DS402 complient. The CANopen manual of this motor is located here. I have configured the canopen_motor_node using the following eds and yaml files.

The canopen_motor_node starts successfully using this configuration file but initialization service fails with the following error:

Transition timeout; Could not enable motor; Transition timeout

Here is a short snippet of candump output that I get when initializing the canopen_motor_node:

can0  000   [2]  82 01
can0  701   [1]  00
can0  601   [8]  2B 17 10 00 00 00 00 00
can0  581   [8]  60 17 10 00 00 00 00 00
can0  601   [8]  40 41 60 00 00 00 00 00
can0  581   [8]  4B 41 60 00 18 06 00 00
can0  601   [8]  40 61 60 00 00 00 00 00
can0  581   [8]  4F 61 60 00 00 00 00 00
can0  601   [8]  40 64 60 00 00 00 00 00
can0  581   [8]  43 64 60 00 00 00 00 00
can0  601   [8]  40 6C 60 00 00 00 00 00
can0  581   [8]  43 6C 60 00 00 00 00 00
can0  601   [8]  40 44 60 00 00 00 00 00
can0  581   [8]  4B 44 60 00 00 00 00 00
can0  601   [8]  40 40 60 00 00 00 00 00
can0  581   [8]  4B 40 60 00 00 00 00 00
can0  601   [8]  40 FF 60 00 00 00 00 00
can0  581   [8]  43 FF 60 00 00 00 00 00
can0  000   [2]  01 01
can0  181   [2]  18 06
can0  281   [3]  18 06 00
can0  601   [8]  40 01 10 00 00 00 00 00
can0  581   [8]  4F 01 10 00 00 00 00 00
can0  601   [8]  40 01 10 00 00 00 00 00
can0  581   [8]  4F 01 10 00 00 00 00 00
can0  301   [3]  00 00 00
can0  081   [8]  20 63 01 00 FF 00 00 00
can0  080   [0] 
can0  381   [6]  18 06 00 00 00 00
can0  000   [2]  02 01

From this I see that the first control word and operation mode is all 00 (message 301). The motor responds with emergency message to this. From the motors documentation this error message is:

Parameter error
The data in the received PDO is either wrong or cannot be accepted due to the
internal state of the drive.

My question is weather this is a configuration issue or a bug in ros_canopen. The motor documentation has a section describing that in order to move the motor its operation mode has to be changed first and only then the state switching should occur. Note that I have changed the switching_state parameter to 2 but this did not have any effect on init function behavior.

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2016-06-06 04:18:20 -0500

Mathias Lüdtke gravatar image

updated 2016-06-06 04:18:38 -0500

(from the manual:) The motor will not run when the operating mode is set to 0. It will be stopped when the motor is running in one of the supported operating modes and the operating mode is then switched to 0

Note that I have changed the switching_state parameter to 2 but this did not have any effect on init function behavior.

I don't think this is necessary, at least I don't find the requirement in the manual at a first glance. The mode 0 in the PDO message does not correspond to mode switch, but to the default value in the EDS. (Your config couples the control word and the mode in one PDO, so both get sent)

If you want to set the initial value, just add a ParameterValue with a specific mode to your EDS/DCF. Or use the dcf_overlay feature for convenience ( http://wiki.ros.org/canopen_chain_nod... ). This way 6060 gets written before the start-up and any PDO.

edit flag offensive delete link more

Comments

Adding ParameterValue to 6060 and 6040 helped to get rid of 081 emergency message. The problem is that I still get the same error: Transition timeout; Could not enable motor; Transition timeout. Maybe you know what other issues I might have?

JuliusGel gravatar image JuliusGel  ( 2016-06-07 00:38:02 -0500 )edit

Hard to tell. Please add an updated log (at gist?).

Mathias Lüdtke gravatar image Mathias Lüdtke  ( 2016-06-07 03:13:25 -0500 )edit

I have updated the eds and added updated candump here

JuliusGel gravatar image JuliusGel  ( 2016-06-07 03:55:07 -0500 )edit

We have found the issue, the main problem was that at the moment ros_canopen does not allow to map the same object in multiple PDO. Related issue here.

JuliusGel gravatar image JuliusGel  ( 2016-06-29 09:07:02 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2016-06-06 02:53:47 -0500

Seen: 731 times

Last updated: Jun 06 '16