From 7431834bd32be1d00b60575e4356c2c403746cf9 Mon Sep 17 00:00:00 2001 From: sandst_a Date: Mon, 24 Feb 2025 14:02:20 +0100 Subject: [PATCH] Test --- GNUmakefile | 7 +- iocsh/cfg/axis.yaml | 219 +++++++++---------------------- iocsh/cfg/enc_open_loop.yaml | 12 ++ iocsh/test_4ax_box.script | 22 ++-- iocsh/test_lab_rack.script | 75 +++++++++++ scripts/addAxisToSafetyGroup.cmd | 4 +- scripts/addSS1Group.cmd | 14 +- src/ecmcPluginSafety.c | 209 +++-------------------------- src/ecmcSafetyPlgWrap.h | 8 +- 9 files changed, 195 insertions(+), 375 deletions(-) create mode 100644 iocsh/cfg/enc_open_loop.yaml create mode 100644 iocsh/test_lab_rack.script diff --git a/GNUmakefile b/GNUmakefile index 6a4533a..26c2810 100644 --- a/GNUmakefile +++ b/GNUmakefile @@ -2,11 +2,14 @@ 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% # Run 7.0.6 for now -EXCLUDE_VERSIONS+=3 7.0.5 7.0.6 +EXCLUDE_VERSIONS+=3 7.0.5 7.0.6 7.0.7 IGNORE_MODULES += asynMotor IGNORE_MODULES += motorBase @@ -16,7 +19,7 @@ OPT_CXXFLAGS_YES = -O3 # dependencies ECmasterECMC_VERSION = v1.1.0 -ecmc_VERSION = 9.6 +ecmc_VERSION = v10.0.0_RC1 ################################################################################ # THIS RELATES TO THE EtherCAT MASTER LIBRARY diff --git a/iocsh/cfg/axis.yaml b/iocsh/cfg/axis.yaml index 0c31463..da7c169 100644 --- a/iocsh/cfg/axis.yaml +++ b/iocsh/cfg/axis.yaml @@ -1,189 +1,94 @@ 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: 'powerAutoOnOff=2;powerOffDelay=4;powerOnDelay=4;' # 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 + id: ${AXIS_ID=1} # Axis id + parameters: 'powerAutoOnOff=2;powerOffDelay=-1;' +# feedSwitchesOutput: ec0.s${BO_SID}.binaryOutput${BO_CH=01} # Ethercat entry for feed switches epics: - name: Axis1 # Axis anme + name: ${AX_NAME=M1} # Axis anme precision: 3 # Decimal count - description: very important motor axis # Axis description + description: Test cfg # Axis description unit: mm # Unit motorRecord: - enable: true - description: This is MR - fieldInit: 'TWV=1000' # Extra config for Motor record + fieldInit: 'RTRY=0,FOFF=Frozen' # 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 + numerator: 10 # Fastest speed in eng. units (2000 Fullsteps/s==10mm/s) + denominator: 32768 # I/O range for ECMC_EC_ALIAS_DRV_VELO_SET (normally +-16bit) + type: 0 # Stepper: 0. DS402: 1 (DS402 = servos and advanced stepper drives) + setpoint: ec0.s$(DRV_SID).velocitySetpoint01 # Velocity setpoint if CSV. Position setpoint if CSP + control: ec0.s$(DRV_SID).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) + reset: 1 # Reset bit in control word (if no drive reset bit then leave empty) + reduceTorque: 2 # Reduce torque bit in drive control word + reduceTorqueEnable: True # Enable reduce torque functionality + status: ec0.s$(DRV_SID).driveStatus01 # Status word ethercat entry + enabled: 1 # Enabled bit index in status word (not used if DS402) + warning: 2 # Warning bit in status word (if no drive warning bit then leave empty) + error: # max 3 error bits in status word + - 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: 720 # 2mm Scaling numerator example 360 deg/rev - denominator: 8192 # 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$(ENC_SLAVE).positionActual01 # Ethercat entry for actual position input (encoder) - control: ec0.s$(ENC_SLAVE).encoderControl01 # mandatory only if 'reset' is used - status: ec0.s$(ENC_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: '' # 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: 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) - # 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 readinf drive internal homing seq ready (seq id 26) + desc: BISS-C + numerator: 1 # Scaling numerator example 1 mm/rev + denominator: 4096 # Scaling denominator example 4096 ticks per 360 degree + type: 1 # Type: 0=Incremental, 1=Absolute + bits: 32 # Total bit count of encoder raw data + absBits: 26 # Absolute bit count (for absolute encoders) always least significant part of 'bits' + absOffset: -15615 # Encoder offset in eng units (for absolute encoders) + position: ec0.s$(ENC_SID).positionActual${ENC_CH=01} # Ethercat entry for actual position input (encoder) + status: ec0.s$(ENC_SID).encoderStatus${ENC_CH=01} # mandatory only if 'warning' or 'error' are used + ready: 2 # Bit in encoder status word for encoder ready + warning: 0 # Warning (optional) + error: # max 3 (optional) + - 1 # Error 0 + delayComp: # Delay compensation for time between application of setpoint to reading of encoder (normally atleast 2 cycles) + time: 0 # Delay time between set and act [cycles] + enable: true # enable (defaults to 1 if not set) + controller: - Kp: 1 # Kp proportinal gain - Ki: 0.02 # Ki integral gain - Kd: 0 # Kd derivative gain - limits: - minIntegral: -10000 - maxIntegral: 10000 - - # 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 + Kp: 10 # Kp proportinal gain + Ki: 0 # Ki integral gain + Kd: 0 # Kd derivative gain trajectory: - # type: 1 # Default 0 = trapetz, 1 = S-curve (ruckig) axis: - velocity: 500 # Default velo for axis - acceleration: 200 # Default acc for axis - deceleration: 200 # Default dec for axis - emergencyDeceleration: 1200 # Deceleration when axis in error state + velocity: 2 # Default velo for axis + acceleration: 2 # Default acc for axis + deceleration: 2 # Default dec for axis + emergencyDeceleration: 5 # 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 + velocity: 1 # Default velo fro JOG (motor record) input: limit: - 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).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 - 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 + forward: ec0.s$(DRV_SID).driveStatus01.12 # Ethercat entry for low limit switch input + backward: ec0.s$(DRV_SID).driveStatus01.11 # Ethercat entry for high limit switch input + home: 'ec0.s$(DRV_SID).ONE.0' # Ethercat entry for home switch + interlock: 'ec0.s$(DRV_SID).ONE.0' # Ethercat entry for interlock switch input 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 + enable: true # Enable soft limits + forward: 30 # Soft limit position fwd + forwardEnable: true # Soft limit position fwd enable + backward: -30 # Soft limit position bwd + backwardEnable: true # Soft limit position bwd enable monitoring: lag: enable: true # Enable position lag monitoring (following error) - tolerance: 10 # Allowed tolerance - time: 10 # Allowed time outside tolerance target: - target: - enable: true # Enable at target monitoring (needs to be enabled if using motor record) tolerance: 0.1 # Allowed tolerance - time: 10 # Filter time inside tolerance to be at target + time: 10 # Allowed time outside tolerance target: velocity: - enable: true # Enable velocity monitoring - max: 1000 # Allowed max velocity + enable: false # Enable velocity monitoring + max: 8 # 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 + target: + enable: true # Enable at target monitoring (needs to be enabled if using motor record) + tolerance: 0.01 # Allowed tolerance + time: 10 # Filter time inside tolerance to be at target diff --git a/iocsh/cfg/enc_open_loop.yaml b/iocsh/cfg/enc_open_loop.yaml new file mode 100644 index 0000000..822eacc --- /dev/null +++ b/iocsh/cfg/enc_open_loop.yaml @@ -0,0 +1,12 @@ +encoder: + desc: 'Open loop' + unit: mm + numerator: 1 # Scaling numerator + denominator: 12800 # Scaling denominator + type: 0 # Type: 0=Incremental, 1=Absolute + bits: 16 # Total bit count of encoder raw data + absBits: 0 # Absolute bit count (for absolute encoders) + absOffset: 0 # Encoder offset in eng units (for absolute encoders) + position: ec0.s$(ENC_SID).positionActual01 # Ethercat entry for actual position input (encoder) + homing: + refToEncIDAtStartup: 1 # Ref encoder at startup (to BISS value) diff --git a/iocsh/test_4ax_box.script b/iocsh/test_4ax_box.script index 142e64f..cb0acf2 100644 --- a/iocsh/test_4ax_box.script +++ b/iocsh/test_4ax_box.script @@ -11,8 +11,7 @@ ## camon c6025a:SS1-first-Err c6025a:SS1-first-RmpDwnCmdAct c6025a:SS1-first-AxsStndStllAct ## -#epicsEnvSet(IOC,c6025a) -require ecmccfg "ENG_MODE=1,MASTER_ID=1" +require ecmccfg "ENG_MODE=1,MASTER_ID=0" ############################################################################## ## Load components lib @@ -54,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: # diff --git a/iocsh/test_lab_rack.script b/iocsh/test_lab_rack.script new file mode 100644 index 0000000..a5e8a26 --- /dev/null +++ b/iocsh/test_lab_rack.script @@ -0,0 +1,75 @@ +############################################################################## +#- test config for ecmc_plugin_safety +#- +#- In this config the interface to safety system is liked to simulated ethercat entries: +#- * To allow motion: +#- caput c6025a:m0s013-Zero 3 +#- * To simulate interlock from safety system: +#- caput c6025a:m0s013-Zero 0 +#- +#- Monitor status with: +#- camon c6025a:SS1-first-Err c6025a:SS1-first-RmpDwnCmdAct c6025a:SS1-first-AxsStndStllAct +#- +#- ethercat slaves +#- 0 0:0 PREOP + EK1100 EtherCAT Coupler (2A E-Bus) +#- 1 0:1 PREOP + EL2068 8Ch. Dig Output 24V, 0.5A +#- 2 0:2 PREOP + EL2819 16K. Dig. Ausgang 24V, 0,5A, Diagnose +#- 3 0:3 PREOP + EL5001 1K. SSI Encoder +#- 4 0:4 PREOP + EL6224 (IO Link Master) +#- 5 0:5 PREOP + EL2008 8K. Dig. Ausgang 24V, 0.5A +#- 6 0:6 PREOP + EL2522 2K. Pulse Train Ausgang +#- 7 0:7 PREOP + EL5072 2Ch. Inductive sensor interface (LVDT, Half Bridge) +#- 8 0:8 PREOP + EL1008 8K. Dig. Eingang 24V, 3ms +#- 9 0:9 PREOP + EL5042 2Ch. BiSS-C Encoder +#- 10 0:10 PREOP + EL6692 EtherCAT Bridge-Klemme (Prim�r) +#- 11 0:11 PREOP + EK1100 EtherCAT-Koppler (2A E-Bus) +#- 12 0:12 PREOP + EL7031 1K. Schrittmotor-Endstufe (24V, 1.5A) +#- 13 0:13 PREOP + EL7201 1K. MDP742 Servo-Motor-Endstufe (50V, 4A) +#- 14 0:14 PREOP + EL7041-0052 1Ch. Stepper motor output stage (50V, 5A) +#- 15 0:15 PREOP + EL7342 2Ch. DC motor output stage (50V, 3.5A) +#- 16 0:16 PREOP + EL7411 BLDC Terminal with incremental encoder/Hall, 50 V DC, 4. +#- 17 0:17 PREOP + EL7211-9014 1K. MDP742 Servo-Motor-Endstufe mit OCT (50V, 4,5A +#- 18 0:18 PREOP + EL9576 Bremschopper Klemme + +require ecmccfg v10.0.0_RC1 "ENG_MODE=1,MASTER_ID=0,ECMC_VER=v10.0.0_RC1" + +# 0:14 - EL7041 1Ch Stepper +${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=14,HW_DESC=EL7041-0052" +${SCRIPTEXEC} ${ecmccfg_DIR}applyComponent.cmd "COMP=Motor-Generic-2Phase-Stepper, MACROS='I_MAX_MA=1500, I_STDBY_MA=1000, U_NOM_MV=48000, R_COIL_MOHM=1230'" +epicsEnvSet(DRV_SID,${ECMC_EC_SLAVE_NUM}) + +# 0:9 - EL5042 2Ch BiSS-C Encoder, RLS-LA11 +${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=9,HW_DESC=EL5042" +${SCRIPTEXEC} ${ecmccfg_DIR}applyComponent.cmd "COMP=Encoder-RLS-LA11-26bit-BISS-C,CH_ID=1" +${SCRIPTEXEC} ${ecmccfg_DIR}applyComponent.cmd "COMP=Encoder-RLS-LA11-26bit-BISS-C,CH_ID=2" +epicsEnvSet(ENC_SID,${ECMC_EC_SLAVE_NUM}) + +${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml, DEV=${IOC}, AX_NAME=M1, AXIS_ID=1, DRV_SID=${DRV_SID}, ENC_SID=${ENC_SID}, ENC_CH=01" +${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlEnc.cmd, "FILE=./cfg/enc_open_loop.yaml, DEV=${IOC}, ENC_SID=${DRV_SID}" + + +############################################################################## +## Load safety plugin +require ecmc_plugin_safety v10.0.0_RC1 + +# simulate inputs and outputs in drive "ZERO" and ONE dummy-entry +epicsEnvSet(BI_SLAVE,${DRV_SID}) +epicsEnvSet(BO_SLAVE,${DRV_SID}) + +# Create SS1 group +#- EC_RAMP_DOWN_CMD : Digital Input: Ethercat entry for ramp down command, input to ecmc (command from safety PLC/system) +#- EC_AXES_AT_REST_STAT : Digital Output: Ethercat entry for signaling that all axes in group are at rest, output from ecmc (feedback to safety PLC/system) +#- EC_AXES_LIM_VELO_CMD : Digital Input: 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}.ONE.0") +epicsEnvSet(EC_AXES_AT_REST_STAT,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.ZERO.0") +epicsEnvSet(EC_AXES_LIM_VELO_CMD,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.ONE.1") +epicsEnvSet(SAFETY_TIMEOUT,500) +${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN_CMD=${EC_RAMP_DOWN_CMD},EC_AXES_AT_REST_STAT=${EC_AXES_AT_REST_STAT},EC_AXES_LIM_VELO_CMD=${EC_AXES_LIM_VELO_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] +${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_REST_LIM=0.01,VELO_MAX_LIM=0.1" + diff --git a/scripts/addAxisToSafetyGroup.cmd b/scripts/addAxisToSafetyGroup.cmd index c845745..ae08c45 100644 --- a/scripts/addAxisToSafetyGroup.cmd +++ b/scripts/addAxisToSafetyGroup.cmd @@ -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 of safety group to add axis to. #- : Axis index to add (ecmc axis index). #- : Axis at rest velo limit [unit of axis]. -#- : Axis max velo limit [unit of axis]. +#- : Velocity maximum limit when EC_RED_VEL_CMD, set to -1 to disable [unit same as EGU of axis] #- : 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}) diff --git a/scripts/addSS1Group.cmd b/scripts/addSS1Group.cmd index b677a4e..bca6bb4 100644 --- a/scripts/addSS1Group.cmd +++ b/scripts/addSS1Group.cmd @@ -15,11 +15,11 @@ #-############################################################################### #- #- Arguments: -#- NAME : Name of safety 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 +#- NAME : Name of safety group +#- EC_RAMP_DOWN_CMD : Ethercat entry for ramp down command, input to ecmc (command from safety PLC/system) +#- EC_AXES_REST_STAT : Ethercat entry for signaling that all axes in group are at rest, output from ecmc (feedback to safety PLC/system) +#- EC_AXES_LIM_VELO_CMD : Ethercat entry for reducing velocity, input to ecmc (command from safety PLC/system) +#- DELAY_MS : Time between rampdown command and STO #- ################################################################################# @@ -27,10 +27,10 @@ #- : Name of group #- : Ethercat entry input for rampdown cmd #- : Ethercat entry output for group standstill status -#- : Ethercat entry input for activation of maximum velo limitation (set to "empty" to disable) +#- : Ethercat entry input for activation of maximum velo limitation (set to "empty" to disable) #- : Time delay of STO [ms] -ecmcAddSS1SafetyGroup("${NAME}","${EC_RAMP_DOWN}","${EC_AXES_REST}","${EC_AXES_MAX_VELO=empty}",${DELAY_MS=0}) +ecmcAddSS1SafetyGroup("${NAME}","${EC_RAMP_DOWN_CMD}","${EC_AXES_AT_REST_STAT}","${EC_AXES_LIM_VELO_CMD=empty}",${DELAY_MS=0}) #- Load SS1 group records dbLoadRecords("ss1.template","P=${ECMC_PREFIX},NAME=${NAME}") diff --git a/src/ecmcPluginSafety.c b/src/ecmcPluginSafety.c index c24349d..ac15844 100644 --- a/src/ecmcPluginSafety.c +++ b/src/ecmcPluginSafety.c @@ -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.. }; diff --git a/src/ecmcSafetyPlgWrap.h b/src/ecmcSafetyPlgWrap.h index b9592b0..a20dd13 100644 --- a/src/ecmcSafetyPlgWrap.h +++ b/src/ecmcSafetyPlgWrap.h @@ -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();