Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

How does polling work in CiA 402 driver? #331

Open
Aragya1642 opened this issue Nov 4, 2024 · 2 comments
Open

How does polling work in CiA 402 driver? #331

Aragya1642 opened this issue Nov 4, 2024 · 2 comments
Labels
question Further information is requested

Comments

@Aragya1642
Copy link

Aragya1642 commented Nov 4, 2024 •
edited
Loading

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

(1730301035.277560) can0 701#00
(1730301035.278645) can0 000#8200
(1730301035.279886) can0 704#00
(1730301035.280939) can0 701#00
(1730301035.280940) can0 702#00
(1730301035.280940) can0 703#00
(1730301035.281951) can0 603#4000100000000000
(1730301035.282954) can0 583#4200100092010200
(1730301035.283965) can0 603#4018100100000000
(1730301035.283969) can0 583#421810019A000000
(1730301035.284989) can0 734#00
(1730301035.284990) can0 603#4018100200000000
(1730301035.284991) can0 583#4218100235010300
(1730301035.286020) can0 603#2B40600006000000
(1730301035.286021) can0 583#6040600000000000
(1730301035.287039) can0 603#2B40600007000000
(1730301035.287041) can0 583#6040600000000000
(1730301035.288037) can0 603#2F60600003000000
(1730301035.289082) can0 583#6060600000000000
(1730301035.290063) can0 603#2B4060000F000000
(1730301035.290066) can0 583#6040600000000000
(1730301035.291041) can0 603#23FF6000D0070000
(1730301035.297356) can0 583#60FF600000000000
(1730301035.298371) can0 000#0103
(1730301035.299369) can0 183#3702
(1730301035.371379) can0 603#4061600000000000
(1730301035.372440) can0 583#4261600003000000
(1730301035.373793) can0 603#2B40600064730000
(1730301035.374822) can0 583#6040600000000000
(1730301035.374823) can0 183#4002
(1730301035.375807) can0 603#4064600000000000
(1730301035.376888) can0 583#42646000CF821800
(1730301035.377859) can0 603#406C600000000000
(1730301035.378935) can0 583#426C60005D030000
(1730301035.380474) can0 603#4061600000000000
(1730301035.381538) can0 583#4261600003000000
(1730301035.382594) can0 603#2B40600064730000
(1730301035.382596) can0 583#6040600000000000
(1730301035.383601) can0 603#4064600000000000
(1730301035.383603) can0 583#42646000D0821800
(1730301035.384696) can0 603#406C600000000000
(1730301035.385620) can0 583#426C600083000000
(1730301035.390686) can0 603#4061600000000000
(1730301035.391717) can0 583#4261600003000000
(1730301035.392748) can0 603#2B40600064730000
(1730301035.393795) can0 583#6040600000000000
(1730301035.393795) can0 183#4012
(1730301035.394768) can0 603#4064600000000000
(1730301035.394770) can0 583#42646000D0821800
(1730301035.395810) can0 603#406C600000000000
(1730301035.395812) can0 583#426C600000000000
(1730301035.400558) can0 603#4061600000000000
(1730301035.401667) can0 583#4261600003000000
(1730301035.402702) can0 603#2B40600064730000
(1730301035.402704) can0 583#6040600000000000
(1730301035.403699) can0 603#4064600000000000
(1730301035.404685) can0 583#42646000D0821800

