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..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 @@ -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 @@ -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