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

Revision history [back]

click to hide/show revision 1
initial version

The problem was resolved.

According the the DSP 402 protocol, if the halt bit is 1, bit 4 of the control word is not executed in IP mode, and therefore IP mode is not activated, and therefore the axles don't move.

It seems that ros-canopen sets the halt bit to 1 in line 413 of the Handlewrite function in the canopen_402/motor.cpp code and only clears it if the state is Operation Enabled.

For my controller (nanotec controller), the mode cannot be switched in OE state, the controller has to transition to Switched on, or Ready to Switch on, or Switch on disabled states to perform a mode switch. ros-canopen accounts for such controllers by allowing you to switch states on initialization to set the desired mode of operation (IP mode in my case). When the state was switched down from OE, the handlewrite was setting the halt bit in the control word.

Another factor was that the RPDO for the control word i had with transmission type FF, rather than 01. This was done because when i set the transmission type to 01, roscanopen fails to initialize with the error "Could not enable motor", which is a result of failing to transition to OE state. The only way i managed to get past that error was to set RPDO transmission to FF for the control word.

So because the RPDO is asynchronous, the last time the controller processed the control word was when the halt bit was set to "1".

You can see this in the post above. The control word was being written as 31 (1111 binary) with bit 4 set to 1, but the controller was ignoring it because it is asynchronous PDO, so the last time it processed it the halt bit was set, and so IP was never activated.

I solved this by commenting out the setting of the halt bit in line 413 of motor.cpp.

This problem would not have occured if my controller was able to switch modes in OE State.

I caution users. IF your controller cannot switch modes in OE State, AND you want to use IP mode, this issue might occur to you, and you might solve it the way i did, or find a better way.

Ros-canopen wizards, what ramifications does my solution have to the general operation of the controller? Is there a smarter way of doing this?

Thanks.

The problem was resolved.

According the the DSP 402 protocol, if the halt bit is 1, bit 4 of the control word is not executed in IP mode, and therefore IP mode is not activated, and therefore the axles don't move.

It seems that ros-canopen sets the halt bit to 1 in line 413 of the Handlewrite function in the canopen_402/motor.cpp code and only clears it if the state is Operation Enabled.

For my controller (nanotec controller), the mode cannot be switched in OE state, the controller has to transition to Switched on, or Ready to Switch on, or Switch on disabled states to perform a mode switch. ros-canopen accounts for such controllers by allowing you to switch states on initialization to set the desired mode of operation (IP mode in my case). When the state was switched down from OE, the handlewrite was setting the halt bit in the control word.

Another factor was that the RPDO for the control word i had with transmission type FF, rather than 01. This was done because when i set the transmission type to 01, roscanopen fails to initialize with the error "Could not enable motor", which is a result of failing to transition to OE state. The only way i managed to get past that error was to set RPDO transmission to FF for the control word.

So because the RPDO is asynchronous, the last time the controller processed the control word was when the halt bit was set to "1".

You can see this in the post above. The control word was being written as 31 (1111 binary) with bit 4 set to 1, but the controller was ignoring it because it is asynchronous PDO, so the last time it processed it the halt bit was set, and so IP was never activated.

I solved this by commenting out the setting of the halt bit in line 413 of motor.cpp.

This problem would not have occured if my controller was able to switch modes in OE State.

I caution users. IF your controller cannot switch modes in OE State, AND you want to use IP mode, AND you have the transmission type of the control word RPDO set to FF, this issue might occur to you, and you might solve it the way i did, or find a better way.

Ros-canopen wizards, what ramifications does my solution have to the general operation of the controller? Is there a smarter way of doing this?

Thanks.