agoyal1642@agoyal1642-HP-Laptop-14-dq1xxx:~/Documents/Zoe2/zoe2$ ros2 launch zoe_can_network motor_setup.launch.py
[INFO] [launch]: All log files can be found below /home/agoyal1642/.ros/log/2024-10-30-11-10-34-697255-agoyal1642-HP-Laptop-14-dq1xxx-9963
[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 [9977]
[device_container_node-1] [INFO] [1730301035.216817741] [device_container_node]: Starting Device Container with:
[device_container_node-1] [INFO] [1730301035.217015668] [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] [1730301035.217047281] [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] [1730301035.217071213] [device_container_node]: 	 can_interface_name can0
[device_container_node-1] [INFO] [1730301035.218675895] [device_container_node]: Loading Master Configuration.
[device_container_node-1] [INFO] [1730301035.219631067] [device_container_node]: Load Library: /home/agoyal1642/Documents/Zoe2/zoe2/install/canopen_master_driver/lib/libmaster_driver.so
[device_container_node-1] [INFO] [1730301035.225937006] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1730301035.225997021] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1730301035.234871388] [master]: NodeCanopenBasicMaster
[device_container_node-1] [INFO] [1730301035.235178512] [device_container_node]: Load master component.
[device_container_node-1] [INFO] [1730301035.235298030] [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] [1730301035.277179889] [device_container_node]: Loading Driver Configuration.
[device_container_node-1] [INFO] [1730301035.277314912] [device_container_node]: Found device motor1 with driver ros2_canopen::Cia402Driver
[device_container_node-1] [INFO] [1730301035.277722384] [device_container_node]: Load Library: /home/agoyal1642/Documents/Zoe2/zoe2/install/canopen_402_driver/lib/libcia402_driver.so
[device_container_node-1] [INFO] [1730301035.290393342] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1730301035.290458182] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1730301035.300421717] [device_container_node]: Load driver component.
[device_container_node-1] [INFO] [1730301035.300610121] [device_container_node]: Added /motor1 to executor
[device_container_node-1] [ERROR] [1730301035.331045499] [motor1]: Could not read period from config, setting to 10ms
[device_container_node-1] [INFO] [1730301035.331508590] [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] [1730301035.335315621] [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] [1730301035.335373320] [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=2013 subindex=0
[device_container_node-1] [INFO] [1730301035.359009763] [motor1]: Driver booted and ready.
[device_container_node-1] [INFO] [1730301035.359609669] [motor1]: Starting with polling mode.
[device_container_node-1] [INFO] [1730301035.373116157] [canopen_402_driver]: Fault reset

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.

options:
  dcf_path: "@BUS_CONFIG_PATH@"

master:
  node_id: 1
  driver: "ros2_canopen::MasterDriver"
  package: "canopen_master_driver"
  sync_period: 100000
  # boot_time: 10000
  # boot_timeout: 10000

motor1:
  node_id: 3
  dcf: "Eds1406C.eds"
  package: "canopen_402_driver"
  driver: "ros2_canopen::Cia402Driver"
  # period: 10
  revision_number: 0x000103F4
  polling: true # This allows for us to get values of position, velocity, and effort when set True
  # heartbeat_producer: 1000 # Heartbeat every 1000 ms
  sdo:
    - {index: 0x6040, sub_index: 0, value: 0x06} # Shutdown
    - {index: 0x6040, sub_index: 0, value: 0x07} # Switch On
    # - {index: 0x6060, sub_index: 0, value: 1} # Set operation mode to Profile Position
    - {index: 0x6060, sub_index: 0, value: 3} # Set operation mode to Profiled Velocity
    - {index: 0x1800, sub_index: 2, value: 10} # Set TPDO 1 as synchronous per 5 syncs by changing the transmission type
    - {index: 0x6040, sub_index: 0, value: 0x0F} # Enable Operation
    - {index: 0x60FF, sub_index: 0, value: 2000.0} # Set target velocity to 2000 units
  tpdo:
    1:
      enabled: true

These are the specific functions in the motor.cpp file that I believe control what is being polled.

bool Motor402::readState()
{
  uint16_t old_sw, sw = driver->universal_get_value<uint16_t>(
                     status_word_entry_index, 0x0);  // TODO: added error handling
  old_sw = status_word_.exchange(sw);

  state_handler_.read(sw);

  std::unique_lock lock(mode_mutex_);
  uint16_t new_mode;
  new_mode = driver->universal_get_value<int8_t>(op_mode_display_index, 0x0);
  // RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode %hhi",new_mode);

  if (selected_mode_ && selected_mode_->mode_id_ == new_mode)
  {
    if (!selected_mode_->read(sw))
    {
      RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode handler has error.");
    }
  }
  if (new_mode != mode_id_)
  {
    mode_id_ = new_mode;
    mode_cond_.notify_all();
  }
  if (selected_mode_ && selected_mode_->mode_id_ != new_mode)
  {
    RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Mode does not match.");
  }
  if (sw & (1 << State402::SW_Internal_limit))
  {
    if (old_sw & (1 << State402::SW_Internal_limit))
    {
      RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Internal limit active");
    }
    else
    {
      RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Internal limit active");
    }
  }

  return true;
}
void Motor402::handleRead() { readState(); }
void Motor402::handleWrite()
{
  std::scoped_lock lock(cw_mutex_);
  control_word_ |= (1 << Command402::CW_Halt);
  if (state_handler_.getState() == State402::Operation_Enable)
  {
    std::scoped_lock lock(mode_mutex_);
    Mode::OpModeAccesser cwa(control_word_);
    bool okay = false;
    if (selected_mode_ && selected_mode_->mode_id_ == mode_id_)
    {
      okay = selected_mode_->write(cwa);
    }
    else
    {
      cwa = 0;
    }
    if (okay)
    {
      control_word_ &= ~(1 << Command402::CW_Halt);
    }
  }
  if (start_fault_reset_.exchange(false))
  {
    RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Fault reset");
    this->driver->universal_set_value<uint16_t>(
      control_word_entry_index, 0x0, control_word_ & ~(1 << Command402::CW_Fault_Reset));
  }
  else
  {
    // RCLCPP_INFO(rclcpp::get_logger("canopen_402_driver"), "Control Word %s",
    // std::bitset<16>{control_word_}.to_string());
    this->driver->universal_set_value<uint16_t>(control_word_entry_index, 0x0, control_word_);
  }
}
@Aragya1642 Aragya1642 added the question Further information is requested label Nov 4, 2024
@gsalinas
Copy link

gsalinas commented Nov 7, 2024

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.

@Aragya1642
Copy link
Author

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
question Further information is requested
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@gsalinas @Aragya1642