Skip to content

Commit

Permalink
Added fault reset on startup
Browse files Browse the repository at this point in the history
  • Loading branch information
@JensVanhooydonck
JensVanhooydonck committed May 20, 2024
1 parent 4cb009a commit e452211
Show file tree
Hide file tree
Showing 3 changed files with 91 additions and 50 deletions.
5 changes: 4 additions & 1 deletion ethercat_controllers/ethercat_generic_cia402_controller/src/generic_cia402_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
90 changes: 45 additions & 45 deletions ...thercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string, std::string> slave_paramters,
std::vector<double> * state_interface,
std::vector<double> * command_interface,
const std::string& for_name = "");
virtual bool setupSlave(
std::unordered_map<std::string, std::string> slave_paramters,
std::vector<double> *state_interface,
std::vector<double> *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<double>::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<double>::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_
46 changes: 42 additions & 4 deletions ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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;
Expand All @@ -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_
Expand Down Expand Up @@ -194,6 +194,9 @@ namespace ethercat_generic_plugins {
if (drive_config["auto_fault_reset"]) {
auto_fault_reset_ = drive_config["auto_fault_reset"].as<bool>();
}
if (drive_config["fault_reset_on_start_up"]) {
fault_reset_ = drive_config["fault_reset_on_start_up"].as<bool>();
}
if (drive_config["auto_state_transitions"]) {
auto_state_transitions_ =
drive_config["auto_state_transitions"].as<bool>();
Expand Down Expand Up @@ -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;
Expand All @@ -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<double>::quiet_NaN(); // Clear command
// interface

last_position_ =
std::numeric_limits<double>::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<double>::quiet_NaN();
channel.default_value = std::numeric_limits<double>::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;
Expand Down

0 comments on commit e452211

Please sign in to comment.