diff --git a/iocsh/cfg/axis.yaml b/iocsh/cfg/axis.yaml index 7dd90c9..9462689 100644 --- a/iocsh/cfg/axis.yaml +++ b/iocsh/cfg/axis.yaml @@ -133,9 +133,9 @@ trajectory: input: limit: - forward: ec0.s$(DRV_SLAVE).ONE.0 # Ethercat entry for low limit switch input + forward: ec0.s$(DRV_SLAVE).driveStatus01.12 # Ethercat entry for low limit switch input # forwardPolarity: 0 # Polarity of forward limit switch - backward: ec0.s$(DRV_SLAVE).ONE.0 # Ethercat entry for high limit switch input + backward: ec0.s$(DRV_SLAVE).driveStatus01.11 # Ethercat entry for high limit switch input # backwardPolarity: 0 # Polarity of forward limit switch home: 'ec0.s$(DRV_SLAVE).ONE.0' # Ethercat entry for home switch # homePolarity: 0 # Polarity of home switch diff --git a/iocsh/cfg/axis_latch.yaml b/iocsh/cfg/axis_latch.yaml deleted file mode 100644 index ebf8745..0000000 --- a/iocsh/cfg/axis_latch.yaml +++ /dev/null @@ -1,204 +0,0 @@ -macros: LIMIT=10000,TYPE=0,DRV_SLAVE=${DRV_SLAVE} # Macros for all below (evaluated before jinja linter) - -axis: - id: 1 # Axis id - type: joint # this is for future selection of axis type - # mode: CSV # supported mode, CSV and CSP, defaults CSV - # parameters: 'axisPar' # additional parameters # Additional params to motor record driver - #healthOutput: ec0... # Ethercat entry for health output - # autoMode: # Switch drive modes automaticaly for normal motion and homing (smaract for instance) - # modeSet: ec0.. # Ethercat entry drive mode write (set CSV,CSP,homing) - # modeAct: ec0.. # Ethercat entry drive mode reading (set CSV,CSP,homing) - # modeCmdMotion: 9 # Drive mode value for normal motion (written to axis.drvMode.modeSet when normal motion) - # modeCmdHome: 10 # Drive mode value for when homing (written to axis.drvMode.modeSet when homing) - # features: - # blockCom: false # Block communication to axis - # allowedFunctions: - # homing: true # Allow homing - # constantVelocity: true # Allow constant velocity - # positioning: true # Allow positioning - -epics: - name: Axis1 # Axis anme - precision: 3 # Decimal count - description: very important motor axis # Axis description - unit: mm # Unit - # motorRecord: - # enable: true - # description: This is MR - # fieldInit: 'RRES=1.0,RTRY=2,RMOD=1,UEIP=0,RDBD=0.1,URIP=1,RDBL=$(IOC):$(ECMC_MOTOR_NAME)-PosActSim' # Extra config for Motor record - -drive: - numerator: 3600 # Fastest speed in engineering units - denominator: 32768 # I/O range for ECMC_EC_ALIAS_DRV_VELO_SET - # type: 0 # Stepper: 0. DS402: 1 (DS402 = servos and advanced stepper drives) - control: ec0.s$(DRV_SLAVE).driveControl01 # Control word ethercat entry - enable: 0 # Enable bit index in control word (not used if DS402) - enabled: 1 # Enabled bit index in status word (not used if DS402) - status: ec0.s$(DRV_SLAVE).driveStatus01 # Status word ethercat entry - setpoint: ec0.s$(DRV_SLAVE).velocitySetpoint01 # Velocity setpoint if CSV. Position setpoint if CSP -# reduceTorque: 2 # Reduce torque bit in drive control word -# reduceTorqueEnable: True # Enable reduce torque functionality -# brake: -# enable: true # Enable brake -# output: ec0... # Ethercat link to brake output -# openDelay: 0 # Brake timing parameter in cycles (default 1kHz) -# closeAhead: 0 # Brake timing parameter in cycles (default 1kHz) -# reset: 1 # Reset (if no drive reset bit then leave empty) -# warning: 2 # Warning (if no drive warning bit then leave empty) -# error: # max 3 -# - 3 # Error 0 (if no drive error bit then leave empty) -# - 7 # Error 1 (if no drive error bit then leave empty) -# - 14 # Error 2 (if no drive error bit then leave empty) - -encoder: - numerator: 360 # Scaling numerator example 360 deg/rev - denominator: 12800 # Scaling denominator example 4096 ticks per 360 degree - # type: 0 # Type: 0=Incremental, 1=Absolute - bits: 16 # Total bit count of encoder raw data - # absBits: 0 # Absolute bit count (for absolute encoders) always least significant part of 'bits' - # absOffset: 0 # Encoder offset in eng units (for absolute encoders) - # mask: '0xFFF00' # Mask applied to raw encoder value - position: ec0.s$(DRV_SLAVE).positionActual01 # Ethercat entry for actual position input (encoder) - control: ec0.s$(DRV_SLAVE).encoderControl01 # mandatory only if 'reset' is used - status: ec0.s$(DRV_SLAVE).encoderStatus01 # mandatory only if 'warning' or 'error' are used - # ready: 10 # Bit in encoder status word for encoder ready - # source: 0 # 0 = Encoder value from etehrcat hardware, 1 = Encoder value from PLC - # reset: 1 # Reset (optional) - # warning: 2 # Warning (optional) - # error: # max 3 (optional) - # - 5 # Error 0 - # - 9 # Error 1 - # - 11 # Error 2 - # filter: - # velocity: - # size: 100 # Filter size for velocity - # enable: true # enable velocity filter - # position: - # size: 100 # Filter size for encoder value - # enable: true # enable encoder value filter - latch: - position: 'ec0.s$(DRV_SLAVE).encoderLatchPostion01' # Link to latched value. Used for some homing seqs - control: 0 # Bit in encoder control word to arm latch. Used for some homing seqs - status: 0 # Bit in encoder status word for latch triggered status. Used for some homing seqs - primary: 1 # Use this encoder as primary (for control) - homing: - type: 12 # Homing sequence type - position: -30 # Position to reference encoder to - velocity: - to: 10 # Velocity to cam/sensor (used for some homing seqs) - from: 5 # Velocity from cam/sensor (used for some homing seqs) - acceleration: 20 # Acceleration during homing - deceleration: 100 # Deceleration during homing - # refToEncIDAtStartup: 1 # At startup then set the start value of this encoder to actpos of this encoder id - refAtHome: 1 # If homing is executed then set position of this encoder - # tolToPrim: 0 # If set then this is the max allowed tolerance between prim encoder and this encoder - # postMoveEnable: yes # Enable move after successfull homing - # postMovePosition: 10 # Position to move to after successfull homing - # trigg: ec0.. # Ethercat entry for triggering drive internal homing seq (seq id 26) - # ready: ec0.. # Ethercat entry for reading drive internal homing seq ready (seq id 26) - latchCount: 1 - -controller: - Kp: 1 # Kp proportinal gain - Ki: 0.02 # Ki integral gain - Kd: 0 # Kd derivative gain - # Kff: 1 # Feed forward gain - # deadband: - # tol: 0.01 # Stop control if within this distance from target for the below time - # time: 100 - # limits: - # minOutput: -100 # Minimum controller output - # maxOutput: 100 # Maximum controller output - # minIntegral: -100 # Minimum integral output - # maxIntegral: 100 # Maximum integral output - # inner: - # Kp: 0.1 # Kp for when close to target - # Ki: 0.1 # Ki for when close to target - # Kd: 0.1 # Kd for when close to target - # tol: 0.1 # Distance from target for when inner PID params will be used, defaults to atTarget tol - -trajectory: - # type: 1 # Default 0 = trapetz, 1 = S-curve (ruckig) - axis: - velocity: 100 # Default velo for axis - acceleration: 100 # Default acc for axis - deceleration: 100 # Default dec for axis - # emergencyDeceleration: 0.05 # Deceleration when axis in error state - jerk: 10 # Default jerk for axis - jog: - velocity: 5 # Default velo fro JOG (motor record) -# modulo: -# range: 360 # Modulo range 0..360 -# type: 0 # Modulo type - -input: - limit: - forward: ec0.s$(DRV_SLAVE).ONE.0 # Ethercat entry for low limit switch input - # forwardPolarity: 0 # Polarity of forward limit switch - backward: ec0.s$(DRV_SLAVE).ONE.0 # Ethercat entry for high limit switch input - # backwardPolarity: 0 # Polarity of forward limit switch - home: 'ec0.s$(DRV_SLAVE).ONE.0' # Ethercat entry for home switch - # homePolarity: 0 # Polarity of home switch - interlock: 'ec0.s$(DRV_SLAVE).ONE.0' # Ethercat entry for interlock switch input - # interlockPolarity: 0 # Polarity of interlock switch - -# homing: -# type: 3 # Homing sequence type -# position: -30 # Position to reference encoder to -# velocity: -# to: 10 # Velocity to cam/sensor (used for some homing seqs) -# from: 5 # Velocity from cam/sensor (used for some homing seqs) -# acc: 20 # Acceleration during homing -# dec: 100 # Deceleration during homing -# refToEncIDAtStartup: 1 # At startup then set the start value of this encoder to actpos of this encoder id -# refAtHome: 1 # If homing is executed then set position of this encoder -# tolToPrim: 0 # If set then this is the max allowed tolerance between prim encoder and this encoder -# postMoveEnable: yes # Enable move after successfull homing -# postMovePosition: 10 # Position to move to after successfull homing -# timeout: 100 # Sequence timeout - -softlimits: - enable: false # Enable soft limits - forward: 100 # Soft limit position fwd - forwardEnable: false # Soft limit position fwd enable - backward: -100 # Soft limit position bwd - backwardEnable: false # Soft limit position bwd enable - -monitoring: - lag: - enable: true # Enable position lag monitoring (following error) - tolerance: 2 # Allowed tolerance - time: 10 # Allowed time outside tolerance target: - enable: false # Enable at target monitoring (needs to be enabled if using motor record) - tolerance: 0.5 # Allowed tolerance - time: 10 # Filter time inside tolerance to be at target - velocity: - enable: false # Enable velocity monitoring - max: 100 # Allowed max velocity - time: - trajectory: 100 # Time allowed outside max velo before system init halt - drive: 200 # Time allowed outside max velo before system disables drive - # velocityDifference: - # enable: true # Enable velocity diff monitoring (velo set vs velo act) - # max: 100 # Allowed max difference - # time: - # trajectory: 100 # Time allowed outside max diff velo before system init halt - # drive: 200 # Time allowed outside max diff velo before system disables drive - -plc: - enable: true # Enable axis plc - externalCommands: true # Allow axis to inputs from PLC - file: ${PLC_PATH}test.plc - code: # Sync code - - "if(static.counter % ${LIMIT} == 0){println('AX PLC: Appended');static.counter:=0;};" # calculate set pos for physical axis - - "if(not(static.plc_code_loaded)) {static.counter:=static.counter+1;};" - - ec0.s$(DRV_SLAVE).ONE > 1; # Enable axis if one of master axes is enabled - - ec0.s$(DRV_SLAVE).ZERO > 1; # calculate set pos for physical axis -# velocity_filter: # Filter used to smother velocity feedforward -# encoder: # Filter plc enc velo -# enable: false # Filter enable -# size: 100 # Filter size -# trajectory: # Filter plc traj velo -# enable: false # Filter enable -# size: 100 # Filter size diff --git a/iocsh/cfg/el7031.ax b/iocsh/cfg/el7031.ax deleted file mode 100644 index 230ffd9..0000000 --- a/iocsh/cfg/el7031.ax +++ /dev/null @@ -1,93 +0,0 @@ -#General -epicsEnvSet("ECMC_MOTOR_NAME", "Axis1") -epicsEnvSet("ECMC_R", "Axis1-") -epicsEnvSet("ECMC_AXIS_NO", "1") -epicsEnvSet("ECMC_DESC", "Test EL7037") -epicsEnvSet("ECMC_EGU", "deg") # Motor Record Unit -epicsEnvSet("ECMC_PREC", "3") # Motor Record Precision -epicsEnvSet("ECMC_AXISCONFIG", "") # Extra parameters to driver -epicsEnvSet("ECMC_EC_AXIS_HEALTH", "") # Entry for axis health output (example: ec0.s1.binaryOutput01.0) -epicsEnvSet("ECMC_MOD_RANGE" , "0") # Modulo range (traj setpoints and encoder values will be in range 0..ECMC_MOD_RANGE) -epicsEnvSet("ECMC_MOD_TYPE", "0") # For positioning and MOD_RANGE is larger than 0: 0 = Normal, 1 = Always Fwd, 2 = Always Bwd, 3 = Closest Distance - -#Encoder -epicsEnvSet("ECMC_ENC_SCALE_NUM" "360") -epicsEnvSet("ECMC_ENC_SCALE_DENOM" "12800") -epicsEnvSet("ECMC_ENC_TYPE" "0") # Type: 0=Incremental, 1=Absolute -epicsEnvSet("ECMC_ENC_BITS" "16") # Total bit count of encoder raw data -epicsEnvSet("ECMC_ENC_ABS_BITS", "0") # Absolute bit count (for absolute encoders) always least significant part of ECMC_ENC_BITS -epicsEnvSet("ECMC_ENC_ABS_OFFSET" "0") # Encoder offset in eng units (for absolute encoders) -epicsEnvSet("ECMC_EC_ENC_ACTPOS", "ec0.s$(DRV_ID).positionActual01") # Ethercat entry for actual position input (encoder) -epicsEnvSet("ECMC_EC_ENC_RESET", "") # Reset (if no encoder reset bit then leave empty) -epicsEnvSet("ECMC_EC_ENC_ALARM_0", "") # Error 0 (if no encoder error bit then leave empty) -epicsEnvSet("ECMC_EC_ENC_ALARM_1", "") # Error 1 (if no encoder error bit then leave empty) -epicsEnvSet("ECMC_EC_ENC_ALARM_2", "") # Error 2 (if no encoder error bit then leave empty) -epicsEnvSet("ECMC_EC_ENC_WARNING", "") # Warning (if no encoder warning bit then leave empty) - -#Drive -epicsEnvSet("ECMC_DRV_TYPE" "0") # Stepper: 0. DS402: 1 (DS402 = servos and advanced stepper drives) -epicsEnvSet("ECMC_DRV_SCALE_NUM" "3600") # Fastest speed in engineering units -epicsEnvSet("ECMC_DRV_SCALE_DENOM" "32768") # I/O range for ECMC_EC_ALIAS_DRV_VELO_SET -epicsEnvSet("ECMC_EC_DRV_CONTROL", "ec0.s$(DRV_ID).driveControl01.0") # Ethercat entry for control word or bit output -epicsEnvSet("ECMC_EC_DRV_STATUS", "ec0.s$(DRV_ID).driveStatus01.1") # Ethercat entry for status word or bit input -epicsEnvSet("ECMC_EC_DRV_VELOCITY", "ec0.s$(DRV_ID).velocitySetpoint01") # Ethercat entry for velocity setpoint output -epicsEnvSet("ECMC_EC_DRV_REDUCE_TORQUE", "ec0.s$(DRV_ID).driveControl01.2") # Ethercat entry for reduce torque output -epicsEnvSet("ECMC_EC_DRV_BRAKE", "ec0.s$(DO_ID).binaryOutput01.0") # Ethercat entry for brake output -epicsEnvSet("ECMC_DRV_BRAKE_OPEN_DLY_TIME", "1000") # Brake timing parameter in cycles (default 1kHz) -epicsEnvSet("ECMC_DRV_BRAKE_CLOSE_AHEAD_TIME", "2000") # Brake timing parameter in cycles (default 1kHz) -epicsEnvSet("ECMC_EC_DRV_RESET", "ec0.s$(DRV_ID).driveControl01.1") # Reset -epicsEnvSet("ECMC_EC_DRV_ALARM_0", "ec0.s$(DRV_ID).driveStatus01.3") # Error -epicsEnvSet("ECMC_EC_DRV_ALARM_1", "ec0.s$(DRV_ID).driveStatus01.7") # Stall -epicsEnvSet("ECMC_EC_DRV_ALARM_2", "ec0.s$(DRV_ID).driveStatus01.14") # Sync error -epicsEnvSet("ECMC_EC_DRV_WARNING", "ec0.s$(DRV_ID).driveStatus01.2") # Warning - -#Trajectory -epicsEnvSet("ECMC_VELO", "360.0") -epicsEnvSet("ECMC_JOG_VEL", "360.0") -epicsEnvSet("ECMC_JAR", "0.0") # JAR defaults to VELO/ACCL -epicsEnvSet("ECMC_ACCS_EGU_PER_S2", "360") -epicsEnvSet("ECMC_EMERG_DECEL", "1000") # Emergency deceleration - -#Homing -epicsEnvSet("ECMC_HOME_PROC", "1") -epicsEnvSet("ECMC_HOME_POS", "0.0") -epicsEnvSet("ECMC_HOME_VEL_TO", "5") -epicsEnvSet("ECMC_HOME_VEL_FRM", "4") -epicsEnvSet("ECMC_HOME_ACC", "21") -epicsEnvSet("ECMC_HOME_DEC", "100") -epicsEnvSet("ECMC_HOME_POS_MOVE_ENA", "0") # Enable move to position after successfull homing -epicsEnvSet("ECMC_HOME_POS_MOVE_TARG_POS","0") # Target position to go to after successfull homing - -#Controller -epicsEnvSet("ECMC_CNTRL_KP", "5.0") -epicsEnvSet("ECMC_CNTRL_KI", "0.02") -epicsEnvSet("ECMC_CNTRL_KD", "0.0") -epicsEnvSet("ECMC_CNTRL_KFF", "1.0") - -#Monitoring -# Switches -epicsEnvSet("ECMC_EC_MON_LOWLIM", "ec0.s$(DRV_ID).ONE.0") # Ethercat entry for low limit switch input -epicsEnvSet("ECMC_EC_MON_HIGHLIM", "ec0.s$(DRV_ID).ONE.0") # Ethercat entry for high limit switch inpuit -epicsEnvSet("ECMC_EC_MON_HOME_SWITCH", "ec0.s$(DRV_ID).ONE.0") # Ethercat entry for home switch input -epicsEnvSet("ECMC_EC_MON_EXT_INTERLOCK", "ec0.s$(DRV_ID).ONE.0") # Ethercat entry for external interlock input - -# Softlimits (disable with 0,0,0) -epicsEnvSet("ECMC_SOFT_LOW_LIM", "$(SM_DLLM=0)") -epicsEnvSet("ECMC_SOFT_HIGH_LIM", "$(SM_DHLM=0)") -epicsEnvSet("ECMC_DXLM_ENABLE", "0") - -# Position lag -epicsEnvSet("ECMC_MON_LAG_MON_TOL", "5") -epicsEnvSet("ECMC_MON_LAG_MON_TIME", "100") -epicsEnvSet("ECMC_MON_LAG_MON_ENA", "0") - -# At target -epicsEnvSet("ECMC_MON_AT_TARGET_TOL", "0.1") -epicsEnvSet("ECMC_MON_AT_TARGET_TIME", "100") -epicsEnvSet("ECMC_MON_AT_TARGET_ENA", "1") - -# Velocity -epicsEnvSet("ECMC_MON_VELO_MAX", "21000.0") -epicsEnvSet("ECMC_MON_VELO_MAX_TRAJ_TIME","100") -epicsEnvSet("ECMC_MON_VELO_MAX_DRV_TIME", "200") -epicsEnvSet("ECMC_MON_VELO_MAX_ENA", "0") diff --git a/iocsh/el7031.script b/iocsh/el7042-0052.script similarity index 75% rename from iocsh/el7031.script rename to iocsh/el7042-0052.script index f4aa5d2..9759faf 100644 --- a/iocsh/el7031.script +++ b/iocsh/el7042-0052.script @@ -10,17 +10,13 @@ require ecmccomp ############################################################################## ## Configure hardware -epicsEnvSet("DRV_SLAVE", "4") -${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(DRV_SLAVE), HW_DESC=EL7031" - -#- Apply motor config with some custom macros +epicsEnvSet("DRV_SLAVE", "13") +${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(DRV_SLAVE), HW_DESC=EL7041-0052" ${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Motor-Generic-2Phase-Stepper,MACROS='I_STDBY_MA=200,I_MAX_MA=1000,R_COIL_MOHM=1700'" epicsEnvSet("ENC_SLAVE", "14") ${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(ENC_SLAVE), HW_DESC=EL5042" - -#- Apply motor config with some custom macros -${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Encoder-Generic-BISS-C" +${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Encoder-RLS-LA11-24bit" #Apply hardware configuration ecmcConfigOrDie "Cfg.EcApplyConfig(1)" @@ -37,6 +33,16 @@ ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml,LIMIT=1 # ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlPlc.cmd, "FILE=./plc/plc_cfg.yaml,LIMIT=5000,TYPE=1" +############################################################################## +## Load safety plugin +# +#- PLUGIN_ID = Plugin instansiation index, must be unique for each call + +require ecmc_plugin_safety sandst_a "PLUGIN_ID=0" + +ecmcAddSafetyGroup("first","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.0","ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ONE.1",500) +ecmcAddAxisToSafetyGroup("first",1,1,0) + ############################################################################## ############# Configure diagnostics: diff --git a/scripts/startup.cmd b/scripts/startup.cmd index 37fb6f6..89b6af6 100644 --- a/scripts/startup.cmd +++ b/scripts/startup.cmd @@ -11,22 +11,20 @@ #- #- Arguments #- [mandatory] -#- PLUGIN_ID = Plugin instansiation index, must be unique for each call +#- PLUGIN_ID : Plugin instansiation index, must be unique for each call #- -#- [optional] -#- AX = Axis id, default 1 -#- BUFF_SIZE = Buffer size, default 1000 -#- DBG = Debug mode, default 1 -#- ENA = Enable operation, default 1 -#- REPORT = Printout plugin details, default 1 - ################################################################################# -#- Load plugin: MOTION +#- Print discalimer +# +# IMPORTANT!!! +# This plugin has _NO_ safety rated functionalities. +# The intended use of this plugin is only to handle interfacing with a safety PLC. +# -# Only allow call startup.cmd once. if more objects are needed then use addMotionObj.cmd directlly. +#- Load plugin: Safety -#- add One motion plugin object, only run startup once -${ECMC_PLG_MOTION_INIT=""}${SCRIPTEXEC} $(ecmc_plugin_motion_DIR)addMotionObj.cmd "PLUGIN_ID=${PLUGIN_ID},AX=${AX=1},BUFF_SIZE=${BUFF_SIZE=1000},DBG=${DBG=1},ENA=${ENA=1},REPORT=${REPORT=1}" - -epicsEnvSet("ECMC_PLG_MOTION_INIT" ,"#") +# Might need differet paths for PSI and ESS.. must check +epicsEnvSet(ECMC_PLUGIN_FILNAME,"$(ecmc_plugin_safety_DIR)/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_safety.so") +epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;") +${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=${PLUGIN_ID=0},FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG=""}', REPORT=${REPORT=1}" diff --git a/src/ecmcPluginSafety.c b/src/ecmcPluginSafety.c index 024642e..a33f432 100644 --- a/src/ecmcPluginSafety.c +++ b/src/ecmcPluginSafety.c @@ -26,8 +26,9 @@ extern "C" { #include "ecmcSafetyPlgDefs.h" #include "ecmcSafetyPlgWrap.h" -static int lastEcmcError = 0; -static char* lastConfStr = NULL; +static int lastEcmcError = 0; +static int alreadyLoaded = 0; +static int destructs_ = 0; /** Optional. * Will be called once after successfull load into ecmc. @@ -36,11 +37,14 @@ static char* lastConfStr = NULL; **/ int safetyConstruct(char *configStr) { - //This module is allowed to load several times so no need to check if loaded - - // create FFT object and register data callback - lastConfStr = strdup(configStr); - return createSafetyGroup(configStr); + + if(alreadyLoaded) { + return ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE; + } + + alreadyLoaded = 1; + + return setCfgString(configStr); } /** Optional function. @@ -48,10 +52,8 @@ int safetyConstruct(char *configStr) **/ void safetyDestruct(void) { + destructs_ = 1; deleteAllSafetyGroups(); - if(lastConfStr){ - free(lastConfStr); - } } /** Optional function. @@ -62,6 +64,8 @@ void safetyDestruct(void) **/ int safetyRealtime(int ecmcError) { + if(destructs_) return; + executeSafetyGroups(); lastEcmcError = ecmcError; return 0; @@ -71,7 +75,7 @@ int safetyRealtime(int ecmcError) * (for example ecmc PLC variables are defined only at enter of realtime) **/ int safetyEnterRT(){ - return linkDataToSafetyObjs(); + return validate(); } /** Optional function. @@ -117,8 +121,6 @@ struct ecmcPluginData pluginDataDef = { .desc = "Safety plugin.", // Option description .optionDesc = "\n "ECMC_PLUGIN_DBG_PRINT_OPTION_CMD"<1/0> : Enables/disables printouts from plugin, default = disabled.\n" - " "ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD" : EtherCat entry input for ramp down cmd (bit).\n" - " "ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD" : EtherCat entry output for all axes standstill status (bit).\n" , // Plugin version .version = ECMC_PLUGIN_VERSION, diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp index 383de2d..3ed9290 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSafetyGroup.cpp @@ -34,10 +34,13 @@ extern DBBASE *pdbbase; * - invalid_argument * - runtime_error */ -ecmcSafetyGroup::ecmcSafetyGroup(int objIndex, // index of this object (if several is created) - char* configStr, - char* portName) - : asynPortDriver(portName, +ecmcSafetyGroup::ecmcSafetyGroup(const char *name, + const char *ec_rampdown_cmd, + const char *ec_standstill_status, + int time_delay_ms, + const char *cfg_string, + char* portName) + : asynPortDriver(portName, 1, /* maxAddr */ asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask | asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask | @@ -52,23 +55,58 @@ ecmcSafetyGroup::ecmcSafetyGroup(int objIndex, // index of this object ( 0, /* Default priority */ 0) /* Default stack size */ { - - status_ = 0; - asynStatusId_ = -1; - axesCounter_ = 0; - ecmcSampleRateHz_ = getEcmcSampleRate(); - dataSourcesLinked_ = 0; + sName_ = strdup(name); + sEcRampDownCmdNameOrg_ = strdup(ec_rampdown_cmd); + sEcAxesStandStillStatOrg_= strdup(ec_standstill_status); + sEcRampDownCmdNameStrip_ = strdup(ec_rampdown_cmd); + sEcAxesStandStillStatStrip_ = strdup(ec_standstill_status); + sConfig_ = strdup(cfg_string); + delayMs_ = time_delay_ms; + status_ = 0; + asynStatusId_ = -1; + axesCounter_ = 0; + ecmcSampleRateHz_ = getEcmcSampleRate(); + dataSourcesLinked_ = 0; + dataItemRampDownCmd_ = NULL; + dataItemStandStillStat_ = NULL; + masterId_ = 0; + slaveIdRampDown_ = 0; + bitRampDown_ = 0; + slaveIdStandStill_ = 0; + bitStandStill_ = 0; + aliasRampDown_[0] = 0; + aliasStandStill_[0] = 0; + // Config defaults - cfgDbgMode_ = 0; - - parseConfigStr(configStr); // Assigns all configs + cfgDbgMode_ = 0; + parseConfigStr(cfg_string); initAsyn(); + if(cfgDbgMode_) { + printf("Safety: 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_); + } } -ecmcSafetyGroup::~ecmcSafetyGroup() { +ecmcSafetyGroup::~ecmcSafetyGroup() { + if(cfgDbgMode_) { + printf("Safety: Cleanup\n"); + } + + free(sName_); + free(sEcRampDownCmdNameOrg_); + free(sEcAxesStandStillStatOrg_); + free(sEcRampDownCmdNameStrip_); + free(sEcAxesStandStillStatStrip_); + free(sConfig_); } -void ecmcSafetyGroup::parseConfigStr(char *configStr) { +void ecmcSafetyGroup::parseConfigStr(const char *configStr) { // check config parameters if (configStr && configStr[0]) { @@ -89,61 +127,147 @@ void ecmcSafetyGroup::parseConfigStr(char *configStr) { cfgDbgMode_ = atoi(pThisOption); } - // ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD (string) - if (!strncmp(pThisOption, ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD, strlen(ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD); - printf("RAMP_DOWN_CMD=%s",pThisOption); - //cfgDbgMode_ = atoi(pThisOption); - } - - // ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD (string) - if (!strncmp(pThisOption, ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD, strlen(ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD))) { - pThisOption += strlen(ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD); - printf("STANDSTILL_STAT=%s",pThisOption); - //cfgDbgMode_ = atoi(pThisOption); - } - pThisOption = pNextOption; } free(pOptions); } } -void ecmcSafetyGroup::connectToDataSource() { - ///* Check if already linked (one call to enterRT per loaded FFT lib (FFT object)) - // But link should only happen once!!*/ - //if( dataSourcesLinked_ ) { - // return; - //} - // - //// Get dataItem - //dataItem_ = (ecmcDataItem*) getEcmcDataItem(cfgDataSourceStr_); - //if(!dataItem_) { - // throw std::runtime_error( "Data item NULL." ); - //} - // - //dataItemInfo_ = dataItem_->getDataItemInfo(); -// - //// Register data callback - //callbackHandle_ = dataItem_->regDataUpdatedCallback(f_dataUpdatedCallback, this); - //if (callbackHandle_ < 0) { - // throw std::runtime_error( "Failed to register data source callback."); - //} -// - //// Check data source - //if( !dataTypeSupported(dataItem_->getEcmcDataType()) ) { - // throw std::invalid_argument( "Data type not supported." ); - //} -// - //// Add oversampling - //cfgDataSampleRateHz_ = cfgSampleRateHz_ * dataItem_->getEcmcDataSize()/dataItem_->getEcmcDataElementSize(); - //setDoubleParam(asynSRateId_, cfgDataSampleRateHz_); - //callParamCallbacks(); -// - // dataSourcesLinked_ = 1; - // updateStatus(IDLE); +void ecmcSafetyGroup::validate() { + validateCfgs(); + connectToDataSources(); + validateAxes(); +} - dataSourcesLinked_ = 1; +void ecmcSafetyGroup::validateCfgs() { + + // Validate data sources + int masterIdRampDown=-1; + if(parseEcPath(sEcRampDownCmdNameOrg_, + &masterIdRampDown, + &slaveIdRampDown_, + aliasRampDown_, + &bitRampDown_)) { + throw std::runtime_error( "Safety: Parse error: Data source for rampdown cmd."); + } + + int masterIdStandStill=-1; + if(parseEcPath(sEcAxesStandStillStatOrg_, + &masterIdStandStill, + &slaveIdStandStill_, + aliasStandStill_, + &bitStandStill_)) { + throw std::runtime_error( "Safety: Parse error: Data source for standstill status."); + } + + if(masterIdStandStill != masterIdRampDown ) { + throw std::runtime_error( "Safety: Parse error: Master id for datasources different."); + } + masterId_ = masterIdStandStill; + + if(bitRampDown_ < 0) { + throw std::runtime_error( "Safety: Parse error: Rampdown cmd, bit invalid."); + } + + if(bitStandStill_ < 0) { + throw std::runtime_error( "Safety: Parse error: Standstill status, bit invalid."); + } + + if(cfgDbgMode_) { + printf("Safety: I/O link parsed:\n" + " name: %s\n" + " masterid: %d\n" + " slaveid: %d\n" + " alias: %s\n" + " bit: %d\n", + sEcRampDownCmdNameOrg_, + masterIdRampDown, + slaveIdRampDown_, + aliasRampDown_, + bitRampDown_); + + printf("Safety: I/O link parsed:\n" + " name: %s\n" + " masterid: %d\n" + " slaveid: %d\n" + " alias: %s\n" + " bit: %d\n", + sEcAxesStandStillStatOrg_, + masterIdStandStill, + slaveIdStandStill_, + aliasStandStill_, + bitStandStill_); + } + + // Now bits are idenfied. Now bits need to be removed in order to find data item. + stripBits(); +} + +void ecmcSafetyGroup::stripBits() { + char * lastdot = strrchr(sEcRampDownCmdNameStrip_,'.'); + if(lastdot) { + lastdot[0] = 0; + } + else { + throw std::runtime_error( "Safety: Failed trim bit from ramp down I/O link."); + } + + if(cfgDbgMode_) { + printf("Safety: I/O bit removed:\n" + " before: %s\n" + " after: %s\n", + sEcRampDownCmdNameOrg_, + sEcRampDownCmdNameStrip_); + } + + lastdot = strrchr(sEcAxesStandStillStatStrip_,'.'); + if(lastdot) { + lastdot[0] = 0; + } + else { + throw std::runtime_error( "Safety: Failed trim bit from stand still I/O link."); + } + + if(cfgDbgMode_) { + printf("Safety: I/O bit removed:\n" + " before: %s\n" + " after: %s\n", + sEcAxesStandStillStatOrg_, + sEcAxesStandStillStatStrip_); + } +} + +void ecmcSafetyGroup::validateAxes() { + + for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { + if(!*saxis) { + throw std::runtime_error( "Safety: Axis object NULL."); + } + } +} + +void ecmcSafetyGroup::connectToDataSources() { + if( dataSourcesLinked_ ) { + 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 ecmcSafetyGroup::initAsyn() { @@ -153,7 +277,7 @@ void ecmcSafetyGroup::initAsyn() { "." + ECMC_PLUGIN_ASYN_SAFETY_STAT; int *paramId = &asynStatusId_; if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) { - throw std::runtime_error("Failed create asyn parameter mode"); + throw std::runtime_error("Safety: Failed create asyn parameter mode"); } setIntegerParam(*paramId, (epicsInt32)status_); @@ -183,19 +307,29 @@ asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) { return asynError; } -void ecmcSafetyGroup::addAxis(int axisId) { - ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId); +void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) { + ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId); + if(axis) { - axes_.push_back(axis); + safetyAxis* saxis = new safetyAxis(axis, axisId, veloLimit, standStillTimeMs); + axes_.push_back(saxis); axesCounter_++; } else { - throw std::out_of_range("Invalid axis id"); + throw std::out_of_range("Safety: Invalid axis id"); + } + + if(cfgDbgMode_) { + printf("Safety: Added axis %d to safety group \"%s\"\n",axisId,sName_); } return; } +std::string ecmcSafetyGroup::getName() { + return sName_; +} + uint8_t ecmcSafetyGroup::getUint8(uint8_t* data) { return *data; } diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h index 877daa9..f0ed774 100644 --- a/src/ecmcSafetyGroup.h +++ b/src/ecmcSafetyGroup.h @@ -20,6 +20,23 @@ #include "ecmcAxisBase.h" #include +class safetyAxis { + public: + safetyAxis(ecmcAxisBase* axis, + int axisIndex, + double veloLimit, + int standStillTimeMs) { + veloLimit_ = veloLimit; + axisIndex_ = axisIndex; + axis_ = axis; + standStillTimeMs_ = standStillTimeMs; + } + double veloLimit_; + int axisIndex_; + int standStillTimeMs_; + ecmcAxisBase* axis_; +}; + class ecmcSafetyGroup : public asynPortDriver { public: @@ -28,19 +45,28 @@ class ecmcSafetyGroup : public asynPortDriver { * - bad_alloc * - out_of_range */ - ecmcSafetyGroup(int objIndex, // index of this object - char* configStr, + ecmcSafetyGroup(const char *name, + const char *ec_rampdown_cmd, + const char *ec_standstill_status, + int time_delay_ms, + const char *cfg_string, char* portName); + ~ecmcSafetyGroup(); // Call just before realtime because then all data sources should be available - void connectToDataSource(); - void addAxis(int axisId); + void validate(); + void addAxis(int axisId, double veloLimit,int standStillTimeMs); void execute(); virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + std::string getName(); private: - void parseConfigStr(char *configStr); + void validateCfgs(); + void validateAxes(); + void stripBits(); + void connectToDataSources(); + void parseConfigStr(const char *configStr); void initAsyn(); double ecmcSampleRateHz_; int dataSourcesLinked_; // To avoid link several times @@ -52,12 +78,29 @@ class ecmcSafetyGroup : public asynPortDriver { // Asyn int asynStatusId_; - - // int status_; int axesCounter_; std::vector axes_; + char* sName_; + char* sEcRampDownCmdNameOrg_; + char* sEcAxesStandStillStatOrg_; + char* sEcRampDownCmdNameStrip_; + char* sEcAxesStandStillStatStrip_; + char* sConfig_; + ecmcDataItem *dataItemRampDownCmd_; + ecmcDataItem *dataItemStandStillStat_; + int delayMs_; + int masterId_; + int slaveIdRampDown_; + int bitRampDown_; + int slaveIdStandStill_; + int bitStandStill_; + char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; + char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; + + std::vector axes_; + // Some generic utility functions static uint8_t getUint8(uint8_t* data); static int8_t getInt8(uint8_t* data); diff --git a/src/ecmcSafetyPlg.dbd b/src/ecmcSafetyPlg.dbd new file mode 100644 index 0000000..6c96784 --- /dev/null +++ b/src/ecmcSafetyPlg.dbd @@ -0,0 +1 @@ +registrar("ecmcSafetyPlgRegister") diff --git a/src/ecmcSafetyPlgDefs.h b/src/ecmcSafetyPlgDefs.h index f120f22..552e90d 100644 --- a/src/ecmcSafetyPlgDefs.h +++ b/src/ecmcSafetyPlgDefs.h @@ -14,15 +14,14 @@ #ifndef ECMC_MOTION_PLG_DEFS_H_ #define ECMC_MOTION_PLG_DEFS_H_ -#define ECMC_PLUGIN_ASYN_PREFIX "plugin.safety" +#define ECMC_PLUGIN_ASYN_PREFIX "plugin.safety" // Options -#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT=" -#define ECMC_PLUGIN_RAMP_DOWN_ENTRY_OPTION_CMD "RAMP_DOWN_ENTRY=" -#define ECMC_PLUGIN_AXES_STANDSTILL_ENTRY_OPTION_CMD "AXES_STANDSTILL_ENTRY=" +#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT=" /** Just one error code in "c" part of plugin (error handled with exceptions i c++ part) */ #define ECMC_PLUGIN_SAFETY_ERROR_CODE 1 +#define ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE 2 #endif /* ECMC_MOTION_PLG_DEFS_H_ */ diff --git a/src/ecmcSafetyPlgWrap.cpp b/src/ecmcSafetyPlgWrap.cpp index 52ef567..39e3370 100644 --- a/src/ecmcSafetyPlgWrap.cpp +++ b/src/ecmcSafetyPlgWrap.cpp @@ -17,6 +17,9 @@ #include #include #include +#include + +#include #include "ecmcSafetyPlgWrap.h" #include "ecmcSafetyGroup.h" @@ -27,7 +30,17 @@ static std::vector safetyGroups; static int safetyGroupsCounter = 0; static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS]; -int createSafetyGroup(char* configStr) { +static const char *configString = NULL; + +int setCfgString(const char* cfgString) { + configString = cfgString; + return 0; +} + +int createSafetyGroup(const char *name, + const char *ec_rampdown_cmd, + const char *ec_standstill_status, + int time_delay_ms) { // create new ecmcSafetyGroup object ecmcSafetyGroup* safetyGroup = NULL; @@ -37,13 +50,17 @@ int createSafetyGroup(char* configStr) { snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS, ECMC_PLUGIN_PORTNAME_PREFIX "_%d", safetyGroupsCounter); try { - safetyGroup = new ecmcSafetyGroup(safetyGroupsCounter, configStr, portNameBuffer); + safetyGroup = new ecmcSafetyGroup(name, + ec_rampdown_cmd, + ec_standstill_status, + time_delay_ms, + configString, + portNameBuffer); } catch(std::exception& e) { if(safetyGroup) { delete safetyGroup; } - printf("Exception: %s. Plugin will unload.\n",e.what()); return ECMC_PLUGIN_SAFETY_ERROR_CODE; } @@ -53,7 +70,26 @@ int createSafetyGroup(char* configStr) { return 0; } +int addAxisToSafetyGroup(const char *groupName, + int axisId, + double veloLimit, + int standStillTimeMs) { + // Find group by name + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { + bool found = strcmp(groupName, (*psafetyGroup)->getName().c_str()) == 0; + if(found) { + (*psafetyGroup)->addAxis(axisId,veloLimit,standStillTimeMs); + return asynSuccess; + } + } + + return asynError; +} + void deleteAllSafetyGroups() { + + return; // The delete process results in seg fault.. need to investigate.. + for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { if(*psafetyGroup) { delete (*psafetyGroup); @@ -61,22 +97,11 @@ void deleteAllSafetyGroups() { } } -int addMotionAxis(int safetyGroupIndex, int axisIndex) { - try { - safetyGroups.at(safetyGroupIndex)->addAxis(axisIndex); - } - catch(std::exception& e) { - printf("Exception: %s. Safety object index out of range.\n",e.what()); - return ECMC_PLUGIN_SAFETY_ERROR_CODE; - } - return 0; -} - -int linkDataToSafetyGroups() { +int validate() { for(std::vector::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) { if(*psafetyGroup) { try { - (*psafetyGroup)->connectToDataSource(); + (*psafetyGroup)->validate(); } catch(std::exception& e) { printf("Exception: %s. Plugin will unload.\n",e.what()); @@ -102,3 +127,150 @@ int executeSafetyGroups() { } return 0; } + +/** + * EPICS iocsh shell command: ecmcAddSafetyGroup +*/ +void ecmcAddSafetyGroupPrintHelp() { + printf("\n"); + printf(" Use ecmcAddSafetyGroup(, , ,)\n"); + printf(" : Name of group.\n"); + printf(" : Ethercat entry input for rampdown cmd.\n"); + printf(" : Ethercat entry output for group standstill status.\n"); + printf(" : Time delay of STO [ms].\n"); + printf("\n"); +} + +int ecmcAddSafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) { + if(!name) { + printf("Error: name.\n"); + ecmcAddSafetyGroupPrintHelp(); + return asynError; + } + + if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) { + ecmcAddSafetyGroupPrintHelp(); + return asynSuccess; + } + + if(!ec_rampdown_cmd) { + printf("Error: ec_rampdown_cmd ethercat entry not defined.\n"); + ecmcAddSafetyGroupPrintHelp(); + return asynError; + } + + if(!ec_standstill_status) { + printf("Error: ec_standstill_status ethercat entry not defined.\n"); + ecmcAddSafetyGroupPrintHelp(); + return asynError; + } + if(time_delay_ms <= 0) { + printf("Error: time_delay invalid.\n"); + exit (1); + } + + try { + createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms); + } + catch(std::exception& e) { + printf("Exception: %s. Add safety group failed.\n",e.what()); + exit (1); + } + + return asynSuccess; +} + +static const iocshArg initArg0_1 = +{ "Name", iocshArgString }; +static const iocshArg initArg1_1 = +{ "ec entry input ramp down cmd", iocshArgString }; +static const iocshArg initArg2_1 = +{ "ec entry output axes standstill status", iocshArgString }; +static const iocshArg initArg3_1 = +{ "STO delay [ms]", iocshArgInt }; + +static const iocshArg *const initArgs_1[] = { &initArg0_1, + &initArg1_1, + &initArg2_1, + &initArg3_1}; + +static const iocshFuncDef initFuncDef_1 = { "ecmcAddSafetyGroup", 4, initArgs_1 }; +static void initCallFunc_1(const iocshArgBuf *args) { + ecmcAddSafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival); +} + +/** + * EPICS iocsh shell command: ecmcAddAxisToSafetyGroup +*/ +void ecmcAddAxisToSafetyGroupPrintHelp() { + printf("\n"); + printf(" Use ecmcAddAxisToSafetyGroup(, )\n"); + printf(" : Name of safety group.\n"); + printf(" : Axis index to add.\n"); + printf(" : Axis standstill velo limit [unit of axis].\n"); + printf("