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

Start add velo lim cmd

Browse Source
This commit is contained in:
sandst_a
2024-03-12 11:02:50 +01:00
parent a5c2b5a4b3
commit 3272fd62ed
3 changed files with 123 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
src/ecmcSS1SafetyGroup.cpp
View File

@@ -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<safetyAxis*>::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;

35
src/ecmcSS1SafetyGroup.h
View File

@@ -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);
};

2
startup.cmd
View File

@@ -13,7 +13,7 @@
#-
#################################################################################
#- Print discalimer
#- Print disclaimer
#
# !!!!!!!!!!!!! IMPORTANT !!!!!!!!!
# This plugin has _NO_ safety rated functionalities.