From 02701c98ecf16a69d39700af45c9006c4ca14a26 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Tue, 12 Dec 2023 12:58:40 +0000 Subject: [PATCH 1/2] Added basic Profiled Position mode --- .../cia402_common_defs.hpp | 8 +++- .../generic_ec_cia402_drive.hpp | 2 + .../src/generic_ec_cia402_drive.cpp | 40 ++++++++++++++++--- 3 files changed, 42 insertions(+), 8 deletions(-) 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..7d89bd09 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 @@ -39,7 +39,9 @@ enum DeviceState STATE_OPERATION_ENABLED, STATE_QUICK_STOP_ACTIVE, STATE_FAULT_REACTION_ACTIVE, - STATE_FAULT + STATE_FAULT, + STATE_NEW_TARGET, + STATE_NEW_TARGET_RESET }; enum ModeOfOperation @@ -65,7 +67,9 @@ const std::map DEVICE_STATE_STR = { {STATE_QUICK_STOP_ACTIVE, "Quick Stop Active"}, {STATE_FAULT_REACTION_ACTIVE, "Fault Reaction Active"}, {STATE_FAULT, "Fault"}, - {STATE_UNDEFINED, "Undefined State"} + {STATE_UNDEFINED, "Undefined State"}, + {STATE_NEW_TARGET, "STATE_NEW_TARGET"}, + {STATE_NEW_TARGET_RESET, "STATE_NEW_TARGET_RESET"} }; #endif // ETHERCAT_GENERIC_PLUGINS__CIA402_COMMON_DEFS_HPP_ 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..7703dd0c 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 @@ -62,7 +62,9 @@ class EcCiA402Drive : public GenericEcSlave bool auto_state_transitions_ = true; bool fault_reset_ = false; int fault_reset_command_interface_index_ = -1; + int position_command_interface_index_ = -1; bool last_fault_reset_command_ = false; + double previous_target_ = -1; double last_position_ = std::numeric_limits::quiet_NaN(); /** returns device state based upon the 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..e0cd4880 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 @@ -48,7 +48,27 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) if (auto_state_transitions_) { pdo_channels_info_[index].default_value = transition( state_, - pdo_channels_info_[index].ec_read(domain_address)); + pdo_channels_info_[index].ec_read(domain_address) + ); + if(mode_of_operation_ == ModeOfOperation::MODE_PROFILED_POSITION) { + // Send New Target triggers if target position changes + if(state_ == STATE_OPERATION_ENABLED && position_command_interface_index_ >= 0) { + uint16_t control_word = pdo_channels_info_[index].default_value; + double target_position = command_interface_ptr_->at(position_command_interface_index_); + if(!std::isnan(target_position) && target_position == previous_target_ && (control_word & 0b00010000) == 0b00000000) { + pdo_channels_info_[index].default_value = transition( + STATE_NEW_TARGET, + pdo_channels_info_[index].ec_read(domain_address) + ); + } else if((!std::isnan(target_position) || !std::isnan(previous_target_)) && previous_target_ != target_position) { + previous_target_ = target_position; + pdo_channels_info_[index].default_value = transition( + STATE_NEW_TARGET_RESET, + pdo_channels_info_[index].ec_read(domain_address) + ); + } + } + } } } } @@ -61,7 +81,7 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) pdo_channels_info_[index].offset; } pdo_channels_info_[index].override_command = - (mode_of_operation_display_ != ModeOfOperation::MODE_CYCLIC_SYNC_POSITION) ? true : false; + (mode_of_operation_display_ != ModeOfOperation::MODE_CYCLIC_SYNC_POSITION && mode_of_operation_display_ != ModeOfOperation::MODE_PROFILED_POSITION) ? true : false; } // setup mode of operation @@ -92,10 +112,10 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) 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; - } + // if (state_ != last_state_) { + // std::cout << "STATE CIA: " << DEVICE_STATE_STR.at(state_) + // << " with status word :" << status_word_ << std::endl; + // } } initialized_ = ((state_ == STATE_OPERATION_ENABLED) && (last_state_ == STATE_OPERATION_ENABLED)) ? true : false; @@ -135,6 +155,10 @@ bool EcCiA402Drive::setupSlave( fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]); } + if (paramters_.find("command_interface/position") != paramters_.end()) { + position_command_interface_index_ = std::stoi(paramters_["command_interface/position"]); + } + return true; } @@ -206,6 +230,10 @@ uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word) return (control_word & 0b01110111) | 0b00000111; case STATE_SWITCH_ON: // -> STATE_OPERATION_ENABLED return (control_word & 0b01111111) | 0b00001111; + case STATE_NEW_TARGET: + return control_word | 0b00010000; + case STATE_NEW_TARGET_RESET: + return control_word & 0b11101111; case STATE_OPERATION_ENABLED: // -> GOOD return control_word; case STATE_QUICK_STOP_ACTIVE: // -> STATE_OPERATION_ENABLED From 3a82c970c0f404cdbb5c2bb140e4fc2326d4f222 Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Tue, 12 Dec 2023 13:02:52 +0000 Subject: [PATCH 2/2] Reenabled logging --- .../src/generic_ec_cia402_drive.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 e0cd4880..39ba4629 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 @@ -112,10 +112,10 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) 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 CIA: " << DEVICE_STATE_STR.at(state_) - // << " with status word :" << status_word_ << std::endl; - // } + 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;