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
2025-02-11 16:42:26 +01:00
parent 1fb017ac64 18e4821a83
commit 9042a39a1b
6 changed files with 37 additions and 208 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
GNUmakefile
View File

@@ -2,6 +2,9 @@ include /ioc/tools/driver.makefile
MODULE = ecmc_plugin_safety
# "Transfer" module name to plugin
USR_CFLAGS +=-DECMC_PLUGIN_MODULE_NAME=${MODULE}
BUILDCLASSES = Linux
ARCH_FILTER = deb10%

19
iocsh/test_4ax_box.script
View File

@@ -53,23 +53,22 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml"
require ecmc_plugin_safety
# Create SS1 group
#- EC_RAMP_DOWN : Ethercat entry for ramp down command, input to ecmc (command from safety PLC/system)
#- EC_AXES_REST : Ethercat entry for signaling that all axes in group are at rest, output from ecmc (feedback to safety PLC/system)
#- EC_AXES_MAX_VELO : Ethercat entry for reducing velocity, input to ecmc (command from safety PLC/system)
#- DELAY_MS : Time between rampdown command and STO
epicsEnvSet(EC_RAMP_DOWN,"ec${ECMC_EC_MASTER_ID}.s${BI_SLAVE}.binaryInput08.0")
epicsEnvSet(EC_AXES_REST,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.binaryOutput07.0")
epicsEnvSet(EC_AXES_MAX_VELO,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.ONE.0")
#- EC_RAMP_DOWN_CMD : EtherCAT entry for ramp down command, input to ecmc (command from safety PLC/system)
#- EC_REST_STAT : EtherCAT entry for signaling that all axes in group are at rest, output from ecmc (feedback to safety PLC/system)
#- EC_RED_VEL_CMD : EtherCAT entry for reducing velocity, input to ecmc (command from safety PLC/system)
#- DELAY_MS : Time between rampdown command and STO
epicsEnvSet(EC_RAMP_DOWN_CMD,"ec${ECMC_EC_MASTER_ID}.s${BI_SLAVE}.binaryInput08.0")
epicsEnvSet(EC_REST_STAT,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.binaryOutput07.0")
epicsEnvSet(EC_RED_VEL_CMD,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.ONE.0")
epicsEnvSet(SAFETY_TIMEOUT,500)
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN=${EC_RAMP_DOWN},EC_AXES_REST=${EC_AXES_REST},EC_AXES_MAX_VELO=${EC_AXES_MAX_VELO=empty},DELAY_MS=${SAFETY_TIMEOUT}"
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN_CMD=${EC_RAMP_DOWN_CMD},EC_REST_STAT=${EC_REST_STAT},EC_RED_VEL_CMD=${EC_RED_VEL_CMD=empty},DELAY_MS=${SAFETY_TIMEOUT}"
#- Add axis
#- AX_ID : Axis ID
#- VELO_REST_LIM : Velocity at rest limit [unit same as EGU of axis]
#- VELO_MAX_LIM : Velocity maximum limit, -1 to disable [unit same as EGU of axis]
#- VELO_MAX_LIM : Velocity maximum limit when EC_RED_VEL_CMD, set to -1 to disable [unit same as EGU of axis]
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_REST_LIM=1,VELO_MAX_LIM=100"
##############################################################################
## Configure diagnostics:
#

4
scripts/addAxisToSafetyGroup.cmd
View File

@@ -18,7 +18,7 @@
#- NAME : Name of safety group
#- AX_ID : Axis ID
#- VELO_REST_LIM : Velocity at rest limit [unit same as EGU of axis]
#- VELO_MAX_LIM : Velocity maximum limit, -1 to disable [unit same as EGU of axis]
#- VELO_MAX_LIM : Velocity maximum limit when EC_RED_VEL_CMD, set to -1 to disable [unit same as EGU of axis]
#-
#################################################################################
@@ -26,7 +26,7 @@
#- <name> : Name of safety group to add axis to.
#- <Axis id> : Axis index to add (ecmc axis index).
#- <velo_rest_limit> : Axis at rest velo limit [unit of axis].
#- <velo_max_limit> : Axis max velo limit [unit of axis].
#- <velo_max_limit> : Velocity maximum limit when EC_RED_VEL_CMD, set to -1 to disable [unit same as EGU of axis]
#- <filter_time> : NOT USED (for future implemenation). Time for axis to be below velo limit [ms].
ecmcAddAxisToSafetyGroup("${NAME}",${AX_ID},${VELO_REST_LIM=0},0,${VELO_MAX_LIM=0.0})

2
scripts/addSS1Group.cmd
View File

@@ -27,7 +27,7 @@
#- <name> : Name of group
#- <ec_rampdown_cmd> : Ethercat entry input for rampdown cmd
#- <ec_standstill_status> : Ethercat entry output for group standstill status
#- <ec_max_velo_cmd> : Ethercat entry input for activation of maximum velo limitation (set to "empty" to disable)
#- <ec_red_velo_cmd> : Ethercat entry input for activation of maximum velo limitation (set to "empty" to disable)
#- <time_delay_ms> : Time delay of STO [ms]
ecmcAddSS1SafetyGroup("${NAME}","${EC_RAMP_DOWN_CMD}","${EC_AXES_REST_STAT}","${EC_AXES_LIM_VELO_CMD=empty}",${DELAY_MS=0})

209
src/ecmcPluginSafety.c
View File

@@ -8,11 +8,14 @@
* Created on: jan 29, 2024
* Author: anderssandstrom
*
* Instructions:
* - IMPORTANT: Add "USR_CFLAGS +=-DECMC_PLUGIN_MODULE_NAME=${MODULE}" to GNUMakefile
* - All functions and ecmcPluginData struct must be declared static
\*************************************************************************/
// Needed to get headers in ecmc right...
#define ECMC_IS_PLUGIN
#define ECMC_PLUGIN_VERSION 0
#define ECMC_PLUGIN_VERSION 1
#ifdef __cplusplus
extern "C" {
@@ -31,11 +34,11 @@ static int alreadyLoaded = 0;
static int destructs_ = 0;
/** Optional.
* Will be called once after successfull load into ecmc.
* Will be called once after successful load into ecmc.
* Return value other than 0 will be considered error.
* configStr can be used for configuration parameters.
**/
int safetyConstruct(char *configStr)
static int construct(char *configStr)
{
if(alreadyLoaded) {
@@ -50,19 +53,19 @@ int safetyConstruct(char *configStr)
/** Optional function.
* Will be called once at unload.
**/
void safetyDestruct(void)
static void safetyDestruct(void)
{
destructs_ = 1;
deleteAllSafetyGroups();
}
/** Optional function.
* Will be called each realtime cycle if definded
* ecmcError: Error code of ecmc. Makes it posible for
* Will be called each realtime cycle if defined
* ecmcError: Error code of ecmc. Makes it possible for
* this plugin to react on ecmc errors
* Return value other than 0 will be considered to be an error code in ecmc.
**/
int safetyRealtime(int ecmcError)
static int safetyRealtime(int ecmcError)
{
if(destructs_) return 0;
@@ -71,10 +74,10 @@ int safetyRealtime(int ecmcError)
return 0;
}
/** Link to data source here since all sources should be availabe at this stage
/** Link to data source here since all sources should be available at this stage
* (for example ecmc PLC variables are defined only at enter of realtime)
**/
int safetyEnterRT(){
static int safetyEnterRT(){
return validate();
}
@@ -82,37 +85,12 @@ int safetyEnterRT(){
* Will be called once just before leaving realtime mode
* Return value other than 0 will be considered error.
**/
int safetyExitRT(void){
static int safetyExitRT(void){
return 0;
}
//// Plc function for clear of buffers
//double fft_clear(double index) {
// return (double)clearFFT((int)index);
//}
//
//// Plc function for enable
//double fft_enable(double index, double enable) {
// return (double)enableFFT((int)index, (int)enable);
//}
//
//// Plc function for trigg new measurement (will clear buffers)
//double fft_trigg(double index) {
// return (double)triggFFT((int)index);
//}
//
//// Plc function for enable
//double fft_mode(double index, double mode) {
// return (double)modeFFT((int)index, (FFT_MODE)((int)mode));
//}
//
//// Plc function for enable
//double fft_stat(double index) {
// return (double)statFFT((int)index);
//}
// Register data for plugin so ecmc know what to use
struct ecmcPluginData pluginDataDef = {
static struct ecmcPluginData pluginDataDef = {
// Allways use ECMC_PLUG_VERSION_MAGIC
.ifVersion = ECMC_PLUG_VERSION_MAGIC,
// Name
@@ -124,170 +102,19 @@ struct ecmcPluginData pluginDataDef = {
,
// Plugin version
.version = ECMC_PLUGIN_VERSION,
// Optional construct func, called once at load. NULL if not definded.
.constructFnc = safetyConstruct,
// Optional destruct func, called once at unload. NULL if not definded.
// Optional construct func, called once at load. NULL if not defined.
.constructFnc = construct,
// Optional destruct func, called once at unload. NULL if not defined.
.destructFnc = safetyDestruct,
// Optional func that will be called each rt cycle. NULL if not definded.
// Optional func that will be called each rt cycle. NULL if not defined.
.realtimeFnc = safetyRealtime,
// Optional func that will be called once just before enter realtime mode
.realtimeEnterFnc = safetyEnterRT,
// Optional func that will be called once just before exit realtime mode
.realtimeExitFnc = safetyExitRT,
// PLC funcs
// .funcs[0] =
// { /*----fft_clear----*/
// // Function name (this is the name you use in ecmc plc-code)
// .funcName = "fft_clear",
// // Function description
// .funcDesc = "double fft_clear(index) : Clear/reset fft[index].",
// /**
// * 7 different prototypes allowed (only doubles since reg in plc).
// * Only funcArg${argCount} func shall be assigned the rest set to NULL.
// **/
// .funcArg0 = NULL,
// .funcArg1 = fft_clear,
// .funcArg2 = NULL,
// .funcArg3 = NULL,
// .funcArg4 = NULL,
// .funcArg5 = NULL,
// .funcArg6 = NULL,
// .funcArg7 = NULL,
// .funcArg8 = NULL,
// .funcArg9 = NULL,
// .funcArg10 = NULL,
// .funcGenericObj = NULL,
// },
// .funcs[1] =
// { /*----fft_enable----*/
// // Function name (this is the name you use in ecmc plc-code)
// .funcName = "fft_enable",
// // Function description
// .funcDesc = "double fft_enable(index, enable) : Set enable for fft[index].",
// /**
// * 7 different prototypes allowed (only doubles since reg in plc).
// * Only funcArg${argCount} func shall be assigned the rest set to NULL.
// **/
// .funcArg0 = NULL,
// .funcArg1 = NULL,
// .funcArg2 = fft_enable,
// .funcArg3 = NULL,
// .funcArg4 = NULL,
// .funcArg5 = NULL,
// .funcArg6 = NULL,
// .funcArg7 = NULL,
// .funcArg8 = NULL,
// .funcArg9 = NULL,
// .funcArg10 = NULL,
// .funcGenericObj = NULL,
// },
// .funcs[2] =
// { /*----fft_trigg----*/
// // Function name (this is the name you use in ecmc plc-code)
// .funcName = "fft_trigg",
// // Function description
// .funcDesc = "double fft_trigg(index) : Trigg new measurement for fft[index]. Will clear buffers.",
// /**
// * 7 different prototypes allowed (only doubles since reg in plc).
// * Only funcArg${argCount} func shall be assigned the rest set to NULL.
// **/
// .funcArg0 = NULL,
// .funcArg1 = fft_trigg,
// .funcArg2 = NULL,
// .funcArg3 = NULL,
// .funcArg4 = NULL,
// .funcArg5 = NULL,
// .funcArg6 = NULL,
// .funcArg7 = NULL,
// .funcArg8 = NULL,
// .funcArg9 = NULL,
// .funcArg10 = NULL,
// .funcGenericObj = NULL,
// },
// .funcs[3] =
// { /*----fft_mode----*/
// // Function name (this is the name you use in ecmc plc-code)
// .funcName = "fft_mode",
// // Function description
// .funcDesc = "double fft_mode(index, mode) : Set mode Cont(1)/Trigg(2) for fft[index].",
// /**
// * 7 different prototypes allowed (only doubles since reg in plc).
// * Only funcArg${argCount} func shall be assigned the rest set to NULL.
// **/
// .funcArg0 = NULL,
// .funcArg1 = NULL,
// .funcArg2 = fft_mode,
// .funcArg3 = NULL,
// .funcArg4 = NULL,
// .funcArg5 = NULL,
// .funcArg6 = NULL,
// .funcArg7 = NULL,
// .funcArg8 = NULL,
// .funcArg9 = NULL,
// .funcArg10 = NULL,
// .funcGenericObj = NULL,
// },
// .funcs[4] =
// { /*----fft_stat----*/
// // Function name (this is the name you use in ecmc plc-code)
// .funcName = "fft_stat",
// // Function description
// .funcDesc = "double fft_stat(index) : Get status of fft (NO_STAT, IDLE, ACQ, CALC) for fft[index].",
// /**
// * 7 different prototypes allowed (only doubles since reg in plc).
// * Only funcArg${argCount} func shall be assigned the rest set to NULL.
// **/
// .funcArg0 = NULL,
// .funcArg1 = fft_stat,
// .funcArg2 = NULL,
// .funcArg3 = NULL,
// .funcArg4 = NULL,
// .funcArg5 = NULL,
// .funcArg6 = NULL,
// .funcArg7 = NULL,
// .funcArg8 = NULL,
// .funcArg9 = NULL,
// .funcArg10 = NULL,
// .funcGenericObj = NULL,
// },
.funcs[0] = {0}, // last element set all to zero..
// PLC consts
/* CONTINIOUS MODE = 1 */
// .consts[0] = {
// .constName = "fft_CONT",
// .constDesc = "FFT Mode: Continious",
// .constValue = CONT
// },
// /* TRIGGERED MODE = 2 */
// .consts[1] = {
// .constName = "fft_TRIGG",
// .constDesc = "FFT Mode :Triggered",
// .constValue = TRIGG
// },
// /* TRIGGERED MODE = 2 */
// .consts[2] = {
// .constName = "fft_NO_STAT",
// .constDesc = "FFT Status: Invalid state",
// .constValue = NO_STAT,
// },
// /* TRIGGERED MODE = 2 */
// .consts[3] = {
// .constName = "fft_IDLE",
// .constDesc = "FFT Status: Idle state (waiting for trigger)",
// .constValue = IDLE
// },
// /* TRIGGERED MODE = 2 */
// .consts[4] = {
// .constName = "fft_ACQ",
// .constDesc = "FFT Status: Acquiring data",
// .constValue = ACQ
// },
// /* TRIGGERED MODE = 2 */
// .consts[5] = {
// .constName = "fft_CALC",
// .constDesc = "FFT Status: Calculating result",
// .constValue = CALC
// },
.consts[0] = {0}, // last element set all to zero..
};

8
src/ecmcSafetyPlgWrap.h
View File

@@ -25,11 +25,11 @@ int setCfgString(const char* cfgString);
* \param[in] name Name of safety group.\n
* \param[in] ec_rampdown_cmd Name of ethercat entry for ramp down command\n
* \param[in] ec_standstill_status Name of ethercat entry all axis standstill status\n
* \param[in] time_delay_ms Timedelay between ec_rampdown_cmd going high untill STO is triggered by safety relay.\n
* \param[in] time_delay_ms Timedelay between ec_ramp-down_cmd going high until STO is triggered by safety relay.\n
*
* \return 0 if success or otherwise an error code.\n
*/
//int createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
//int createSafetyGroup(name,ec_ramp-down_cmd,ec_standstill_status, time_delay_ms);
/** \brief Deletes all created objects\n
*
@@ -41,8 +41,8 @@ void deleteAllSafetyGroups();
*
* This tells the safety lib to connect to ecmc to find it's data sources.\n
* This function should be called just before entering realtime since then all\n
* data sources in ecmc will be definded (plc sources are compiled just before runtime\n
* so are only fist accesible now).\n
* data sources in ecmc will be defined (plc sources are compiled just before runtime\n
* so are only fist accessible now).\n
* \return 0 if success or otherwise an error code.\n
*/
int validate();