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

Add soem printouts and cleanup

Browse Source
This commit is contained in:
sandst_a
2024-02-16 12:01:58 +01:00
parent 2ffa6e36ff
commit 30a767fe59
2 changed files with 61 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
src/ecmcSafetyGroup.cpp
View File

@@ -344,43 +344,39 @@ void ecmcSafetyGroup::execute() {
setAxesStandstillStatus(0); // set output
throw std::out_of_range("Safety: Read rampdown cmd failed");
}
rampDownCmdOld_ = rampDownCmd_;
rampDownCmd_ = data == 0;
if(cfgDbgMode_ && rampDownCmdOld_ != rampDownCmd_) {
if(rampDownCmd_) {
printf("Safety %s: Ramp down cmd active\n",sName_);
} else {
printf("Safety %s: Ramp down cmd not active\n",sName_);
if(rampDownCmdOld_ != rampDownCmd_) {
resetPrintoutStatus();
if(cfgDbgMode_) {
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
setAxesSafetyInterlocks(rampDownCmd_);
// check if axes are standstill to safety PLC
axesAreStandstill_ = checkAxesStandstill();
axesAreStandstill_ = checkAxesStandstillAndDisableIfNeeded();
setAxesStandstillStatus(axesAreStandstill_);
// Disable
if(axesAreStandstill_ && rampDownCmd_) {
setAxesDisable();
} else {
printEnableStatus_ = true;
}
}
/*
//Needed functions in motion.h
int getAxisBusy(int axisIndex,
int *value);
int getAxisEncVelo(int axisIndex,
double *velo);
int getAxisTrajVelo(int axisIndex,
double *velo);
int getAxisEnabled(int axisIndex,
int *value);
int setAxisEnable(int axisIndex,
int value);
*/
void ecmcSafetyGroup::resetPrintoutStatus() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
(*saxis)->printEnableStat_ = 1;
}
}
void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
@@ -404,32 +400,54 @@ void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) {
bool ecmcSafetyGroup::checkAxesStandstill() {
bool standstill = 1;
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
standstill= standstill && checkAxisStandstill((*saxis)->axisIndex_,(*saxis)->veloLimit_);
standstill= standstill && checkAxisStandstill((*saxis));
}
return standstill;
}
// Check standstill axis
bool ecmcSafetyGroup::checkAxisStandstill(int axisId, double veloLimit) {
bool ecmcSafetyGroup::checkAxisStandstill(safetyAxis* axis) {
double velo = 1;
int err = getAxisTrajVelo(axisId, &velo);
int err = getAxisTrajVelo(axis->axisId_, &velo);
if(err) {
return 0;
}
return std::abs(velo) <= veloLimit;
return std::abs(velo) <= axis->veloLimit_;
}
// Set safety interlock
// Check standstill and disable
bool ecmcSafetyGroup::checkAxesStandstillAndDisableIfNeeded() {
bool standstill = 1;
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
standstill = standstill && checkAxisStandstillAndDisableIfNeeded((*saxis));
}
return standstill;
}
// Check standstill and disable
bool ecmcSafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) {
bool standstill = checkAxisStandstill(axis);
if( standstill && rampDownCmd_) {
setAxisEnable(axis->axisId_,0);
if(cfgDbgMode_ && axis->printEnableStat_) {
printf("Safety %s: Disabling axis %d\n",sName_,axis->axisId_);
axis->printEnableStat_ = 0;
}
}
return standstill;
}
// Disable all axes
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.");
}
setAxisEnable((*saxis)->axisIndex_,0);
setAxisEnable((*saxis)->axisId_,0);
}
if(printEnableStatus_ && cfgDbgMode_) {
printf("Safety %s: Axes are beeing disabled\n",sName_);
printf("Safety %s: All axes in group are beeing disabled\n",sName_);
}
printEnableStatus_ = 0;
}
@@ -440,7 +458,7 @@ void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) {
if(!*saxis) {
throw std::runtime_error( "Safety: Axis object NULL.");
}
setAxisEmergencyStopInterlock((*saxis)->axisIndex_,stop, 1000);
setAxisEmergencyStopInterlock((*saxis)->axisId_,stop);
}
}
@@ -460,9 +478,10 @@ void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs)
throw std::out_of_range("Safety: Invalid axis id");
}
axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs));
axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs));
axesCounter_++;
if(cfgDbgMode_) {
printf("Safety %s: Added axis %d to safety group\n",sName_,axisId);
}

18
src/ecmcSafetyGroup.h
View File

@@ -26,13 +26,16 @@ class safetyAxis {
safetyAxis(int axisIndex,
double veloLimit,
int standStillTimeMs) {
veloLimit_ = veloLimit;
axisIndex_ = axisIndex;
veloLimit_ = veloLimit;
axisId_ = axisIndex;
standStillTimeMs_ = standStillTimeMs;
printEnableStat_ = 1;
}
double veloLimit_;
int axisIndex_;
int standStillTimeMs_;
double veloLimit_;
int axisId_;
int standStillTimeMs_;
int printEnableStat_;
};
class ecmcSafetyGroup : public asynPortDriver {
@@ -68,7 +71,10 @@ class ecmcSafetyGroup : public asynPortDriver {
void setAxesDisable();
void setAxesStandstillStatus(int standstill);
bool checkAxesStandstill();
bool checkAxisStandstill(int axisId, double veloLimit);
bool checkAxisStandstill(safetyAxis* axis);
bool checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis);
bool checkAxesStandstillAndDisableIfNeeded();
void resetPrintoutStatus();
void parseConfigStr(const char *configStr);
void initAsyn();
double ecmcSampleRateHz_;