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

WIP

Browse Source
This commit is contained in:
sandst_a
2024-02-15 16:46:47 +01:00
parent 282a67c1a6
commit b091c6e247
2 changed files with 51 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/ecmcSafetyGroup.cpp
+47 -15
View File
@@ -80,26 +80,28 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name,
rampDownCmd_ = 0;
axesAreStandstill_ = 0;
rampDownCmdOld_ = 0;
axesAreStandstillOld_ = 0;
printEnableStatus_ = 1;
// Config defaults
cfgDbgMode_ = 0;
parseConfigStr(cfg_string);
initAsyn();
if(cfgDbgMode_) {
printf("Safety: Safety group created:\n"
printf("Safety %s: Safety group created:\n"
" Name: %s\n"
" I/O for rampdown command from saftey PLC: %s\n"
" I/O for axes standstill status: %s\n"
" STO delay [ms]: %d\n"
" Configuration string: %s\n",
sName_,sEcRampDownCmdNameOrg_,sEcAxesStandStillStatOrg_,
delayMs_,sConfig_);
sName_,sName_,sEcRampDownCmdNameOrg_,
sEcAxesStandStillStatOrg_,delayMs_,sConfig_);
}
}
ecmcSafetyGroup::~ecmcSafetyGroup() {
if(cfgDbgMode_) {
printf("Safety: Cleanup\n");
printf("Safety %s: Cleanup\n",sName_);
}
free(sName_);
@@ -178,24 +180,26 @@ void ecmcSafetyGroup::validateCfgs() {
}
if(cfgDbgMode_) {
printf("Safety: I/O link parsed:\n"
printf("Safety %s: I/O link parsed:\n"
" name: %s\n"
" masterid: %d\n"
" slaveid: %d\n"
" alias: %s\n"
" bit: %d\n",
sName_,
sEcRampDownCmdNameOrg_,
masterIdRampDown,
slaveIdRampDown_,
aliasRampDown_,
bitRampDown_);
printf("Safety: I/O link parsed:\n"
printf("Safety %s: I/O link parsed:\n"
" name: %s\n"
" masterid: %d\n"
" slaveid: %d\n"
" alias: %s\n"
" bit: %d\n",
sName_,
sEcAxesStandStillStatOrg_,
masterIdStandStill,
slaveIdStandStill_,
@@ -217,9 +221,10 @@ void ecmcSafetyGroup::stripBits() {
}
if(cfgDbgMode_) {
printf("Safety: I/O bit removed:\n"
printf("Safety %s: I/O bit removed:\n"
" before: %s\n"
" after: %s\n",
sName_,
sEcRampDownCmdNameOrg_,
sEcRampDownCmdNameStrip_);
}
@@ -233,9 +238,10 @@ void ecmcSafetyGroup::stripBits() {
}
if(cfgDbgMode_) {
printf("Safety: I/O bit removed:\n"
printf("Safety %s: I/O bit removed:\n"
" before: %s\n"
" after: %s\n",
sName_,
sEcAxesStandStillStatOrg_,
sEcAxesStandStillStatStrip_);
}
@@ -333,7 +339,7 @@ void ecmcSafetyGroup::execute() {
if(ecEntryRampDown_->readBit(bitRampDown_,
&data)) {
// Disable all axes
setAxesEnable(0); // disable
setAxesDisable(); // disable
setAxesSafetyInterlocks(0); // stop
setAxesStandstillStatus(0); // set output
throw std::out_of_range("Safety: Read rampdown cmd failed");
@@ -342,7 +348,11 @@ void ecmcSafetyGroup::execute() {
rampDownCmd_ = data == 0;
if(cfgDbgMode_ && rampDownCmdOld_ != rampDownCmd_) {
printf("Safety: Ramp down cmd changed state %d\n",rampDownCmd_);
if(rampDownCmd_) {
printf("Safety %s: Ramp down cmd active\n",sName_);
} else {
printf("Safety %s: Ramp down cmd not active\n",sName_);
}
}
// set safety interlock in ecmc
@@ -350,6 +360,14 @@ void ecmcSafetyGroup::execute() {
// check if axes are standstill to safety PLC
axesAreStandstill_ = checkAxesStandstill();
setAxesStandstillStatus(axesAreStandstill_);
// Disable
if(axesAreStandstill_ && rampDownCmd_) {
setAxesDisable();
} else {
printEnableStatus_ = true;
}
/*
//Needed functions in motion.h
int getAxisBusy(int axisIndex,
@@ -367,10 +385,19 @@ void ecmcSafetyGroup::execute() {
void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
if(axesAreStandstillOld_ != standstill) {
if(standstill) {
printf("Safety %s: Axes are not moving\n",sName_);
} else {
printf("Safety %s: Axes are moving\n",sName_);
}
}
if(ecEntryStandstill_->writeBit(bitStandStill_,
standstill > 0)) {
throw std::out_of_range("Safety: Read rampdown cmd failed");
}
axesAreStandstillOld_ = standstill;
}
// Check standstill axes
@@ -393,13 +420,18 @@ bool ecmcSafetyGroup::checkAxisStandstill(int axisId, double veloLimit) {
}
// Set safety interlock
void ecmcSafetyGroup::setAxesEnable(int enable) {
void ecmcSafetyGroup::setAxesDisable() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if(!*saxis) {
throw std::runtime_error( "Safety: Axis object NULL.");
throw std::runtime_error("Safety: Axis object NULL.");
}
setAxisEnable((*saxis)->axisIndex_,enable);
setAxisEnable((*saxis)->axisIndex_,0);
}
if(printEnableStatus_ && cfgDbgMode_) {
printf("Safety %s: Axes are beeing disabled\n",sName_);
}
printEnableStatus_ = 0;
}
// Set safety interlock
@@ -432,7 +464,7 @@ void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs)
axesCounter_++;
if(cfgDbgMode_) {
printf("Safety: Added axis %d to safety group \"%s\"\n",axisId,sName_);
printf("Safety %s: Added axis %d to safety group\n",sName_,axisId);
}
return;
src/ecmcSafetyGroup.h
+4 -2
View File
@@ -65,7 +65,7 @@ class ecmcSafetyGroup : public asynPortDriver {
void stripBits();
void connectToDataSources();
void setAxesSafetyInterlocks(int stop);
void setAxesEnable(int enable);
void setAxesDisable();
void setAxesStandstillStatus(int standstill);
bool checkAxesStandstill();
bool checkAxisStandstill(int axisId, double veloLimit);
@@ -101,7 +101,9 @@ class ecmcSafetyGroup : public asynPortDriver {
int slaveIdStandStill_;
int bitStandStill_;
int axesAreStandstill_;
int axesAreStandstillOld_;
int printEnableStatus_;
char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];