From e4522114fb149097a50cc951bf714b9e27c60c8a Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Mon, 20 May 2024 12:41:45 +0000 Subject: [PATCH] Added fault reset on startup --- .../src/generic_cia402_controller.cpp | 5 +- .../generic_ec_cia402_drive.hpp | 90 +++++++++---------- .../src/generic_ec_cia402_drive.cpp | 46 +++++++++- 3 files changed, 91 insertions(+), 50 deletions(-) diff --git a/ethercat_controllers/ethercat_generic_cia402_controller/src/generic_cia402_controller.cpp b/ethercat_controllers/ethercat_generic_cia402_controller/src/generic_cia402_controller.cpp index 33b9b398..9541b69c 100644 --- a/ethercat_controllers/ethercat_generic_cia402_controller/src/generic_cia402_controller.cpp +++ b/ethercat_controllers/ethercat_generic_cia402_controller/src/generic_cia402_controller.cpp @@ -211,7 +211,10 @@ namespace ethercat_controllers { command_interfaces_[3 * i].set_value(control); // control_word reset_homing_[i] = false; } - + if (reset_faults_[i]) { + std::cout << "Setting reset fault to 1 for dof: " << dof_names_[i] + << std::endl; + } command_interfaces_[3 * i + 2].set_value(reset_faults_[i]); // reset_fault reset_faults_[i] = false; } 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 c8107c8c..ececcdf9 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 @@ -28,55 +28,55 @@ #include "ethercat_generic_plugins/generic_ec_slave.hpp" #include "ethercat_generic_plugins/cia402_common_defs.hpp" -namespace ethercat_generic_plugins -{ +namespace ethercat_generic_plugins { -class EcCiA402Drive : public GenericEcSlave -{ -public: - EcCiA402Drive(); - virtual ~EcCiA402Drive(); - /** Returns true if drive has reached "operation enabled" state. - * The transition through the state machine is handled automatically. */ - bool initialized() const; + class EcCiA402Drive : public GenericEcSlave { + public: + EcCiA402Drive(); + virtual ~EcCiA402Drive(); + /** Returns true if drive has reached "operation enabled" state. + * The transition through the state machine is handled automatically. */ + bool initialized() const; - virtual void processData(size_t index, uint8_t * domain_address); + virtual void processData(size_t index, uint8_t *domain_address); - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface, - const std::string& for_name = ""); + virtual bool setupSlave( + std::unordered_map slave_paramters, + std::vector *state_interface, + std::vector *command_interface, + const std::string &for_name = "" + ); - int8_t mode_of_operation_display_ = 0; - int8_t mode_of_operation_ = -1; + int8_t mode_of_operation_display_ = 0; + int8_t mode_of_operation_ = -1; -protected: - uint32_t counter_ = 0; - uint16_t last_status_word_ = -1; - uint16_t status_word_ = 0; - uint16_t control_word_ = 0; - DeviceState last_state_ = STATE_START; - DeviceState state_ = STATE_START; - bool initialized_ = false; - bool auto_fault_reset_ = false; - 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(); + protected: + uint32_t counter_ = 0; + uint16_t last_status_word_ = -1; + uint16_t status_word_ = 0; + uint16_t control_word_ = 0; + DeviceState last_state_ = STATE_START; + DeviceState state_ = STATE_START; + bool initialized_ = false; + bool auto_fault_reset_ = false; + 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 */ - DeviceState deviceState(uint16_t status_word); - /** returns the control word that will take device from state to next desired state */ - uint16_t transition(DeviceState state, uint16_t control_word); - /** set up of the drive configuration from yaml node*/ - bool setup_from_config(YAML::Node drive_config); - /** set up of the drive configuration from yaml file*/ - bool setup_from_config_file(std::string config_file); -}; -} // namespace ethercat_generic_plugins + /** returns device state based upon the status_word */ + DeviceState deviceState(uint16_t status_word); + /** returns the control word that will take device from state to next + * desired state */ + uint16_t transition(DeviceState state, uint16_t control_word); + /** set up of the drive configuration from yaml node*/ + bool setup_from_config(YAML::Node drive_config); + /** set up of the drive configuration from yaml file*/ + bool setup_from_config_file(std::string config_file); + }; +} // namespace ethercat_generic_plugins -#endif // ETHERCAT_GENERIC_PLUGINS__GENERIC_EC_CIA402_DRIVE_HPP_ +#endif // ETHERCAT_GENERIC_PLUGINS__GENERIC_EC_CIA402_DRIVE_HPP_ 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 9f30c743..2d695413 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 @@ -67,7 +67,9 @@ namespace ethercat_generic_plugins { 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) { + } 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, @@ -84,7 +86,6 @@ namespace ethercat_generic_plugins { if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) { if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE && !std::isnan(last_position_)) { - pdo_channels_info_[index].default_value = pdo_channels_info_[index].factor * last_position_ + pdo_channels_info_[index].offset; @@ -104,7 +105,6 @@ namespace ethercat_generic_plugins { pdo_channels_info_[index].default_value = mode_of_operation_; } } - pdo_channels_info_[index].ec_update(domain_address); // get mode_of_operation_display_ @@ -194,6 +194,9 @@ namespace ethercat_generic_plugins { if (drive_config["auto_fault_reset"]) { auto_fault_reset_ = drive_config["auto_fault_reset"].as(); } + if (drive_config["fault_reset_on_start_up"]) { + fault_reset_ = drive_config["fault_reset_on_start_up"].as(); + } if (drive_config["auto_state_transitions"]) { auto_state_transitions_ = drive_config["auto_state_transitions"].as(); @@ -262,6 +265,9 @@ namespace ethercat_generic_plugins { case STATE_NEW_TARGET_RESET: return control_word & 0b11101111; case STATE_OPERATION_ENABLED: // -> GOOD + if (fault_reset_) { + fault_reset_ = false; + } return control_word; case STATE_QUICK_STOP_ACTIVE: // -> STATE_OPERATION_ENABLED return (control_word & 0b01111111) | 0b00001111; @@ -270,8 +276,40 @@ namespace ethercat_generic_plugins { case STATE_FAULT: // -> STATE_SWITCH_ON_DISABLED if (auto_fault_reset_ || fault_reset_) { fault_reset_ = false; - command_interface_ptr_->at(position_command_interface_index_) = + auto current_command = + command_interface_ptr_->at(position_command_interface_index_); + // command_interface_ptr_->at(position_command_interface_index_) = + // std::numeric_limits::quiet_NaN(); // Clear command + // interface + + last_position_ = std::numeric_limits::quiet_NaN(); // Clear command interface + std::cerr << "EcCiA402Drive: Setting last_position_ tot NAN for DRIVE " + << for_name_ << std::endl; + for (auto &channel : pdo_channels_info_) { + if (channel.index == CiA402D_RPDO_POSITION) { + channel.last_value = std::numeric_limits::quiet_NaN(); + channel.default_value = std::numeric_limits::quiet_NaN(); + std::cerr << "EcCiA402Drive: Setting last value tot NAN for DRIVE " + << for_name_ << std::endl; + } else if (channel.index == CiA402D_TPDO_POSITION) { + std::cerr << "EcCiA402Drive: Current position " + << channel.last_value << std::endl; + command_interface_ptr_->at(position_command_interface_index_) = + channel.last_value; + } + } + std::cerr + << "EcCiA402Drive: Setting command_interface_ptr_ tot current " + "for DRIVE " + << for_name_ << " Previous: " << current_command << std::endl; + std::cerr << "Now: " + << command_interface_ptr_->at( + position_command_interface_index_ + ) + << std::endl; + std::cerr << "EcCiA402Drive: RESETTING DRIVE " << for_name_ + << std::endl; return (control_word & 0b11111111) | 0b10000000; // automatic reset } else { return control_word;