motion/ecmc_plugin_safety
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added and tested velo limitation

Browse Source
This commit is contained in:
sandst_a
2024-03-13 08:55:22 +01:00
parent d6f04bfa4d
commit ad8dd2cc36
4 changed files with 52 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
src/ecmcSS1SafetyGroup.cpp
View File

@@ -35,12 +35,12 @@ extern DBBASE *pdbbase;
* - runtime_error
*/
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)
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)
: asynPortDriver(portName,
1, /* maxAddr */
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
@@ -103,11 +103,12 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
" Name: %s\n"
" I/O for rampdown command from saftey PLC: %s\n"
" I/O for axes standstill status: %s\n"
" I/O for velo limit command from safety PLC: %s\n"
" I/O for velo limit command from safety PLC: %s (%s)\n"
" STO delay [ms]: %d\n"
" Configuration string: %s\n",
sName_,sName_,sEcRampDownCmdNameOrg_,
sEcAxesStandStillStatOrg_,sEcLimitVeloOrg_,
limitMaxVeloEnable_ ? "enabled" : "disabled",
delayMs_,sConfig_);
}
}
@@ -177,14 +178,17 @@ void ecmcSS1SafetyGroup::validateCfgs() {
&bitStandStill_)) {
throw std::runtime_error( "Safety: Parse error: Data source for standstill status.");
}
int masterIdLimitVelo=-1;
if(parseEcPath(sEcLimitVeloOrg_,
&masterIdLimitVelo,
&slaveIdLimitVelo_,
aliasLimitVelo_,
&bitLimitVelo_)) {
throw std::runtime_error( "Safety: Parse error: Data source for limit velo.");
// init to masterIdRampDown if case !limitMaxVeloEnable_
int masterIdLimitVelo = masterIdStandStill;
if(limitMaxVeloEnable_) {
if(parseEcPath(sEcLimitVeloOrg_,
&masterIdLimitVelo,
&slaveIdLimitVelo_,
aliasLimitVelo_,
&bitLimitVelo_)) {
throw std::runtime_error( "Safety: Parse error: Data source for limit velo.");
}
}
if(masterIdStandStill != masterIdRampDown || masterIdLimitVelo != masterIdRampDown) {
@@ -298,17 +302,18 @@ void ecmcSS1SafetyGroup::connectToDataSources() {
};
// standstill
slave = ecMaster_->findSlave(slaveIdLimitVelo_);
if(!slave) {
throw std::runtime_error( "Safety: EtherCAT slave limit velo I/O NULL.");
// Limit velo
if( limitMaxVeloEnable_ ) {
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.");
};
}
ecEntryLimitVelo_ = slave->findEntry(aliasLimitVelo_);
if(!ecEntryLimitVelo_) {
throw std::runtime_error( "Safety: EtherCAT entry for limit velo I/O NULL.");
};
dataSourcesLinked_ = 1;
return;
@@ -593,28 +598,6 @@ void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) {
}
}
void ecmcSS1SafetyGroup::addAxis(int axisId,
double veloStandstillLimit,
int standStillTimeMs) {
if(!getAxisValid(axisId)) {
throw std::out_of_range("Safety: Invalid axis id");
}
axes_.push_back(new safetyAxis(axisId, veloStandstillLimit,
standStillTimeMs));
axesCounter_++;
if(cfgDbgMode_) {
printf("Safety %s: Added axis %d to safety group.\n"
" velo stand still limit = %lf,\n"
" standstill filter time = %d\n"
,sName_,axisId,veloStandstillLimit,standStillTimeMs);
}
return;
}
void ecmcSS1SafetyGroup::addAxis(int axisId,
double veloStandstillLimit,
int standStillTimeMs,
@@ -627,13 +610,14 @@ void ecmcSS1SafetyGroup::addAxis(int axisId,
axes_.push_back(new safetyAxis(axisId, veloStandstillLimit,
standStillTimeMs, veloMaxLimit));
axesCounter_++;
if(cfgDbgMode_) {
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);
" Velo stand still limit: %lf\n"
" velo max limit: %lf (%s)\n"
" standstill filter time: %d\n"
,sName_,axisId,veloStandstillLimit,veloMaxLimit,
veloMaxLimit>0 ? "enabled" : "disabled", standStillTimeMs);
}
return;