diff --git a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp index 8862c0a0..c5e633e7 100644 --- a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp +++ b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp @@ -83,7 +83,7 @@ class EthercatDriver : public hardware_interface::SystemInterface "ethercat_interface", "ethercat_interface::EcSlave"}; int control_frequency_; - ethercat_interface::EcMaster master_; + std::unique_ptr master_; std::mutex ec_mutex_; bool activated_; }; diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp index deea06b5..b1a73675 100644 --- a/ethercat_driver/src/ethercat_driver.cpp +++ b/ethercat_driver/src/ethercat_driver.cpp @@ -33,6 +33,16 @@ CallbackReturn EthercatDriver::on_init( const std::lock_guard lock(ec_mutex_); activated_ = false; + // Parse master_id from hardware parameters + int master_id = 0; // Default to 0 if not specified + if (info_.hardware_parameters.find("master_id") != info_.hardware_parameters.end()) { + master_id = std::stoi(info_.hardware_parameters["master_id"]); + } + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Master ID: %d", master_id); + + // Initialize master with the specified ID + master_ = std::make_unique(master_id); + hw_joint_states_.resize(info_.joints.size()); for (uint j = 0; j < info_.joints.size(); j++) { hw_joint_states_[j].resize( @@ -282,10 +292,10 @@ CallbackReturn EthercatDriver::on_activate( // start EC and wait until state operative - master_.setCtrlFrequency(control_frequency_); + master_->setCtrlFrequency(control_frequency_); for (auto i = 0ul; i < ec_modules_.size(); i++) { - master_.addSlave( + master_->addSlave( std::stod(ec_module_parameters_[i]["alias"]), std::stod(ec_module_parameters_[i]["position"]), ec_modules_[i].get()); @@ -295,7 +305,7 @@ CallbackReturn EthercatDriver::on_activate( for (auto i = 0ul; i < ec_modules_.size(); i++) { for (auto & sdo : ec_modules_[i]->sdo_config) { uint32_t abort_code; - int ret = master_.configSlaveSdo( + int ret = master_->configSlaveSdo( std::stod(ec_module_parameters_[i]["position"]), sdo, &abort_code @@ -311,7 +321,7 @@ CallbackReturn EthercatDriver::on_activate( } } - if (!master_.activate()) { + if (!master_->activate()) { RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), "Activate EcMaster failed"); return CallbackReturn::ERROR; } @@ -328,7 +338,7 @@ CallbackReturn EthercatDriver::on_activate( clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); // update EtherCAT bus - master_.update(); + master_->update(); RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!"); // check if operational @@ -340,7 +350,7 @@ CallbackReturn EthercatDriver::on_activate( running = false; } // calculate next shot. carry over nanoseconds into microseconds. - t.tv_nsec += master_.getInterval(); + t.tv_nsec += master_->getInterval(); while (t.tv_nsec >= 1000000000) { t.tv_nsec -= 1000000000; t.tv_sec++; @@ -363,8 +373,11 @@ CallbackReturn EthercatDriver::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait..."); + // Clean up all modules before stopping the master + ec_modules_.clear(); + // stop EC and disconnect - master_.stop(); + master_->stop(); RCLCPP_INFO( rclcpp::get_logger("EthercatDriver"), "System successfully stopped!"); @@ -379,7 +392,7 @@ hardware_interface::return_type EthercatDriver::read( // try to lock so we can avoid blocking the read/write loop on the lock. const std::unique_lock lock(ec_mutex_, std::try_to_lock); if (lock.owns_lock() && activated_) { - master_.readData(); + master_->readData(); } return hardware_interface::return_type::OK; } @@ -391,7 +404,7 @@ hardware_interface::return_type EthercatDriver::write( // try to lock so we can avoid blocking the read/write loop on the lock. const std::unique_lock lock(ec_mutex_, std::try_to_lock); if (lock.owns_lock() && activated_) { - master_.writeData(); + master_->writeData(); } return hardware_interface::return_type::OK; } diff --git a/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst b/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst index 51a4ce0a..6347c179 100644 --- a/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst +++ b/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst @@ -83,6 +83,12 @@ Each PDO Channel has the following configuration flags: - Data conversion factor/scale (:code:`type` : :code:`double`). * - :code:`offset` - Data offset term (:code:`type` : :code:`double`). + * - :code:`peak_upper_limit` + - Peak Upper limit (:code:`type` : :code:`double`) applied to command_interface as saturation before multiplying factor and adding offset. default value is :code:`6.2831853` (2*PI). + * - :code:`peak_lower_limit` + - Peak Lower limit (:code:`type` : :code:`double`) applied to command_interface as saturation before multiplying factor and adding offset. default value is :code:`-6.2831853` (-2*PI). + * - :code:`peak_limit_multiplier` + - Peak limit multiplier (:code:`type` : :code:`double`) applied to peak limits to scale them slightly below the actual actuator limits (specified in peak upper/lower limits). This is useful to avoid hitting the limits during operation. .. warning:: For each channel, tags :code:`index`, :code:`sub_index` and :code:`type` are **mandatory** even if the channel is not used in order to fill the data layout expected by the module. All other tags can remain unset. diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp index c4ba4aad..e2fb1b89 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp @@ -24,6 +24,7 @@ #define CiA402D_TPDO_POSITION ((uint16_t) 0x6064) #define CiA402D_TPDO_STATUSWORD ((uint16_t) 0x6041) #define CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY ((uint16_t) 0x6061) +#define CiA402D_TPDO_ERROR_CODE ((uint16_t) 0x603f) #include #include diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp index 07ec2a04..654396d8 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "yaml-cpp/yaml.h" #include "ethercat_interface/ec_slave.hpp" @@ -51,6 +52,9 @@ class EcCiA402Drive : public GenericEcSlave int8_t mode_of_operation_ = -1; protected: + // Global aggregated count of faulted instances (process-wide) + static std::atomic num_faulted_; + uint32_t counter_ = 0; uint16_t last_status_word_ = -1; uint16_t status_word_ = 0; @@ -60,10 +64,18 @@ class EcCiA402Drive : public GenericEcSlave bool initialized_ = false; bool auto_fault_reset_ = false; bool auto_state_transitions_ = true; + bool auto_state_transitions_cmd_ = false; bool fault_reset_ = false; int fault_reset_command_interface_index_ = -1; bool last_fault_reset_command_ = false; double last_position_ = std::numeric_limits::quiet_NaN(); + bool temperature_fault_detected_ = false; + uint16_t error_code_ = 0; + int auto_state_cmd_index_ = -1; + bool last_auto_state_cmd_ = false; + + // Quick stop behavior controls + bool exclude_from_global_quick_stop_ = false; // Do not propagate this drive's faults globally if true /** returns device state based upon the status_word */ DeviceState deviceState(uint16_t status_word); diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp index f06426db..d5f88a2a 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp @@ -21,211 +21,354 @@ namespace ethercat_generic_plugins { +std::atomic EcCiA402Drive::num_faulted_{0}; + EcCiA402Drive::EcCiA402Drive() -: GenericEcSlave() {} + : GenericEcSlave() +{ +} EcCiA402Drive::~EcCiA402Drive() {} -bool EcCiA402Drive::initialized() const {return initialized_;} +bool EcCiA402Drive::initialized() const { return initialized_; } -void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) +void EcCiA402Drive::processData(size_t index, uint8_t *domain_address) { - // Special case: ControlWord - if (pdo_channels_info_[index].index == CiA402D_RPDO_CONTROLWORD) { - if (is_operational_) { - if (fault_reset_command_interface_index_ >= 0) { - if (command_interface_ptr_->at(fault_reset_command_interface_index_) == 0) { - last_fault_reset_command_ = false; + // Special case: ControlWord + if (pdo_channels_info_[index].index == CiA402D_RPDO_CONTROLWORD) + { + if (is_operational_) + { + if (fault_reset_command_interface_index_ >= 0) + { + if (command_interface_ptr_->at(fault_reset_command_interface_index_) == 0) + { + last_fault_reset_command_ = false; + } + if (last_fault_reset_command_ == false && + command_interface_ptr_->at(fault_reset_command_interface_index_) != 0 && + !std::isnan(command_interface_ptr_->at(fault_reset_command_interface_index_))) + { + last_fault_reset_command_ = true; + fault_reset_ = true; + } + } + + if (auto_state_transitions_) + { + uint16_t cw = transition(state_, pdo_channels_info_[index].ec_read(domain_address)); + // If any instance is faulted, force quick-stop only on non-faulted drives + if (num_faulted_.load(std::memory_order_relaxed) > 0) { + if (error_code_ == 0) { + // Clear bit7 and bit0, set bit2 and bit1 (CiA-402 Shutdown) + cw = (cw & 0x7E) | 0x06; // same as (cw & 0b01111110) | 0b00000110 + // safe quick-stop path for healthy drives, this is writing 6 to the actuators. @babak + } + // If this drive is faulted (error_code_ != 0), do not override so reset can proceed + } + pdo_channels_info_[index].default_value = cw; + } + if (auto_state_cmd_index_ >= 0) + { + if (command_interface_ptr_->at(auto_state_cmd_index_) == 0) + { + last_auto_state_cmd_ = false; + } + if (last_auto_state_cmd_ == false && + command_interface_ptr_->at(auto_state_cmd_index_) != 0 && + !std::isnan(command_interface_ptr_->at(auto_state_cmd_index_))) + { + last_auto_state_cmd_ = true; + auto_state_transitions_cmd_ = true; + } + } + } + } + + // setup current position as default position + if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) + { + if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE) + { + pdo_channels_info_[index].default_value = + pdo_channels_info_[index].factor * last_position_ + pdo_channels_info_[index].offset; + } + pdo_channels_info_[index].override_command = + (mode_of_operation_display_ != ModeOfOperation::MODE_CYCLIC_SYNC_POSITION) ? true : false; + } + + // setup mode of operation + if (pdo_channels_info_[index].index == CiA402D_RPDO_MODE_OF_OPERATION) + { + if (mode_of_operation_ >= 0 && mode_of_operation_ <= 10) + { + pdo_channels_info_[index].default_value = mode_of_operation_; + } + } + + pdo_channels_info_[index].ec_update(domain_address); + + // get mode_of_operation_display_ + if (pdo_channels_info_[index].index == CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY) + { + mode_of_operation_display_ = pdo_channels_info_[index].last_value; + } + + if (pdo_channels_info_[index].index == CiA402D_TPDO_POSITION) + { + double current_position = pdo_channels_info_[index].last_value; + // Initialize last_position_ if it hasn't been set yet (first time) and the drive is operational + if (std::isnan(last_position_) && is_operational_) + { + last_position_ = current_position; + std::cout << "Position initialized: " << last_position_ << std::endl; } - if (last_fault_reset_command_ == false && - command_interface_ptr_->at(fault_reset_command_interface_index_) != 0 && - !std::isnan(command_interface_ptr_->at(fault_reset_command_interface_index_))) + } + + // Special case: StatusWord + if (pdo_channels_info_[index].index == CiA402D_TPDO_STATUSWORD) + { + status_word_ = pdo_channels_info_[index].last_value; + } + + // Special case: Error Code (0x603F) + if (pdo_channels_info_[index].index == CiA402D_TPDO_ERROR_CODE) + { + uint16_t new_error_code = pdo_channels_info_[index].last_value; + if (new_error_code != error_code_) { - last_fault_reset_command_ = true; - fault_reset_ = true; + uint16_t prev = error_code_; + error_code_ = new_error_code; + std::cout << "Error Code: " << std::hex << error_code_ << std::dec << std::endl; + + // Update registry counters + bool was_faulted = (prev != 0); + bool now_faulted = (error_code_ != 0); + if (!exclude_from_global_quick_stop_) { + if (!was_faulted && now_faulted) { + num_faulted_.fetch_add(1, std::memory_order_relaxed); + } else if (was_faulted && !now_faulted) { + num_faulted_.fetch_sub(1, std::memory_order_relaxed); + } + } + + // During startup, clear any stored temperature faults + if (counter_ < 100) + { // First 100 cycles = startup phase + if (error_code_ == 0x4110) + { + std::cout << "Startup: Clearing stored temperature fault error code" << std::endl; + temperature_fault_detected_ = false; + } + } + // After startup, check for temperature fault using specific error code 0x4110 + else if (error_code_ == 0x4110) + { + temperature_fault_detected_ = true; + std::cout << "TEMPERATURE FAULT DETECTED (Error Code 0x4110) - Disabling automatic fault reset" + << std::endl; + } + // Reset temperature fault flag when error code returns to 0 (no fault) + else if (error_code_ == 0x0) + { + if (temperature_fault_detected_) + { + temperature_fault_detected_ = false; + std::cout << "Temperature fault cleared - Automatic fault reset re-enabled" << std::endl; + } + } } - } - - if (auto_state_transitions_) { - pdo_channels_info_[index].default_value = transition( - state_, - pdo_channels_info_[index].ec_read(domain_address)); - } - } - } - - // setup current position as default position - if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) { - if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE) { - pdo_channels_info_[index].default_value = - pdo_channels_info_[index].factor * last_position_ + - pdo_channels_info_[index].offset; - } - pdo_channels_info_[index].override_command = - (mode_of_operation_display_ != ModeOfOperation::MODE_CYCLIC_SYNC_POSITION) ? true : false; - } - - // setup mode of operation - if (pdo_channels_info_[index].index == CiA402D_RPDO_MODE_OF_OPERATION) { - if (mode_of_operation_ >= 0 && mode_of_operation_ <= 10) { - pdo_channels_info_[index].default_value = mode_of_operation_; - } - } - - pdo_channels_info_[index].ec_update(domain_address); - - // get mode_of_operation_display_ - if (pdo_channels_info_[index].index == CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY) { - mode_of_operation_display_ = pdo_channels_info_[index].last_value; - } - - if (pdo_channels_info_[index].index == CiA402D_TPDO_POSITION) { - last_position_ = pdo_channels_info_[index].last_value; - } - - // Special case: StatusWord - if (pdo_channels_info_[index].index == CiA402D_TPDO_STATUSWORD) { - status_word_ = pdo_channels_info_[index].last_value; - } - - - // CHECK FOR STATE CHANGE - if (index == all_channels_.size() - 1) { // if last entry in domain - if (status_word_ != last_status_word_) { - state_ = deviceState(status_word_); - if (state_ != last_state_) { - std::cout << "STATE: " << DEVICE_STATE_STR.at(state_) - << " with status word :" << status_word_ << std::endl; - } - } - initialized_ = ((state_ == STATE_OPERATION_ENABLED) && - (last_state_ == STATE_OPERATION_ENABLED)) ? true : false; - - last_status_word_ = status_word_; - last_state_ = state_; - counter_++; - } + } + + // CHECK FOR STATE CHANGE + if (index == all_channels_.size() - 1) + { // if last entry in domain + if (status_word_ != last_status_word_) + { + state_ = deviceState(status_word_); + if (state_ != last_state_) + { + std::cout << "STATE: " << DEVICE_STATE_STR.at(state_) << " with status word :" << status_word_ + << std::endl; + } + } + initialized_ = ((state_ == STATE_OPERATION_ENABLED) && (last_state_ == STATE_OPERATION_ENABLED)) ? true : false; + + last_status_word_ = status_word_; + last_state_ = state_; + counter_++; + } } -bool EcCiA402Drive::setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) +bool EcCiA402Drive::setupSlave(std::unordered_map slave_paramters, + std::vector *state_interface, std::vector *command_interface) { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; + state_interface_ptr_ = state_interface; + command_interface_ptr_ = command_interface; + paramters_ = slave_paramters; - if (paramters_.find("slave_config") != paramters_.end()) { - if (!setup_from_config_file(paramters_["slave_config"])) { - return false; + if (paramters_.find("slave_config") != paramters_.end()) + { + if (!setup_from_config_file(paramters_["slave_config"])) + { + return false; + } + } + else + { + std::cerr << "EcCiA402Drive: failed to find 'slave_config' tag in URDF." << std::endl; + return false; } - } else { - std::cerr << "EcCiA402Drive: failed to find 'slave_config' tag in URDF." << std::endl; - return false; - } - setup_interface_mapping(); - setup_syncs(); + setup_interface_mapping(); + setup_syncs(); - if (paramters_.find("mode_of_operation") != paramters_.end()) { - mode_of_operation_ = std::stod(paramters_["mode_of_operation"]); - } + if (paramters_.find("mode_of_operation") != paramters_.end()) + { + mode_of_operation_ = std::stod(paramters_["mode_of_operation"]); + } - if (paramters_.find("command_interface/reset_fault") != paramters_.end()) { - fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]); - } + if (paramters_.find("command_interface/reset_fault") != paramters_.end()) + { + fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]); + } + + if (paramters_.find("command_interface/auto_state_transitions") != paramters_.end()) + { + auto_state_cmd_index_ = std::stoi(paramters_["command_interface/auto_state_transitions"]); + } - return true; + return true; } bool EcCiA402Drive::setup_from_config(YAML::Node drive_config) { - if (!GenericEcSlave::setup_from_config(drive_config)) {return false;} - // additional configuration parameters for CiA402 Drives - if (drive_config["auto_fault_reset"]) { - auto_fault_reset_ = drive_config["auto_fault_reset"].as(); - } - if (drive_config["auto_state_transitions"]) { - auto_state_transitions_ = drive_config["auto_state_transitions"].as(); - } - return true; + if (!GenericEcSlave::setup_from_config(drive_config)) + { + return false; + } + // additional configuration parameters for CiA402 Drives + if (drive_config["auto_fault_reset"]) + { + auto_fault_reset_ = drive_config["auto_fault_reset"].as(); + } + if (drive_config["auto_state_transitions"]) + { + auto_state_transitions_ = drive_config["auto_state_transitions"].as(); + } + if (drive_config["exclude_from_global_quick_stop"]) + { + exclude_from_global_quick_stop_ = drive_config["exclude_from_global_quick_stop"].as(); + } + return true; } bool EcCiA402Drive::setup_from_config_file(std::string config_file) { - // Read drive configuration from YAML file - try { - slave_config_ = YAML::LoadFile(config_file); - } catch (const YAML::ParserException & ex) { - std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl; - return false; - } catch (const YAML::BadFile & ex) { - std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl; - return false; - } - if (!setup_from_config(slave_config_)) { - return false; - } - return true; + // Read drive configuration from YAML file + try + { + slave_config_ = YAML::LoadFile(config_file); + } + catch (const YAML::ParserException &ex) + { + std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl; + return false; + } + catch (const YAML::BadFile &ex) + { + std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl; + return false; + } + if (!setup_from_config(slave_config_)) + { + return false; + } + return true; } /** returns device state based upon the status_word */ DeviceState EcCiA402Drive::deviceState(uint16_t status_word) { - if ((status_word & 0b01001111) == 0b00000000) { - return STATE_NOT_READY_TO_SWITCH_ON; - } else if ((status_word & 0b01001111) == 0b01000000) { - return STATE_SWITCH_ON_DISABLED; - } else if ((status_word & 0b01101111) == 0b00100001) { - return STATE_READY_TO_SWITCH_ON; - } else if ((status_word & 0b01101111) == 0b00100011) { - return STATE_SWITCH_ON; - } else if ((status_word & 0b01101111) == 0b00100111) { - return STATE_OPERATION_ENABLED; - } else if ((status_word & 0b01101111) == 0b00000111) { - return STATE_QUICK_STOP_ACTIVE; - } else if ((status_word & 0b01001111) == 0b00001111) { - return STATE_FAULT_REACTION_ACTIVE; - } else if ((status_word & 0b01001111) == 0b00001000) { - return STATE_FAULT; - } - return STATE_UNDEFINED; + if ((status_word & 0b01001111) == 0b00000000) + { + return STATE_NOT_READY_TO_SWITCH_ON; + } + else if ((status_word & 0b01001111) == 0b01000000) + { + return STATE_SWITCH_ON_DISABLED; + } + else if ((status_word & 0b01101111) == 0b00100001) + { + return STATE_READY_TO_SWITCH_ON; + } + else if ((status_word & 0b01101111) == 0b00100011) + { + return STATE_SWITCH_ON; + } + else if ((status_word & 0b01101111) == 0b00100111) + { + return STATE_OPERATION_ENABLED; + } + else if ((status_word & 0b01101111) == 0b00000111) + { + return STATE_QUICK_STOP_ACTIVE; + } + else if ((status_word & 0b01001111) == 0b00001111) + { + return STATE_FAULT_REACTION_ACTIVE; + } + else if ((status_word & 0b01001111) == 0b00001000) + { + return STATE_FAULT; + } + return STATE_UNDEFINED; } /** returns the control word that will take device from state to next desired state */ uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word) { - switch (state) { - case STATE_START: // -> STATE_NOT_READY_TO_SWITCH_ON (automatic) - return control_word; - case STATE_NOT_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON_DISABLED (automatic) - return control_word; - case STATE_SWITCH_ON_DISABLED: // -> STATE_READY_TO_SWITCH_ON - return (control_word & 0b01111110) | 0b00000110; - case STATE_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON - return (control_word & 0b01110111) | 0b00000111; - case STATE_SWITCH_ON: // -> STATE_OPERATION_ENABLED - return (control_word & 0b01111111) | 0b00001111; - case STATE_OPERATION_ENABLED: // -> GOOD - return control_word; - case STATE_QUICK_STOP_ACTIVE: // -> STATE_OPERATION_ENABLED - return (control_word & 0b01111111) | 0b00001111; - case STATE_FAULT_REACTION_ACTIVE: // -> STATE_FAULT (automatic) - return control_word; - case STATE_FAULT: // -> STATE_SWITCH_ON_DISABLED - if (auto_fault_reset_ || fault_reset_) { - fault_reset_ = false; - return (control_word & 0b11111111) | 0b10000000; // automatic reset - } else { + switch (state) + { + case STATE_START: // -> STATE_NOT_READY_TO_SWITCH_ON (automatic) + return control_word; + case STATE_NOT_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON_DISABLED (automatic) + return control_word; + case STATE_SWITCH_ON_DISABLED: // -> STATE_READY_TO_SWITCH_ON + return (control_word & 0b01111110) | 0b00000110; + case STATE_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON + return (control_word & 0b01110111) | 0b00000111; + case STATE_SWITCH_ON: // -> STATE_OPERATION_ENABLED + if (auto_state_transitions_cmd_) + { + return (control_word & 0b01111111) | 0b00001111; + } + else + { + return (control_word & 0b01110111) | 0b00000111; + } + case STATE_OPERATION_ENABLED: // -> GOOD return control_word; - } + case STATE_QUICK_STOP_ACTIVE: // -> STATE_OPERATION_ENABLED + return (control_word & 0b01111111) | 0b00001111; + case STATE_FAULT_REACTION_ACTIVE: // -> STATE_FAULT (automatic) + return control_word; + case STATE_FAULT: // -> STATE_SWITCH_ON_DISABLED + if ((auto_fault_reset_ || fault_reset_) && !temperature_fault_detected_) + { + fault_reset_ = false; + return (control_word & 0b11111111) | 0b10000000; // automatic reset + } + else + { + return control_word; + } default: - break; - } - return control_word; + break; + } + return control_word; } -} // namespace ethercat_generic_plugins +} // namespace ethercat_generic_plugins #include diff --git a/ethercat_interface/CMakeLists.txt b/ethercat_interface/CMakeLists.txt index 3843fe6e..d77f2e41 100644 --- a/ethercat_interface/CMakeLists.txt +++ b/ethercat_interface/CMakeLists.txt @@ -11,9 +11,12 @@ find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) # EtherLab -set(ETHERLAB_DIR /usr/local/etherlab) +if(DEFINED ENV{CONDA_PREFIX}) + set(ETHERLAB_DIR $ENV{CONDA_PREFIX}/usr/local/etherlab) +else() + set(ETHERLAB_DIR /usr/local/etherlab) +endif() set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - find_library(ETHERCAT_LIB ethercat HINTS ${ETHERLAB_DIR}/lib) ament_export_include_directories( diff --git a/ethercat_interface/include/ethercat_interface/ec_master.hpp b/ethercat_interface/include/ethercat_interface/ec_master.hpp index 26ae64f0..a3b1107e 100644 --- a/ethercat_interface/include/ethercat_interface/ec_master.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_master.hpp @@ -90,6 +90,8 @@ class EcMaster void readData(uint32_t domain = 0); void writeData(uint32_t domain = 0); + bool isValid() const { return master_ != NULL; } + private: /** true if running */ volatile bool running_ = false; diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp index f6981d93..4b302e0a 100644 --- a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp @@ -1,3 +1,6 @@ +// Modified by: Muhammad Awais Rafique (mraf@skl.vc), Peyman Karimi Eskandari (pkes@skl.vc) +// Modification: Added peak limits to specify the torque limits in the EtherCAT PDO channel manager before sending them to actuator drivers. The limits do not apply to CSP unless explicitly specified in the ethercat_config .yaml files because the default upper and lower limit is 2*pi and -2*pi which is the maximum position range. + // Copyright 2023 ICUBE Laboratory, University of Strasbourg // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -21,6 +24,7 @@ #include #include #include +#include #include "yaml-cpp/yaml.h" @@ -109,19 +113,24 @@ class EcPdoChannelManager void ec_update(uint8_t * domain_address) { // update state interface - if (pdo_type == TPDO) { + if (pdo_type == TPDO) + { ec_read(domain_address); - if (interface_index >= 0) { + if (interface_index >= 0) + { state_interface_ptr_->at(interface_index) = last_value; } - } else if (pdo_type == RPDO && allow_ec_write) { - if (interface_index >= 0 && - !std::isnan(command_interface_ptr_->at(interface_index)) && - !override_command) + } + else if (pdo_type == RPDO && allow_ec_write) + { + if (interface_index >= 0 && !std::isnan(command_interface_ptr_->at(interface_index)) && !override_command) + { + ec_write(domain_address, factor * std::clamp(command_interface_ptr_->at(interface_index), lower_limit, upper_limit) + offset); + } + else { - ec_write(domain_address, factor * command_interface_ptr_->at(interface_index) + offset); - } else { - if (!std::isnan(default_value)) { + if (!std::isnan(default_value)) + { ec_write(domain_address, default_value); } } @@ -179,6 +188,21 @@ class EcPdoChannelManager data_mask = channel_config["mask"].as(); } + if (channel_config["peak_limit_multiplier"]) { + limit_multiplier = std::abs(channel_config["peak_limit_multiplier"].as()); + } + + // lower limit + if (channel_config["peak_lower_limit"]) { + lower_limit = limit_multiplier * channel_config["peak_lower_limit"].as(); + } + // upper limit + if (channel_config["peak_upper_limit"]) { + upper_limit = limit_multiplier * channel_config["peak_upper_limit"].as(); + } + + + return true; } @@ -217,6 +241,9 @@ class EcPdoChannelManager double factor = 1; double offset = 0; + double upper_limit = 6.2831853; // Set to 2*PI by default, all torque limits are lower than this value + double lower_limit = -6.2831853; // Set to -2*PI by default, all torque limits are higher than this value + double limit_multiplier = 0.95; // Used to scale the peak limits, only applies if peak limits are set in the yaml file, otherwise the default limits are used. private: std::vector * command_interface_ptr_; std::vector * state_interface_ptr_; diff --git a/ethercat_interface/include/ethercat_interface/ec_slave.hpp b/ethercat_interface/include/ethercat_interface/ec_slave.hpp index da88a826..99104454 100644 --- a/ethercat_interface/include/ethercat_interface/ec_slave.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_slave.hpp @@ -41,6 +41,7 @@ class EcSlave virtual const ec_sync_info_t * syncs() {return NULL;} virtual bool initialized() {return true;} virtual void set_state_is_operational(bool value) {is_operational_ = value;} + bool is_operational() const { return is_operational_; } /** Assign activate DC synchronization. return activate word*/ virtual int assign_activate_dc_sync() {return 0x00;} /** number of elements in the syncs array. */ diff --git a/ethercat_interface/src/ec_master.cpp b/ethercat_interface/src/ec_master.cpp index db31e680..390d99ca 100644 --- a/ethercat_interface/src/ec_master.cpp +++ b/ethercat_interface/src/ec_master.cpp @@ -74,6 +74,10 @@ EcMaster::~EcMaster() delete domain.second; } } + if (master_ != NULL) { + ecrt_release_master(master_); + master_ = NULL; + } } void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) diff --git a/ethercat_manager/CMakeLists.txt b/ethercat_manager/CMakeLists.txt index eef818f6..29dc4db1 100644 --- a/ethercat_manager/CMakeLists.txt +++ b/ethercat_manager/CMakeLists.txt @@ -6,9 +6,12 @@ find_package(rclcpp REQUIRED) find_package(ethercat_msgs REQUIRED) # EtherLab -set(ETHERLAB_DIR /usr/local/etherlab) +if(DEFINED ENV{CONDA_PREFIX}) + set(ETHERLAB_DIR $ENV{CONDA_PREFIX}/usr/local/etherlab) +else() + set(ETHERLAB_DIR /usr/local/etherlab) +endif() set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - find_library(ETHERCAT_LIB ethercat HINTS ${ETHERLAB_DIR}/lib) ament_export_include_directories( diff --git a/ethercat_manager/include/ethercat_manager/ec_master_async.hpp b/ethercat_manager/include/ethercat_manager/ec_master_async.hpp index d1a8a86a..f603322e 100644 --- a/ethercat_manager/include/ethercat_manager/ec_master_async.hpp +++ b/ethercat_manager/include/ethercat_manager/ec_master_async.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include diff --git a/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp b/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp index 00294775..b3e4fb4e 100644 --- a/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp +++ b/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp @@ -20,7 +20,7 @@ #define EC_IOR(nr, type) _IOR(EC_IOCTL_TYPE, nr, type) #define EC_IOW(nr, type) _IOW(EC_IOCTL_TYPE, nr, type) #define EC_IOWR(nr, type) _IOWR(EC_IOCTL_TYPE, nr, type) -#define EC_IOCTL_VERSION_MAGIC 31 +#define EC_IOCTL_VERSION_MAGIC 37 // ioctl() version magic is matching: /dev/EtherCAT0: 37, ethercat tool: 37 #define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t) #define EC_IOCTL_SLAVE_SDO_UPLOAD EC_IOWR(0x0e, ec_ioctl_slave_sdo_upload_t) #define EC_IOCTL_SLAVE_SDO_DOWNLOAD EC_IOWR(0x0f, ec_ioctl_slave_sdo_download_t) diff --git a/ethercat_manager/src/ethercat_sdo_srv_server.cpp b/ethercat_manager/src/ethercat_sdo_srv_server.cpp index 24a70132..72154626 100644 --- a/ethercat_manager/src/ethercat_sdo_srv_server.cpp +++ b/ethercat_manager/src/ethercat_sdo_srv_server.cpp @@ -54,7 +54,7 @@ void upload( EcMasterAsync master(request->master_id); try { - master.open(EcMasterAsync::Read); + master.open(EcMasterAsync::ReadWrite); } catch (MasterException & e) { return_stream << e.what(); response->success = false;