diff --git a/src/ecmcSS1SafetyGroup.cpp b/src/ecmcSS1SafetyGroup.cpp index ef0f24a..ebaf3d2 100644 --- a/src/ecmcSS1SafetyGroup.cpp +++ b/src/ecmcSS1SafetyGroup.cpp @@ -37,6 +37,7 @@ extern DBBASE *pdbbase; ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name, const char *ec_rampdown_cmd, const char *ec_standstill_status, + const char *ec_limit_velo, int time_delay_ms, const char *cfg_string, char* portName) @@ -55,11 +56,14 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name, 0, /* Default priority */ 0) /* Default stack size */ { - sName_ = strdup(name); - sEcRampDownCmdNameOrg_ = strdup(ec_rampdown_cmd); - sEcAxesStandStillStatOrg_= strdup(ec_standstill_status); - sEcRampDownCmdNameStrip_ = strdup(ec_rampdown_cmd); + sName_ = strdup(name); + sEcRampDownCmdNameOrg_ = strdup(ec_rampdown_cmd); + sEcRampDownCmdNameStrip_ = strdup(ec_rampdown_cmd); + sEcAxesStandStillStatOrg_ = strdup(ec_standstill_status); sEcAxesStandStillStatStrip_ = strdup(ec_standstill_status); + sEcReduceVeloOrg_ = strdup(ec_limit_velo); + sEcReduceVeloStrip_ = strdup(ec_limit_velo); + sConfig_ = strdup(cfg_string); delayMs_ = time_delay_ms; memset(&status_,0,sizeof(status_)); @@ -71,18 +75,22 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name, ecMaster_ = NULL; ecEntryRampDown_ = NULL; ecEntryStandstill_ = NULL; + ecEntryLimitVelo_ = NULL; masterId_ = 0; slaveIdRampDown_ = 0; bitRampDown_ = 0; slaveIdStandStill_ = 0; bitStandStill_ = 0; - aliasRampDown_[0] = 0; + aliasRampDown_[0] = 0; aliasStandStill_[0] = 0; + aliasLimitVelo_[0] = 0; rampDownCmd_ = 0; axesAreStandstill_ = 0; rampDownCmdOld_ = 0; axesAreStandstillOld_ = 0; printEnableStatus_ = 1; + limitVeloCmdOld_ = 0; + limitVeloCmd_ = 0; // Config defaults cfgDbgMode_ = 0; @@ -112,6 +120,8 @@ ecmcSS1SafetyGroup::~ecmcSS1SafetyGroup() { free(sEcRampDownCmdNameStrip_); free(sEcAxesStandStillStatStrip_); free(sConfig_); + free(sEcReduceVeloOrg_); + free(sEcReduceVeloStrip_); } void ecmcSS1SafetyGroup::parseConfigStr(const char *configStr) { @@ -168,7 +178,16 @@ void ecmcSS1SafetyGroup::validateCfgs() { throw std::runtime_error( "Safety: Parse error: Data source for standstill status."); } - if(masterIdStandStill != masterIdRampDown ) { + int masterIdLimitVelo=-1; + if(parseEcPath(sEcReduceVeloOrg_, + &masterIdLimitVelo, + &slaveIdLimitVelo_, + aliasLimitVelo_, + &bitLimitVelo_)) { + throw std::runtime_error( "Safety: Parse error: Data source for limit velo."); + } + + if(masterIdStandStill != masterIdRampDown || sEcReduceVeloOrg_ != masterIdRampDown) { throw std::runtime_error( "Safety: Parse error: Master id for datasources different."); } masterId_ = masterIdStandStill; @@ -180,6 +199,10 @@ void ecmcSS1SafetyGroup::validateCfgs() { if(bitStandStill_ < 0) { throw std::runtime_error( "Safety: Parse error: Standstill status, bit invalid."); } + + if(bitLimitVelo_ < 0) { + throw std::runtime_error( "Safety: Parse error: Limit velo, bit invalid."); + } } void ecmcSS1SafetyGroup::validateAxes() { @@ -255,30 +278,24 @@ void ecmcSS1SafetyGroup::connectToDataSources() { ecEntryStandstill_ = slave->findEntry(aliasStandStill_); if(!ecEntryStandstill_) { - throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL."); + throw std::runtime_error( "Safety: EtherCAT entry for standstill I/O NULL."); }; - + + + // standstill + slave = ecMaster_->findSlave(slaveIdLimitVelo_); + if(!slave) { + throw std::runtime_error( "Safety: EtherCAT slave limit velo I/O NULL."); + } + + ecEntryLimitVelo_ = slave->findEntry(aliasLimitVelo_); + if(!ecEntryLimitVelo_) { + throw std::runtime_error( "Safety: EtherCAT entry for limit velo I/O NULL."); + }; + dataSourcesLinked_ = 1; return; - - //// Get dataItem for rampdown command - //dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_); - //if(!dataItemRampDownCmd_) { - // throw std::runtime_error( "Safety: Data item for ramp down command NULL."); - //} - // - //// Get dataItem for axes standstill status - //dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_); - //if(!dataItemStandStillStat_) { - // throw std::runtime_error( "Safety: Data item for axes standstill status NULL."); - //} - // - //if(cfgDbgMode_) { - // printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_); - //} - // - //dataSourcesLinked_ = 1; } void ecmcSS1SafetyGroup::refreshAsyn() { @@ -318,9 +335,8 @@ std::string ecmcSS1SafetyGroup::to_string(int value) { return os.str(); } -// Executed by ecmc rt thread. -void ecmcSS1SafetyGroup::execute() { - +// Ramp down and disable if safety interlock +void ecmcSS1SafetyGroup::exeRampDown() { uint64_t data = 0; // Read ramp down command from safety plc if(ecEntryRampDown_->readBit(bitRampDown_, @@ -333,7 +349,7 @@ void ecmcSS1SafetyGroup::execute() { } rampDownCmdOld_ = rampDownCmd_; - rampDownCmd_ = data == 0; + rampDownCmd_ = data == 0; if(rampDownCmdOld_ != rampDownCmd_) { @@ -353,6 +369,7 @@ void ecmcSS1SafetyGroup::execute() { // set safety interlock in ecmc setAxesSafetyInterlocks(rampDownCmd_); + // check if axes are standstill to safety PLC axesAreStandstill_ = checkAxesStandstillAndDisableIfNeeded(); setAxesStandstillStatus(axesAreStandstill_); @@ -365,6 +382,46 @@ void ecmcSS1SafetyGroup::execute() { } } +// Limit velo if needed +void ecmcSS1SafetyGroup::exeLimitVelo() { + uint64_t data = 0; + // Read ramp down command from safety plc + if(ecEntryLimitVelo_->readBit(bitLimitVelo_, + &data)) { + // Disable all axes + setAxesDisable(); // disable + setAxesSafetyInterlocks(0); // stop + setAxesStandstillStatus(0); // set output + throw std::out_of_range("Safety: Read limit velo cmd failed"); + } + + limitVeloCmdOld_ = limitVeloCmd_; + limitVeloCmd_ = data == 0; + + if(limitVeloCmdOld_ != limitVeloCmd_) { + // Update asyn status wd + status_.limitVeloCmdActive = limitVeloCmd_; + refreshAsyn(); + + resetPrintoutStatus(); + if(cfgDbgMode_) { + if(rampDownCmd_) { + printf("Safety %s: Limit velo cmd active\n",sName_); + } else { + printf("Safety %s: Limit velo cmd not active\n",sName_); + } + } + } + + if() +} + +// Executed by ecmc rt thread. +void ecmcSS1SafetyGroup::execute() { + exeRampDown(); + exeLimitVelo(); +} + void ecmcSS1SafetyGroup::resetPrintoutStatus() { for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { (*saxis)->printEnableStat_ = 1; @@ -416,7 +473,7 @@ bool ecmcSS1SafetyGroup::checkAxisStandstill(safetyAxis* axis) { return 0; } - return std::abs(traj) <= axis->veloLimit_ && std::abs(enc) <= axis->veloLimit_; + return std::abs(traj) <= axis->veloStandstillLimit_ && std::abs(enc) <= axis->veloStandstillLimit_; } // Check standstill and disable @@ -466,19 +523,22 @@ void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) { } } -void ecmcSS1SafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) { +void ecmcSS1SafetyGroup::addAxis(int axisId, double veloStandstillLimit,int standStillTimeMs,double veloMaxLimit) { if(!getAxisValid(axisId)) { throw std::out_of_range("Safety: Invalid axis id"); } - axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs)); + axes_.push_back(new safetyAxis(axisId, veloStandstillLimit, + standStillTimeMs, veloMaxLimit)); axesCounter_++; - if(cfgDbgMode_) { - printf("Safety %s: Added axis %d to safety group (velo limit = %lf, standstill filter : %d)\n" - ,sName_,axisId,veloLimit,standStillTimeMs); + printf("Safety %s: Added axis %d to safety group.\n" + " velo stand still limit = %lf,\n" + " velo max limit = %lf,\n" + " standstill filter time = %d\n" + ,sName_,axisId,veloStandstillLimit,veloMaxLimit,standStillTimeMs); } return; diff --git a/src/ecmcSS1SafetyGroup.h b/src/ecmcSS1SafetyGroup.h index 3a43a2c..3f510ab 100644 --- a/src/ecmcSS1SafetyGroup.h +++ b/src/ecmcSS1SafetyGroup.h @@ -25,21 +25,25 @@ typedef struct { bool error : 1; bool rampDownCmdActive : 1; bool axesAtStandstill : 1; - int dummy : 29; + bool limitVeloCmdActive : 1; + int dummy : 28; } ecmcSafetyStatusWd; class safetyAxis { public: safetyAxis(int axisId, - double veloLimit, - int standStillTimeMs) { - veloLimit_ = veloLimit; - axisId_ = axisId; - standStillTimeMs_ = standStillTimeMs; - printEnableStat_ = 1; + double velostandstillLimit, + int standStillTimeMs, + double veloMaxLimit) { + veloStandstillLimit_ = velostandstillLimit; + axisId_ = axisId; + standStillTimeMs_ = standStillTimeMs; + veloMaxLimit_ = veloMaxLimit; + printEnableStat_ = 1; } - double veloLimit_; + double veloStandstillLimit_; + double veloMaxLimit_; // disable with -1 int axisId_; int standStillTimeMs_; int printEnableStat_; @@ -56,6 +60,7 @@ class ecmcSS1SafetyGroup : public asynPortDriver { ecmcSS1SafetyGroup(const char *name, const char *ec_rampdown_cmd, const char *ec_standstill_status, + const char *ec_limit_velo, int time_delay_ms, const char *cfg_string, char* portName); @@ -84,12 +89,17 @@ class ecmcSS1SafetyGroup : public asynPortDriver { void parseConfigStr(const char *configStr); void initAsyn(); void refreshAsyn(); + void addMaxVeloLimit(const char *ec_rampdown_cmd); + void exeRampDown(); + void exeLimitVelo(); double ecmcSampleRateHz_; int dataSourcesLinked_; // To avoid link several times int objectId_; // Unique object id int cycleCounter_; int rampDownCmd_; int rampDownCmdOld_; + int limitVeloCmdOld_; + int limitVeloCmd_; // Config options int cfgDbgMode_; // Config: allow dbg printouts @@ -107,6 +117,8 @@ class ecmcSS1SafetyGroup : public asynPortDriver { char* sEcAxesStandStillStatOrg_; char* sEcRampDownCmdNameStrip_; char* sEcAxesStandStillStatStrip_; + char* sEcReduceVeloOrg_; + char* sEcReduceVeloStrip_; char* sConfig_; int delayMs_; @@ -115,16 +127,21 @@ class ecmcSS1SafetyGroup : public asynPortDriver { int bitRampDown_; int slaveIdStandStill_; int bitStandStill_; + int slaveIdLimitVelo_; + int bitLimitVelo_; + int axesAreStandstill_; int axesAreStandstillOld_; int printEnableStatus_; char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; - + char aliasLimitVelo_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; + ecmcEc *ecMaster_; ecmcEcEntry *ecEntryRampDown_; ecmcEcEntry *ecEntryStandstill_; + ecmcEcEntry *ecEntryLimitVelo_; static std::string to_string(int value); }; diff --git a/startup.cmd b/startup.cmd index 210de07..e4e9297 100644 --- a/startup.cmd +++ b/startup.cmd @@ -13,7 +13,7 @@ #- ################################################################################# -#- Print discalimer +#- Print disclaimer # # !!!!!!!!!!!!! IMPORTANT !!!!!!!!! # This plugin has _NO_ safety rated functionalities.