Open Aragya1642 opened 1 week ago
Possibly one of the project maintainers can help answer your specific question about how to get polling mode working, but this caught my attention as I've successfully used Elmo motor controllers with this library by setting polling to false, so are you sure polling set to true is something you need? I ask because I notice the comment in your bus.yml file says:
polling: true # This allows for us to get values of position, velocity, and effort when set True
The way I get those values in my system is by including the following section in my bus.yml file:
tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
1:
enabled: true
cob_id: "auto"
transmission: 0x01
mapping:
- {index: 0x6041, sub_index: 0} # status word
- {index: 0x6061, sub_index: 0} # mode of operation display
2:
enabled: true
cob_id: "auto"
transmission: 0x01
mapping:
- {index: 0x6064, sub_index: 0} # position actual value
- {index: 0x606c, sub_index: 0} # velocity actual position
and then the position and velocity are available as PDOs. You could find the index of the actual current / torque in the Elmo manual and include that as a PDO to get effort as well, I believe.
Possibly one of the project maintainers can help answer your specific question about how to get polling mode working, but this caught my attention as I've successfully used Elmo motor controllers with this library by setting polling to false, so are you sure polling set to true is something you need? I ask because I notice the comment in your bus.yml file says:
polling: true # This allows for us to get values of position, velocity, and effort when set True
The way I get those values in my system is by including the following section in my bus.yml file:
tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation 1: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6041, sub_index: 0} # status word - {index: 0x6061, sub_index: 0} # mode of operation display 2: enabled: true cob_id: "auto" transmission: 0x01 mapping: - {index: 0x6064, sub_index: 0} # position actual value - {index: 0x606c, sub_index: 0} # velocity actual position
and then the position and velocity are available as PDOs. You could find the index of the actual current / torque in the Elmo manual and include that as a PDO to get effort as well, I believe.
The issue is that when I try to do PDO mapping, the device does not boot up. Have you ever seen an issue of that sort? This is the output that I see in the terminal:
[INFO] [launch]: All log files can be found below /home/agoyal1642/.ros/log/2024-11-08-15-58-33-372583-agoyal1642-HP-Laptop-14-dq1xxx-8911
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [launch.user]: /home/agoyal1642/Documents/Zoe2/zoe2/install/zoe_can_network/share/zoe_can_network/config/motorBus/bus.yml
[INFO] [launch.user]: /home/agoyal1642/Documents/Zoe2/zoe2/install/zoe_can_network/share/zoe_can_network/config/motorBus/master.dcf
[INFO] [launch.user]:
[INFO] [launch.user]: can0
[INFO] [device_container_node-1]: process started with pid [8925]
[device_container_node-1] [INFO] [1731099513.852722978] [device_container_node]: Starting Device Container with:
[device_container_node-1] [INFO] [1731099513.852860306] [device_container_node]: master_config /home/agoyal1642/Documents/Zoe2/zoe2/install/zoe_can_network/share/zoe_can_network/config/motorBus/master.dcf
[device_container_node-1] [INFO] [1731099513.852875881] [device_container_node]: bus_config /home/agoyal1642/Documents/Zoe2/zoe2/install/zoe_can_network/share/zoe_can_network/config/motorBus/bus.yml
[device_container_node-1] [INFO] [1731099513.852888644] [device_container_node]: can_interface_name can0
[device_container_node-1] [INFO] [1731099513.854100523] [device_container_node]: Loading Master Configuration.
[device_container_node-1] [INFO] [1731099513.854798437] [device_container_node]: Load Library: /home/agoyal1642/Documents/Zoe2/zoe2/install/canopen_master_driver/lib/libmaster_driver.so
[device_container_node-1] [INFO] [1731099513.860438425] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1731099513.860477735] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1731099513.868141550] [master]: NodeCanopenBasicMaster
[device_container_node-1] [INFO] [1731099513.868383942] [device_container_node]: Load master component.
[device_container_node-1] [INFO] [1731099513.868499155] [device_container_node]: Added /master to executor
[device_container_node-1] NMT: entering reset application state
[device_container_node-1] NMT: entering reset communication state
[device_container_node-1] NMT: running as master
[device_container_node-1] NMT: entering pre-operational state
[device_container_node-1] NMT: entering operational state
[device_container_node-1] [INFO] [1731099513.907817002] [device_container_node]: Loading Driver Configuration.
[device_container_node-1] [INFO] [1731099513.907957315] [device_container_node]: Found device motor1 with driver ros2_canopen::Cia402Driver
[device_container_node-1] [INFO] [1731099513.908566077] [device_container_node]: Load Library: /home/agoyal1642/Documents/Zoe2/zoe2/install/canopen_402_driver/lib/libcia402_driver.so
[device_container_node-1] error: SDO abort code 06090030 received while updating the configuration of node 03: Invalid value for parameter
[device_container_node-1] [INFO] [1731099513.922038561] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1731099513.922147928] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1731099513.930999509] [device_container_node]: Load driver component.
[device_container_node-1] [INFO] [1731099513.931203388] [device_container_node]: Added /motor1 to executor
[device_container_node-1] [INFO] [1731099513.960521674] [motor1]: scale_pos_to_dev_ 1000.000000
[device_container_node-1] scale_pos_from_dev_ 0.001000
[device_container_node-1] scale_vel_to_dev_ 1000.000000
[device_container_node-1] scale_vel_from_dev_ 0.001000
[device_container_node-1]
[device_container_node-1] [INFO] [1731099513.963599293] [motor1]: eds file /home/agoyal1642/Documents/Zoe2/zoe2/install/zoe_can_network/share/zoe_can_network/config/motorBus/Eds1406C.eds
[device_container_node-1] [INFO] [1731099513.963674935] [motor1]: bin file /home/agoyal1642/Documents/Zoe2/zoe2/install/zoe_can_network/share/zoe_can_network/config/motorBus/motor1.bin
[device_container_node-1] 2F60: warning: DefaultValue underflow
[device_container_node-1] 2F60: warning: ParameterValue underflow
[device_container_node-1] Found rpdo mapped object: index=2001 subindex=0
[device_container_node-1] Found rpdo mapped object: index=2012 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6061 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6064 subindex=0
[device_container_node-1] Found tpdo mapped object: index=606c subindex=0
[device_container_node-1] [WARN] [1731099513.986734721] [motor1]: Wait for device to boot.
This was the output that I saw on the canbus:
can0 701 [1] 00
can0 000 [2] 82 00
can0 704 [1] 00
can0 703 [1] 00
can0 702 [1] 00
can0 603 [8] 40 00 10 00 00 00 00 00
can0 583 [8] 42 00 10 00 92 01 02 00
can0 603 [8] 40 18 10 01 00 00 00 00
can0 583 [8] 42 18 10 01 9A 00 00 00
can0 603 [8] 40 18 10 02 00 00 00 00
can0 583 [8] 42 18 10 02 35 01 03 00
can0 603 [8] 23 00 18 01 83 01 00 80
can0 583 [8] 80 00 18 01 30 00 09 06
can0 080 [0]
can0 080 [0]
can0 080 [0]
can0 080 [0]
can0 080 [0]
can0 080 [0]
can0 734 [1] 00
can0 733 [1] 00
can0 732 [1] 00
This is the eds file which I am working with: eds.txt
Describe the bug I am trying to use the CiA 402 driver to be able to control a servomotor via an elmo motor controller. An issue I am encountering is that when I turn on polling mode, I am not able to move through the state machine of the servomotor as the polling mode constantly writes to the controlword which keeps it in an operation disabled mode. I was able to go through the source code for the driver and found two functions in the motor.cpp file. I am unsure how these are operating but I believe these functions are determining what parameters to poll and what to write to the controlword of the motor. If someone could explain what the functions are doing, it would be of massive help. Thanks!
Logs This is the CAN log and terminal output when I enable polling mode: Note: The last 8 messages in the log repeat as they are the polling messages
Setup:
Additional context This is my bus.yml file. When I set polling to false, my motor starts to spin but when I set it to true, the motor shaft does not spin.
These are the specific functions in the motor.cpp file that I believe control what is being polled.