depend on ecmc 9

.vscode/settings.json

@@ -2,6 +2,7 @@
     "files.associations": {
         "string_view": "cpp",
         "stdexcept": "cpp",
-        "iosfwd": "cpp"
+        "iosfwd": "cpp",
+        "typeinfo": "cpp"
     }
 }

Db/ss1.template

@@ -51,4 +51,15 @@ record(bi,"${P}SS1-${NAME}-AxsStndStllAct"){
   field(ONAM, "Standstill")
   field(ZSV,  "NO_ALARM")
   field(OSV,  "NO_ALARM")
+  field(FLNK, "${P}SS1-${NAME}-RedVeloAct.PROC")
+}
+
+#  // bit 3 reduce velo active
+record(bi,"${P}SS1-${NAME}-RedVeloAct"){
+  field(DESC, "SS1-${NAME}: Reduce velo active")
+  field(INP,  "${P}SS1-${NAME}-Stat_.B3")
+  field(ZNAM, "Not Active")
+  field(ONAM, "Active")
+  field(ZSV,  "NO_ALARM")
+  field(OSV,  "NO_ALARM")
 }

GNUmakefile

@@ -16,9 +16,7 @@ OPT_CXXFLAGS_YES = -O3
 
 # dependencies
 ECmasterECMC_VERSION = v1.1.0
-# motorECMC_VERSION = 7.0.7-ESS
-#ecmc_VERSION = v9.0.1_RC4
-ecmc_VERSION = 9.1.0
+ecmc_VERSION = 9
 
 ################################################################################
 # THIS RELATES TO THE EtherCAT MASTER LIBRARY

README.md

@@ -10,14 +10,22 @@ This plugin is designed for interfacing safety systems, see example in below pic
 # SS1-t
 The plugin supports stopping axes according to a concept described as SS1-t, see below picture. In SS1-t the STO (or removal of power) from the axes are delayed for a certain defined time allowing a controlled rampdown of velocity. The rampdown of the axis is handled by the non-safe motion controller while the removal of power (or triggering of STO) after the time delay is handled by a safety system, . This makes it possible to stop moving axes in a controlled way and disable the drives before the power is interrupted (or STO triggered). This will result in a safer system and less harware failures and error messages.
 
- After the time delay expires, hopfully all axes are at standstill and power is already removed from the drives.  
+After the time delay expires, hopfully all axes are at standstill and power is already removed from the drives.  
 
 ![SS1-t](docs/SS1-t.png)
 
+## Velocity limit
+An additional optional feature of this plugin is limiting of axis velocity based on an the state of an ethercat I/O (this is not a safety certified fucntionality).
+
+Axes that are configured with a max velocity will be monitored by this plugin and if exceeding the configured max velocity, the axis will be disabled.
+
+For initiation of new movements, the target velocity will be limited to 95% of the configured maximum velocity. The reason for this is toi allow small fluctuations in actual velocity without disablaing the axes.
+
 ## Interface
-Basically the safey system is interfaced with two binary signals (ethercat I/O):
-* Ramp down command (from safety system to ecmc)
-* Axis stand still status (to safety system from ecmc)
+Basically the safey system is interfaced with three binary signals (ethercat I/O):
+* Ramp down command (from safety system to ecmc). 0 means ramp down command is active.
+* Axis stand still status (to safety system from ecmc). 1 means that all axes are at rest _AND_ disabled
+* Limitation of velocity (optional). 0 means limitation of velocity is acrive
 
  If, for instance, an safety event is triggerd by the safety system, it will immediately command this plugin to rampdown velocity of all axes (that a configured to stop). When all axes, that are configured to rampdown, have stopped then this plugin will disable the axes and set an ethercat output informing the safety system that the axes are standstill. After a certain timout the safety system will make sure power is removed from the motion axes by triggering an STO or removing power. The removal of power or triggering of STO will made regardless if the axes are at rest or not. A reset of the safety system, allowing power to the drives, will only be possible once the safety system gets a confirmation that all axes are at rest.
 
@@ -29,11 +37,12 @@ The configuration is made by two commands:
 
 The ecmcAddSS1SafetyGroup() adds a SS1 safety group. The command takes the following parameters:
 ```
-ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)
-          <name>                     : Name of group.
-          <ec_rampdown>              : Ethercat entry input for rampdown cmd.
-          <ec_axes_at_rest>          : Ethercat entry output for group standstill status.
-          <time_delay_ms>            : Time delay of STO [ms].
+ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<ec_max_velo_cmd>,<time_delay_ms>)
+        <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)
+        <time_delay_ms>            : Time delay of STO [ms] 
 ```
 Each group is interfacing the safety system through the same I/O (ethercat).
 Axes that needs the SS1 fucntionality can be added to this group with the ecmcAddAxisToSafetyGroup() command.
@@ -41,19 +50,21 @@ Axes that needs the SS1 fucntionality can be added to this group with the ecmcAd
 This command is also wrapped in a snippet addSS1Group.cmd with the follwoing parameters:
 * NAME               : Name of group
 * EC_RAMP_DOWN       : Ethercat input of ramp down signal from safety PLC
-* EC_AXES_STANDSTILL : Ethercat output for signaling safetysystem that all axes are at rest.
+* 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), put 0 to disable
 * DELAY_MS           : Safety system delay time of STO or removal of power
 
 Example:
 ```
 # Create SS1 group
 epicsEnvSet(EC_RAMP_DOWN,"ec${ECMC_EC_MASTER_ID}.s${BI_SLAVE}.binaryInput08.0")
-epicsEnvSet(EC_AXES_STANDSTILL,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.binaryOutput07.0")
+epicsEnvSet(EC_AXES_REST,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.binaryOutput07.0")
 epicsEnvSet(SAFETY_TIMEOUT,500)
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN=${EC_RAMP_DOWN},EC_AXES_STANDSTILL=${EC_AXES_STANDSTILL},DELAY_MS=${SAFETY_TIMEOUT}"
+${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN=${EC_RAMP_DOWN},EC_AXES_REST=${EC_AXES_REST},DELAY_MS=${SAFETY_TIMEOUT}"
 
 ```
 ## ecmcAddAxisToSafetyGroup()
+
 With the "ecmcAddAxisToSafetyGroup()" command an ecmc axis can be added to a safety group.
 All axes linked to a certain group will act on command from the safety system (initiation of velocity rampdown).
 
@@ -62,7 +73,8 @@ The ecmcAddAxisToSafetyGroup() command takes the following parameters:
 ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>, <velo_limit>, <filter_time>)
           <name>                     : Name of safety group to add axis to.
           <Axis id>                  : Axis index to add (ecmc axis index).
-          <velo limit>               : Axis standstill velo limit [unit of axis].
+          <velo_rest_limit>          : Axis at rest velo limit [unit of axis].
+          <velo_max_limit>           : Axis max velo limit [unit of axis].
           <filter_time>              : NOT USED (for future implemenation). Time for axis to be below velo limit [ms].
 ```
 Note: The "filter_time" parameter is not used right now. As soon as the axis is below teh velo_limit it will be considered to stand still and will then be disabled. 
@@ -70,13 +82,14 @@ Note: The "filter_time" parameter is not used right now. As soon as the axis is
 Note: The plugin checks the trajectory generated velocity setpoint and not the actual velocity.
 
 This command is also wrapped in a snippet addAxisToSafetyGroup.cmd with the following parameters:
-* NAME        : Name of group to add axis to (group must be created first with addSS1Group.cmd)
-* AX_ID       : ecmc axis index of axis to add
-* VELO_LIM    : Velocity limit [EGU of axis], default 0. If setpoint and actual velocity is lower or equal than VELO_LIM the axis is considered to be at rest
+* NAME            : Name of group to add axis to (group must be created first with addSS1Group.cmd)
+* AX_ID           : ecmc axis index of axis to add
+* 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]
 
 Example:
 ```
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_LIM=1"
+${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_REST_LIM=1,VELO_MAX_LIM=100"
 ```
 
 # Example of startup script:
@@ -84,17 +97,26 @@ ${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_I
 ##############################################################################
 ## Load safety plugin
 #
+
 require ecmc_plugin_safety sandst_a
 
-# Create SS1 group
-epicsEnvSet(RAMP_DOWN_CMD,"ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ZERO.0")
-epicsEnvSet(STANDSTILL_STAT,"ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ZERO.1")
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN_CMD=${RAMP_DOWN_CMD},EC_STANDSTILL_STAT=${STANDSTILL_STAT},DELAY_MS=500"
 
-# Add axes to group "first"
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd.cmd "NAME=first,AX_ID=1,VELO_LIM=1"
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd.cmd "NAME=first,AX_ID=2,VELO_LIM=1"
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd.cmd "NAME=first,AX_ID=3,VELO_LIM=1"
+# 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), set to "empty" to disable
+#-   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")
+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},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=1,VELO_MAX_LIM=100"
 ....
 ```
 
@@ -107,3 +129,20 @@ Triggering of e-stop:
 
 ![Velocity](docs/velo.png)
 ![Digital](docs/digital.png)
+
+# Test at Debye
+
+```
+libversionShow
+require            3.3.1     /gfa/.mounts/sls_ioc/modules/require/3.3.1/R7.0.7/             
+misc               2.15.0    /gfa/.mounts/sls_ioc/modules/misc/2.15.0/R7.0.7/               
+ecmccfg            safety3   /gfa/.mounts/sls_ioc/modules/ecmccfg/safety3/R7.0.7/           
+ECmasterECMC       v1.1.0    /gfa/.mounts/sls_ioc/modules/ECmasterECMC/v1.1.0/R7.0.7/       
+calc               3.7.6     /gfa/.mounts/sls_ioc/modules/calc/3.7.6/R7.0.7/                
+asyn               4.42.2    /gfa/.mounts/sls_ioc/modules/asyn/4.42.2/R7.0.7/               
+motorECMC          7.0.9-ESS /gfa/.mounts/sls_ioc/modules/motorECMC/7.0.9-ESS/R7.0.7/       
+ruckig             0.6.3     /gfa/.mounts/sls_ioc/modules/ruckig/0.6.3/R7.0.7/              
+ecmc               safety3   /gfa/.mounts/sls_ioc/modules/ecmc/safety3/R7.0.7/              
+ecmccomp           0.1.0     /gfa/.mounts/sls_ioc/modules/ecmccomp/0.1.0/R7.0.7/            
+ecmc_plugin_safety safety3   /gfa/.mounts/sls_ioc/modules/ecmc_plugin_safety/safety3/R7.0.7/
+```

iocsh/cfg/axis.yaml

@@ -1,10 +1,8 @@
-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
+  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)
@@ -23,10 +21,10 @@ epics:
   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
+  motorRecord:
+    enable: true
+    description: This is MR
+    fieldInit: 'TWV=1000' # Extra config for Motor record
 
 drive:
   numerator: 3600                                     # Fastest speed in engineering units
@@ -37,31 +35,31 @@ drive:
   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
+  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   (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
+  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$(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
+  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)
@@ -102,6 +100,10 @@ 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
@@ -167,15 +169,15 @@ softlimits:
 monitoring:
   lag:
     enable: true                                      # Enable position lag monitoring (following error)
-    tolerance: 2                                      # Allowed tolerance
+    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.5                                    # Allowed tolerance
+    tolerance: 0.1                                    # Allowed tolerance
     time: 10                                          # Filter time inside tolerance to be at target
   velocity:
-    enable: false                                     # Enable velocity monitoring
-    max: 100                                          # Allowed max velocity
+    enable: true                                     # Enable velocity monitoring
+    max: 1000                                          # 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
@@ -185,20 +187,3 @@ monitoring:
   #   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

iocsh/plc/plc_cfg.yaml

@@ -1,9 +0,0 @@
-macros: LIMIT=1000,TYPE=1                             # Macros to pass to this file (also plc.file)
-plc:
-  id: 1
-  enable: yes
-  rateMilliseconds: 1
-  file: ${PLC_PATH}test.plc
-  code:
-  - "if(static.counter % ${LIMIT} == 0){println('PLC:    Appended');static.counter:=0;};" # calculate set pos for physical axis
-  - "if(not(static.plc_code_loaded)) {static.counter:=static.counter+1;};"

iocsh/plc/plc_no_ec_fft_sin.plc

@@ -1,9 +0,0 @@
-###############################################################################################
-# For help on syntax, variables and functions, please read the file: "plcSyntaxHelp.plc"
-#
-# PLC Functionality Demo:
-#   No hardware related variables
-#
-
-static.time:=ec_get_time()/1E9;
-static.sineval:=sin(2*pi*${FREQ=10}*static.time);

iocsh/plc/test.plc

@@ -1,9 +0,0 @@
-static.counter:=static.counter+1;
-static.plc_code_loaded:=1;
-if(static.counter % ${LIMIT} == 0) {    
-  if(${TYPE}=0) {
-    println('AX PLC: File');
-  } else {
-    println('PLC   : File');
-  };
-};

iocsh/test.script

@@ -1,74 +0,0 @@
-##############################################################################
-## 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
-##
-
-epicsEnvSet(IOC,c6025a)
-require ecmccfg "ENG_MODE=1"
-
-##############################################################################
-## Load components lib
-#
-require ecmccomp
-
-##############################################################################
-## Configure hardware
-
-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"
-${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Encoder-RLS-LA11-24bit-BISS-C"
-
-#Apply hardware configuration
-ecmcConfigOrDie "Cfg.EcApplyConfig(1)"
-
-##############################################################################
-## AXIS 1
-#
-epicsEnvSet("DEV",      "$(IOC)")
-epicsEnvSet("PLC_PATH",      "/ioc/c6025a5a/ecmccfg/examples/test/ecmccomp/plc/")
-${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml,LIMIT=1000,TYPE=0"
-
-##############################################################################
-## PLC 1
-#
-#-${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlPlc.cmd, "FILE=./plc/plc_cfg.yaml,LIMIT=5000,TYPE=1"
-
-##############################################################################
-## Load safety plugin
-#
-require ecmc_plugin_safety
-
-# Create SS1 group
-epicsEnvSet(EC_RAMP_DOWN,"ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ZERO.0")
-epicsEnvSet(EC_AXES_STANDSTILL,"ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ZERO.1")
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN=${EC_RAMP_DOWN},EC_AXES_STANDSTILL=${EC_AXES_STANDSTILL},DELAY_MS=500"
-
-#- Add axis
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_LIM=1"
-
-##############################################################################
-## Configure diagnostics:
-#
-ecmcConfigOrDie "Cfg.EcSetDiagnostics(1)"
-ecmcConfigOrDie "Cfg.EcEnablePrintouts(0)"
-ecmcConfigOrDie "Cfg.EcSetDomainFailedCyclesLimit(100)"
-ecmcConfigOrDie "Cfg.SetDiagAxisIndex(1)"
-ecmcConfigOrDie "Cfg.SetDiagAxisFreq(2)"
-ecmcConfigOrDie "Cfg.SetDiagAxisEnable(0)"
-
-##############################################################################
-## go active
-#
-$(SCRIPTEXEC) ($(ecmccfg_DIR)setAppMode.cmd)

iocsh/test_4ax_box.script

@@ -11,8 +11,8 @@
 ##       camon c6025a:SS1-first-Err c6025a:SS1-first-RmpDwnCmdAct c6025a:SS1-first-AxsStndStllAct
 ##
 
-epicsEnvSet(IOC,c6025a)
-require ecmccfg "ENG_MODE=1"
+#epicsEnvSet(IOC,c6025a)
+require ecmccfg "ENG_MODE=1,MASTER_ID=1"
 
 ##############################################################################
 ## Load components lib
@@ -28,7 +28,8 @@ ${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Motor-Generic-2Phase-Stepp
 
 epicsEnvSet("ENC_SLAVE",              "2")
 ${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(ENC_SLAVE), HW_DESC=EL5042"
-${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Encoder-RLS-LA11-24bit-BISS-C"
+${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Encoder-RLS-LA11-26bit-BISS-C,CH_ID=1"
+${SCRIPTEXEC} ${ecmccomp_DIR}applyComponent.cmd "COMP=Encoder-RLS-LA11-26bit-SSI,CH_ID=2"
 
 epicsEnvSet("BO_SLAVE",              "5")
 ${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(BO_SLAVE), HW_DESC=EL2008"
@@ -45,7 +46,6 @@ ecmcConfigOrDie "Cfg.EcApplyConfig(1)"
 ## AXIS 1
 #
 epicsEnvSet("DEV",      "$(IOC)")
-epicsEnvSet("PLC_PATH",      "/ioc/c6025a5a/ecmccfg/examples/test/ecmccomp/plc/")
 ${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml"
 
 ##############################################################################
@@ -54,13 +54,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_STANDSTILL,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.binaryOutput07.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")
 epicsEnvSet(SAFETY_TIMEOUT,500)
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN=${EC_RAMP_DOWN},EC_AXES_STANDSTILL=${EC_AXES_STANDSTILL},DELAY_MS=${SAFETY_TIMEOUT}"
+${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}"
 
 #- Add axis
-${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_LIM=1"
+#-   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=1,VELO_MAX_LIM=100"
+
 
 ##############################################################################
 ## Configure diagnostics:

scripts/addAxisToSafetyGroup.cmd

@@ -17,14 +17,16 @@
 #- Arguments:
 #-   NAME               :   Name of safety group
 #-   AX_ID              :   Axis ID
-#-   VELO_LIM           :   Velocity standstill limit [unit same as EGU of axis]
+#-   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]
 #-
 #################################################################################
 
 #- ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>, <velo_limit>, <filter_time>)
 #-           <name>                     : Name of safety group to add axis to.
 #-           <Axis id>                  : Axis index to add (ecmc axis index).
-#-           <velo limit>               : Axis standstill velo limit [unit of axis].
+#-           <velo_rest_limit>          : Axis at rest velo limit [unit of axis].
+#-           <velo_max_limit>           : Axis max velo limit [unit of axis].
 #-           <filter_time>              : NOT USED (for future implemenation). Time for axis to be below velo limit [ms].
 
-ecmcAddAxisToSafetyGroup("${NAME}",${AX_ID},${VELO_LIM=0},0)
+ecmcAddAxisToSafetyGroup("${NAME}",${AX_ID},${VELO_REST_LIM=0},0,${VELO_MAX_LIM=0.0})

scripts/addSS1Group.cmd

@@ -16,18 +16,21 @@
 #-
 #- Arguments:
 #-   NAME               :   Name of safety group
-#-   EC_RAMP_DOWN       :   Ethercat entry for rampd down command, input to ecmc (command from safety PLC/system)
-#-   EC_AXES_STANDSTILL :   Ethercat entry for all axes in group at standstill, output from ecmc (feedback to safety PLC/system)
+#-   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
 #-
 #################################################################################
 
-#- ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)
-#-           <name>                     : Name of group.
-#-           <ec_rampdown>              : Ethercat entry input for rampdown cmd.
-#-           <ec_standstill>            : Ethercat entry output for group standstill status.
-#-           <time_delay_ms>            : Time delay of STO [ms].
-ecmcAddSS1SafetyGroup("${NAME}","${EC_RAMP_DOWN}","${EC_AXES_STANDSTILL}",${DELAY_MS=0})
+#-Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)
+#-   <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)
+#-   <time_delay_ms>            : Time delay of STO [ms]
+
+ecmcAddSS1SafetyGroup("${NAME}","${EC_RAMP_DOWN}","${EC_AXES_REST}","${EC_AXES_MAX_VELO=empty}",${DELAY_MS=0})
 
 #- Load SS1 group records
 dbLoadRecords("ss1.template","P=${ECMC_PREFIX},NAME=${NAME}")

src/ecmcSS1SafetyGroup.cpp

@@ -35,11 +35,12 @@ extern DBBASE *pdbbase;
  *    - runtime_error
 */
 ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
-                                 const char *ec_rampdown_cmd,
-                                 const char *ec_standstill_status,
-                                 int time_delay_ms,
-                                 const char *cfg_string,
-                                 char* portName) 
+                                       const char *ec_rampdown_cmd,
+                                       const char *ec_standstill_status,
+                                       const char *ec_limit_velo,
+                                       int time_delay_ms,
+                                       const char *cfg_string,
+                                       char* portName) 
                 : asynPortDriver(portName,
                    1, /* maxAddr */
                    asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
@@ -55,49 +56,61 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
                    0, /* Default priority */
                    0) /* Default stack size */
                    {
-  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;
+  sName_                    = strdup(name);
+  sEcRampDownCmdNameOrg_    = strdup(ec_rampdown_cmd);
+  sEcAxesStandStillStatOrg_ = strdup(ec_standstill_status);
+  sEcLimitVeloOrg_          = strdup(ec_limit_velo);
+  limitMaxVeloEnable_       = 1;
+  
+  if(strcmp(sEcLimitVeloOrg_, "empty") == 0) {
+    limitMaxVeloEnable_     = 0;
+  }
+  
+  sConfig_                  = strdup(cfg_string);
+  delayMs_                  = time_delay_ms;  
+  ptrStatus_                = (int*)&status_;
+  asynStatusId_             = -1;
+  axesCounter_              = 0;
+  ecmcSampleRateHz_         = getEcmcSampleRate();
+  dataSourcesLinked_        = 0;
+  ecMaster_                 = NULL;
+  ecEntryRampDown_          = NULL;
+  ecEntryStandstill_        = NULL;
+  ecEntryLimitVelo_         = NULL;
+  masterId_                 = 0;
+  slaveIdRampDown_          = 0;
+  bitRampDown_              = 0;
+  slaveIdStandStill_        = 0;
+  bitStandStill_            = 0;
+  aliasRampDown_[0]         = 0;  
+  aliasStandStill_[0]       = 0;
+  aliasLimitVelo_[0]        = 0;
+  rampDownCmd_              = 0;
+  axesAreStandstill_        = 0;
+  rampDownCmdOld_           = 0;
+  axesAreStandstillOld_     = 0;
+  printEnableStatus_        = 1;
+  limitVeloCmdOld_          = 0;
+  limitVeloCmd_             = 0;
+  axesDisabled_             = 0;
+  cfgDbgMode_               = 0;
   memset(&status_,0,sizeof(status_));
-  ptrStatus_            = (int*)&status_;
-  asynStatusId_         = -1;
-  axesCounter_          = 0;
-  ecmcSampleRateHz_     = getEcmcSampleRate();
-  dataSourcesLinked_    = 0;
-  ecMaster_             = NULL;
-  ecEntryRampDown_      = NULL;
-  ecEntryStandstill_    = NULL;
-  masterId_             = 0;
-  slaveIdRampDown_      = 0;
-  bitRampDown_          = 0;
-  slaveIdStandStill_    = 0;
-  bitStandStill_        = 0;
-  aliasRampDown_[0]     = 0;
-  aliasStandStill_[0]   = 0;
-  rampDownCmd_          = 0;
-  axesAreStandstill_    = 0;
-  rampDownCmdOld_       = 0;
-  axesAreStandstillOld_ = 0;
-  printEnableStatus_    = 1;
 
-  // Config defaults
-  cfgDbgMode_           = 0;
   parseConfigStr(cfg_string);
   initAsyn();
   if(cfgDbgMode_) {
-    printf("Safety %s: Safety group created:\n"
-           "    Type:                                      SS1\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",           
+    printf("Safety %s: Group created:\n"
+           "    Type:                                        SS1-t\n"
+           "    Name:                                        %s\n"
+           "    I/O for rampdown command from saftey PLC:    %s\n" 
+           "    I/O for axes standstill status:              %s\n"
+           "    I/O for velo limit command from safety PLC:  %s (%s)\n"
+           "    STO delay [ms]:                              %d\n"
+           "    Configuration string:                        %s\n",           
            sName_,sName_,sEcRampDownCmdNameOrg_,
-           sEcAxesStandStillStatOrg_,delayMs_,sConfig_);
+           sEcAxesStandStillStatOrg_,sEcLimitVeloOrg_,
+           limitMaxVeloEnable_ ? "enabled" : "disabled",
+           delayMs_,sConfig_);
   }
 }
 
@@ -109,9 +122,8 @@ ecmcSS1SafetyGroup::~ecmcSS1SafetyGroup() {
   free(sName_);
   free(sEcRampDownCmdNameOrg_);
   free(sEcAxesStandStillStatOrg_);
-  free(sEcRampDownCmdNameStrip_);
-  free(sEcAxesStandStillStatStrip_);
   free(sConfig_);
+  free(sEcLimitVeloOrg_);
 }
 
 void ecmcSS1SafetyGroup::parseConfigStr(const char *configStr) {
@@ -167,8 +179,20 @@ void ecmcSS1SafetyGroup::validateCfgs() {
                 &bitStandStill_)) {    
     throw std::runtime_error( "Safety: Parse error: Data source for standstill status.");
   }
+  
+  // init to masterIdRampDown if case !limitMaxVeloEnable_ 
+  int masterIdLimitVelo = masterIdStandStill;
+  if(limitMaxVeloEnable_) {
+    if(parseEcPath(sEcLimitVeloOrg_,
+                  &masterIdLimitVelo,
+                  &slaveIdLimitVelo_,
+                  aliasLimitVelo_,
+                  &bitLimitVelo_)) {    
+      throw std::runtime_error( "Safety: Parse error: Data source for limit velo.");
+    }
+  }
 
-  if(masterIdStandStill != masterIdRampDown ) {
+  if(masterIdStandStill != masterIdRampDown || masterIdLimitVelo != masterIdRampDown) {
     throw std::runtime_error( "Safety: Parse error: Master id for datasources different.");
   }
   masterId_ = masterIdStandStill;
@@ -180,6 +204,10 @@ void ecmcSS1SafetyGroup::validateCfgs() {
   if(bitStandStill_ < 0) {
     throw std::runtime_error( "Safety: Parse error: Standstill status, bit invalid.");
   }
+  
+  if(bitLimitVelo_ < 0) {
+    throw std::runtime_error( "Safety: Parse error: Limit velo, bit invalid.");
+  }
 }
 
 void ecmcSS1SafetyGroup::validateAxes() {  
@@ -226,6 +254,22 @@ void ecmcSS1SafetyGroup::validateAxes() {
     printf("Safety %s: Error, group empty (axis count zero)\n",sName_);
     throw std::runtime_error( "Safety: Error, empty group not allowed.");
   }
+
+  // Check if max velo enabled in axis
+  int axesMaxVeloEnabled = 0;
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
+    axesMaxVeloEnabled = (*saxis)->veloMaxLimitEnabled_ || axesMaxVeloEnabled;
+  }
+
+  if(limitMaxVeloEnable_ && !axesMaxVeloEnabled) {
+    printf("Safety %s: Error, Limit max velo enabled but no axis configured with max velo\n",sName_);
+    throw std::runtime_error( "Safety: Error, Limit max velo enabled but no axis configured with max velo");
+  }
+
+  if(!limitMaxVeloEnable_ && axesMaxVeloEnabled) {
+    printf("Safety %s: Error, Limit max velo disabled but axis configured with a max velo\n",sName_);
+    throw std::runtime_error( "Safety: Error, Limit max velo disabled but axis configured with a max velo");
+  }
 }
 
 void ecmcSS1SafetyGroup::connectToDataSources() {
@@ -247,6 +291,10 @@ void ecmcSS1SafetyGroup::connectToDataSources() {
     throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL.");
   }
 
+  if(bitRampDown_ >= ecEntryRampDown_->getBits()) {
+    throw std::runtime_error( "Safety: Bit out of range for rampdown ethercat entry.");
+  }
+
   // standstill
   slave = ecMaster_->findSlave(slaveIdStandStill_);
   if(!slave) {
@@ -255,30 +303,32 @@ void ecmcSS1SafetyGroup::connectToDataSources() {
 
   ecEntryStandstill_ = slave->findEntry(aliasStandStill_);
   if(!ecEntryStandstill_) {
-    throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL.");
+    throw std::runtime_error( "Safety: EtherCAT entry for standstill I/O NULL.");
   };
+
+  if(bitStandStill_ >= ecEntryStandstill_->getBits()) {
+    throw std::runtime_error( "Safety: Bit out of range for standstill ethercat entry.");
+  }
+
+  // Limit velo
+  if( limitMaxVeloEnable_ ) {
+    slave = ecMaster_->findSlave(slaveIdLimitVelo_);
+    if(!slave) {
+      throw std::runtime_error( "Safety: EtherCAT slave limit velo I/O NULL.");
+    }
   
+    ecEntryLimitVelo_ = slave->findEntry(aliasLimitVelo_);
+    if(!ecEntryLimitVelo_) {
+      throw std::runtime_error( "Safety: EtherCAT entry for limit velo I/O NULL.");
+    };
+
+    if(bitLimitVelo_ >= ecEntryLimitVelo_->getBits()) {
+      throw std::runtime_error( "Safety: Bit out of range for limit velo ethercat entry.");
+    }
+  }
   dataSourcesLinked_ = 1;
 
   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 ecmcSS1SafetyGroup::refreshAsyn() { 
@@ -318,9 +368,8 @@ std::string ecmcSS1SafetyGroup::to_string(int value) {
   return os.str();
 }
 
-// Executed by ecmc rt thread.
-void ecmcSS1SafetyGroup::execute() {
-  
+// Ramp down and disable if safety interlock
+void ecmcSS1SafetyGroup::exeRampDown() {
   uint64_t data = 0;
   // Read ramp down command from safety plc
   if(ecEntryRampDown_->readBit(bitRampDown_,
@@ -333,7 +382,7 @@ void ecmcSS1SafetyGroup::execute() {
   }
 
   rampDownCmdOld_ = rampDownCmd_;
-  rampDownCmd_ = data == 0;
+  rampDownCmd_    = data == 0;
 
   if(rampDownCmdOld_ != rampDownCmd_) {
     
@@ -353,8 +402,12 @@ void ecmcSS1SafetyGroup::execute() {
 
   // set safety interlock in ecmc
   setAxesSafetyInterlocks(rampDownCmd_);
+
+  axesDisabled_ = checkAxesDisabled();
+  
   // check if axes are standstill to safety PLC
   axesAreStandstill_ = checkAxesStandstillAndDisableIfNeeded();
+
   setAxesStandstillStatus(axesAreStandstill_);
  
   // Disable
@@ -365,6 +418,56 @@ void ecmcSS1SafetyGroup::execute() {
   }
 }
 
+// Limit velo if needed
+void ecmcSS1SafetyGroup::exeLimitVelo() {
+
+  if(!limitMaxVeloEnable_) {
+    return;
+  }
+
+    uint64_t data = 0;
+  // Read ramp down command from safety plc
+  if(ecEntryLimitVelo_->readBit(bitLimitVelo_,
+                               &data)) {
+    // Disable all axes
+    setAxesDisable();           // disable
+    setAxesSafetyInterlocks(0); // stop
+    setAxesStandstillStatus(0); // set output
+    throw std::out_of_range("Safety: Read limit velo cmd failed");
+  }
+
+  limitVeloCmdOld_ = limitVeloCmd_;
+  limitVeloCmd_    = data == 0;
+
+  if(limitVeloCmdOld_ != limitVeloCmd_) { 
+    // Update asyn status wd
+    status_.limitVeloCmdActive = limitVeloCmd_;
+    refreshAsyn();
+
+    resetPrintoutStatus();
+    if(cfgDbgMode_) {
+      if(limitVeloCmd_) {
+        printf("Safety %s: Limit velo cmd active\n",sName_);
+      } else {
+        printf("Safety %s: Limit velo cmd not active\n",sName_);
+      }
+    }
+  }
+  
+  if(limitVeloCmd_) {    
+    checkAxesMaxVeloAndDisableIfNeeded();
+  }
+
+  // Write velo limit and activation to ecmc axis object
+  setAxesMaxVelo();
+}
+
+// Executed by ecmc rt thread.
+void ecmcSS1SafetyGroup::execute() {
+  exeRampDown();
+  exeLimitVelo();
+}
+
 void ecmcSS1SafetyGroup::resetPrintoutStatus() {
   for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
     (*saxis)->printEnableStat_ = 1;    
@@ -384,9 +487,10 @@ void ecmcSS1SafetyGroup::setAxesStandstillStatus(int standstill) {
     status_.axesAtStandstill = standstill;
     refreshAsyn();
   }
-
+  
+  // Only write axis standstill bit if all axes are disabled
   if(ecEntryStandstill_->writeBit(bitStandStill_,
-                                  standstill > 0)) {
+                                 ((standstill > 0) && axesDisabled_) )) {
     throw std::out_of_range("Safety: Read rampdown cmd failed");
   }
   axesAreStandstillOld_ = standstill;
@@ -396,11 +500,66 @@ void ecmcSS1SafetyGroup::setAxesStandstillStatus(int standstill) {
 bool ecmcSS1SafetyGroup::checkAxesStandstill() {
   bool standstill = 1;
   for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
-    standstill= standstill && checkAxisStandstill((*saxis));    
+    standstill= standstill && checkAxisStandstill((*saxis));
   }
   return standstill;
 }
 
+// Check axes disabled
+bool ecmcSS1SafetyGroup::checkAxesDisabled() {
+  bool enabledsum = 1;
+  int enabled     = 0;
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {    
+    getAxisEnabled((*saxis)->axisId_,&enabled);
+    enabledsum = enabledsum && enabled;
+  }
+  return !enabledsum;
+}
+
+// Check max velo violation
+void ecmcSS1SafetyGroup::checkAxesMaxVeloAndDisableIfNeeded() {
+  if(!limitVeloCmd_) {
+    return;
+  }
+
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
+    if(!(*saxis)->veloMaxLimitEnabled_) {
+      break;
+    }
+    if(checkAxisMaxVelo((*saxis))) {
+      printf("Safety %s: Axis %d, velo too high, disabling axis.\n", sName_, (*saxis)->axisId_);
+      setAxisEnable((*saxis)->axisId_,0);
+    }
+  }
+  return;
+}
+
+// Set max velo in axis object
+void ecmcSS1SafetyGroup::setAxesMaxVelo() {
+  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
+    if( (*saxis)->veloMaxLimitEnabled_ ) {
+      setAxisExtMaxVelo((*saxis)->axisId_,0.95*(*saxis)->veloMaxLimit_,limitVeloCmd_);
+    }
+  }
+}
+
+bool ecmcSS1SafetyGroup::checkAxisMaxVelo(safetyAxis* axis) {
+  double traj = 1;
+
+  int err = getAxisTrajVelo(axis->axisId_, &traj);
+  if(err) {
+    return 0;
+  }
+
+  double enc = 1;
+  err = getAxisEncVelo(axis->axisId_, &enc);
+  if(err) {
+    return 0;
+  }
+
+  return std::abs(traj) >= axis->veloMaxLimit_ || std::abs(enc) >= axis->veloMaxLimit_;
+}
+
 // Check standstill axis
 bool ecmcSS1SafetyGroup::checkAxisStandstill(safetyAxis* axis) {
   double traj = 1;
@@ -416,7 +575,7 @@ bool ecmcSS1SafetyGroup::checkAxisStandstill(safetyAxis* axis) {
     return 0;
   }
 
-  return std::abs(traj) <= axis->veloLimit_ && std::abs(enc) <= axis->veloLimit_;
+  return std::abs(traj) <= axis->veloStandstillLimit_ && std::abs(enc) <= axis->veloStandstillLimit_;
 }
 
 // Check standstill and disable
@@ -447,7 +606,7 @@ void ecmcSS1SafetyGroup::setAxesDisable() {
     if(!*saxis) {
       throw std::runtime_error("Safety: Axis object NULL.");
     }
-    setAxisEnable((*saxis)->axisId_,0);    
+    setAxisEnable((*saxis)->axisId_,0);
   }
 
   if(printEnableStatus_ && cfgDbgMode_) {
@@ -466,19 +625,26 @@ void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) {
   }
 }
 
-void ecmcSS1SafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {
+void ecmcSS1SafetyGroup::addAxis(int axisId,
+                                 double veloStandstillLimit,
+                                 int standStillTimeMs,
+                                 double veloMaxLimit) {
 
   if(!getAxisValid(axisId)) {  
     throw std::out_of_range("Safety: Invalid axis id");
   }
   
-  axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs));  
+  axes_.push_back(new safetyAxis(axisId, veloStandstillLimit, 
+                                 standStillTimeMs, veloMaxLimit));
   axesCounter_++;
-  
-  
+
   if(cfgDbgMode_) {
-    printf("Safety %s: Added axis %d to safety group (velo limit = %lf, standstill filter : %d)\n"
-    ,sName_,axisId,veloLimit,standStillTimeMs);
+    printf("Safety %s: Added axis %d to safety group.\n"
+           "    Velo stand still limit: %lf\n"
+           "    velo max limit:         %lf (%s)\n"
+           "    standstill filter time: %d\n"
+    ,sName_,axisId,veloStandstillLimit,veloMaxLimit,
+    veloMaxLimit>0 ? "enabled" : "disabled", standStillTimeMs);
   }
 
   return;

src/ecmcSS1SafetyGroup.h

@@ -25,26 +25,45 @@ typedef struct {
   bool               error              : 1;
   bool               rampDownCmdActive  : 1;
   bool               axesAtStandstill   : 1;
-  int                dummy              : 29;
+  bool               limitVeloCmdActive : 1;
+  int                dummy              : 28;
 } ecmcSafetyStatusWd;
 
 class safetyAxis {
   public:
   safetyAxis(int axisId,
-             double veloLimit,
-             int standStillTimeMs) {
-    veloLimit_        = veloLimit;
-    axisId_           = axisId;    
-    standStillTimeMs_ = standStillTimeMs;
-    printEnableStat_  = 1;
+             double velostandstillLimit,             
+             int standStillTimeMs,
+             double veloMaxLimit) {
+    veloStandstillLimit_ = velostandstillLimit;
+    axisId_              = axisId;    
+    standStillTimeMs_    = standStillTimeMs;
+    veloMaxLimit_        = veloMaxLimit;
+    veloMaxLimitEnabled_ = veloMaxLimit > 0;
+    printEnableStat_     = 1;
   }
 
-  double  veloLimit_;
+  safetyAxis(int axisId,
+             double velostandstillLimit,             
+             int standStillTimeMs) {
+    veloStandstillLimit_ = velostandstillLimit;
+    axisId_              = axisId;    
+    standStillTimeMs_    = standStillTimeMs;
+    veloMaxLimit_        = -1;
+    veloMaxLimitEnabled_ = 0;
+    printEnableStat_     = 1;
+  }
+
+  double  veloStandstillLimit_;
+  double  veloMaxLimit_;  // disable with -1
   int     axisId_;
   int     standStillTimeMs_;
   int     printEnableStat_;
+  int     veloMaxLimitEnabled_;
 };
 
+
+
 class ecmcSS1SafetyGroup : public asynPortDriver {
  public:
 
@@ -54,17 +73,21 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
    *    - out_of_range
   */
   ecmcSS1SafetyGroup(const char *name,
-                  const char *ec_rampdown_cmd,
-                  const char *ec_standstill_status,
-                  int time_delay_ms,
-                  const char *cfg_string,
-                  char* portName);
+                     const char *ec_rampdown_cmd,
+                     const char *ec_standstill_status,
+                     const char *ec_limit_velo,
+                     int time_delay_ms,
+                     const char *cfg_string,
+                     char* portName);
                       
   ~ecmcSS1SafetyGroup();  
 
   // Call just before realtime because then all data sources should be available
   void                  validate();
-  void                  addAxis(int axisId, double veloLimit,int standStillTimeMs);
+  void                  addAxis(int axisId,
+                                double veloLimit,
+                                int standStillTimeMs,
+                                double veloMaxLimit);
   void                  execute();
   virtual asynStatus    readInt32(asynUser *pasynUser, epicsInt32 *value);
   std::string           getName();
@@ -77,19 +100,27 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
   void                  setAxesDisable();
   void                  setAxesStandstillStatus(int standstill);
   bool                  checkAxesStandstill();
+  bool                  checkAxesDisabled();
   bool                  checkAxisStandstill(safetyAxis* axis);
   bool                  checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis);
   bool                  checkAxesStandstillAndDisableIfNeeded();
+  void                  checkAxesMaxVeloAndDisableIfNeeded();
+  void                  setAxesMaxVelo();
+  bool                  checkAxisMaxVelo(safetyAxis* axis);
   void                  resetPrintoutStatus();
   void                  parseConfigStr(const char *configStr);
   void                  initAsyn();
   void                  refreshAsyn();
+  void                  exeRampDown();
+  void                  exeLimitVelo();
   double                ecmcSampleRateHz_;
   int                   dataSourcesLinked_;  // To avoid link several times
   int                   objectId_;           // Unique object id
   int                   cycleCounter_;
   int                   rampDownCmd_;
   int                   rampDownCmdOld_;
+  int                   limitVeloCmdOld_;
+  int                   limitVeloCmd_;
 
   // Config options
   int                   cfgDbgMode_;         // Config: allow dbg printouts
@@ -105,8 +136,7 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
   char*                 sName_;
   char*                 sEcRampDownCmdNameOrg_;
   char*                 sEcAxesStandStillStatOrg_;
-  char*                 sEcRampDownCmdNameStrip_;
-  char*                 sEcAxesStandStillStatStrip_;
+  char*                 sEcLimitVeloOrg_;
   char*                 sConfig_;
   
   int                   delayMs_;
@@ -115,16 +145,23 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
   int                   bitRampDown_;
   int                   slaveIdStandStill_;
   int                   bitStandStill_;
+  int                   slaveIdLimitVelo_;
+  int                   bitLimitVelo_;
+
+  int                   limitMaxVeloEnable_;
+  int                   axesDisabled_;
   int                   axesAreStandstill_;
   int                   axesAreStandstillOld_;
   int                   printEnableStatus_;
 
   char                  aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
   char                  aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
-  
+  char                  aliasLimitVelo_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];
+
   ecmcEc               *ecMaster_;
   ecmcEcEntry          *ecEntryRampDown_;
   ecmcEcEntry          *ecEntryStandstill_;
+  ecmcEcEntry          *ecEntryLimitVelo_;
   
   static std::string    to_string(int value);
 };

src/ecmcSafetyPlgWrap.cpp

@@ -51,6 +51,7 @@ ecmcSS1SafetyGroup* getGroupFromName(const char *name) {
 int createSafetyGroup(const char *name,
                       const char *ec_rampdown_cmd,
                       const char *ec_standstill_status,
+                      const char *ec_max_velo_cmd,
                       int time_delay_ms) {
 
   // ensure group does not already exist
@@ -71,6 +72,7 @@ int createSafetyGroup(const char *name,
     safetyGroup = new ecmcSS1SafetyGroup(name,
                                       ec_rampdown_cmd,
                                       ec_standstill_status,
+                                      ec_max_velo_cmd,
                                       time_delay_ms,
                                       configString,
                                       portNameBuffer);
@@ -90,7 +92,8 @@ int createSafetyGroup(const char *name,
 int addAxisToSafetyGroup(const char *groupName, 
                          int axisId,
                          double veloLimit,
-                         int standStillTimeMs) {
+                         int standStillTimeMs,
+                         double maxVeloLimit) {
   
   ecmcSS1SafetyGroup* grp = getGroupFromName(groupName);
 
@@ -99,7 +102,7 @@ int addAxisToSafetyGroup(const char *groupName,
     throw std::runtime_error( "Safety: Error, group not found.");
   }
 
-  grp->addAxis(axisId,veloLimit,standStillTimeMs);
+  grp->addAxis(axisId,veloLimit,standStillTimeMs,maxVeloLimit);
   
   return asynSuccess;
 }
@@ -160,15 +163,20 @@ int executeSafetyGroups() {
 */
 void ecmcAddSS1SafetyGroupPrintHelp() {
   printf("\n");
-  printf("       Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)\n");
+  printf("       Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<ec_max_velo_cmd>,<time_delay_ms>)\n");
   printf("          <name>                     : Name of group.\n");
   printf("          <ec_rampdown_cmd>          : Ethercat entry input for rampdown cmd.\n");
   printf("          <ec_standstill_status>     : Ethercat entry output for group standstill status.\n");
+  printf("          <ec_max_velo_cmd>          : Ethercat entry input for activation of maximum velo limitation (set to \"empty\" to disable).\n");
   printf("          <time_delay_ms>            : Time delay of STO [ms].\n");
   printf("\n");
 }
 
-int ecmcAddSS1SafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) {
+int ecmcAddSS1SafetyGroup(const char* name,
+                          const char* ec_rampdown_cmd,
+                          const char* ec_standstill_status,
+                          const char* ec_max_velo_cmd,
+                          int         time_delay_ms) {
   if(!name) {
     ecmcAddSS1SafetyGroupPrintHelp();
     return asynError;
@@ -190,13 +198,20 @@ int ecmcAddSS1SafetyGroup(const char* name, const char* ec_rampdown_cmd,const ch
     ecmcAddSS1SafetyGroupPrintHelp();
     return asynError;
   }
+
+  if(!ec_max_velo_cmd) {
+    printf("Error: ec_max_velo_cmd ethercat entry not defined.\n");
+    ecmcAddSS1SafetyGroupPrintHelp();
+    return asynError;
+  }
+  
   if(time_delay_ms <= 0) {
     printf("Error: time_delay invalid.\n");
     exit (EXIT_FAILURE);
   }
 
   try {
-    createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
+    createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status,ec_max_velo_cmd,time_delay_ms);
   }
   catch(std::exception& e) {
     printf("Exception: %s. Add safety group failed.\n",e.what());
@@ -213,16 +228,19 @@ static const iocshArg initArg1_1 =
 static const iocshArg initArg2_1 =
 { "ec entry output axes standstill status", iocshArgString };
 static const iocshArg initArg3_1 =
+{ "ec entry input activate max velo", iocshArgString };
+static const iocshArg initArg4_1 =
 { "STO delay [ms]", iocshArgInt };
 
 static const iocshArg *const initArgs_1[]  = { &initArg0_1, 
                                                &initArg1_1,
                                                &initArg2_1,
-                                               &initArg3_1};
+                                               &initArg3_1,
+                                               &initArg4_1,};
 
-static const iocshFuncDef    initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 4, initArgs_1 };
+static const iocshFuncDef    initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 5, initArgs_1 };
 static void initCallFunc_1(const iocshArgBuf *args) {
-  ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival);
+  ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].sval, args[4].ival);
 }
 
 /** 
@@ -231,14 +249,19 @@ static void initCallFunc_1(const iocshArgBuf *args) {
 void ecmcAddAxisToSafetyGroupPrintHelp() {
   printf("\n");
   printf("       Use ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>)\n");
-  printf("          <name>         : Name of safety group.\n");
-  printf("          <Axis id>      : Axis index to add.\n");
-  printf("          <velo limit>   : Axis standstill velo limit [unit of axis].\n");
-  printf("          <time>         : Time for axis to be below velo limit [ms].\n");
+  printf("          <name>                    : Name of safety group.\n");
+  printf("          <Axis id>                 : Axis index to add.\n");
+  printf("          <standstill_velo_limit>   : Axis standstill velo limit [unit of axis].\n");
+  printf("          <standstill_time>         : Time for axis to be below standstill_velo_limit [ms].\n");
+  printf("          <max_velo_limit>          : Axis max velo limit [unit of axis] (0 to disable).\n");
   printf("\n");
 }
 
-int ecmcAddAxisToSafetyGroup(const char* name, int axis_id, double velo_lim, int stand_still_time) {
+int ecmcAddAxisToSafetyGroup(const char* name, 
+                             int axis_id,
+                             double standstill_velo_limit,
+                             int stand_still_time,
+                             double max_velo_limit) {
   if(!name) {    
     ecmcAddAxisToSafetyGroupPrintHelp();
     return asynError;
@@ -254,17 +277,26 @@ int ecmcAddAxisToSafetyGroup(const char* name, int axis_id, double velo_lim, int
     exit(EXIT_FAILURE);
   }
 
-  if(velo_lim < 0) {
-    printf("Error: Invalid velocity limit.\n");
+  if(standstill_velo_limit < 0) {
+    printf("Error: Invalid standstill velocity limit.\n");
     exit(EXIT_FAILURE);
   }
 
   if(stand_still_time < 0) {
-    printf("Error: Invalid stand still filter time.\n");
+    printf("Error: Invalid standstill filter time.\n");
     exit(EXIT_FAILURE);
   }
+
+  if(max_velo_limit < 0) {
+    printf("Error: Invalid maximum velocity limit.\n");
+    exit(EXIT_FAILURE);
+  }
+  
   try {
-    return addAxisToSafetyGroup(name,axis_id, velo_lim, stand_still_time);
+    return addAxisToSafetyGroup(name,axis_id,
+                                standstill_velo_limit,
+                                stand_still_time,
+                                max_velo_limit);
   }
   catch(std::exception& e) {
     printf("Exception: %s. Add axis to safety group failed.\n",e.what());
@@ -279,18 +311,21 @@ static const iocshArg initArg0_2 =
 static const iocshArg initArg1_2 =
 { "Axis id []", iocshArgInt };
 static const iocshArg initArg2_2 =
-{ "Velo limit [unit same as axis cfg]", iocshArgDouble };
+{ "Velo stand still limit [unit same as axis cfg]", iocshArgDouble };
 static const iocshArg initArg3_2 =
 { "Velo stand still filter time [ms]", iocshArgInt };
+static const iocshArg initArg4_2 =
+{ "Velo max limit [unit same as axis cfg]", iocshArgDouble };
 
 static const iocshArg *const initArgs_2[]  = { &initArg0_2, 
                                                &initArg1_2,
                                                &initArg2_2,
-                                               &initArg3_2};
+                                               &initArg3_2,
+                                               &initArg4_2};
 
-static const iocshFuncDef    initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 4, initArgs_2};
+static const iocshFuncDef    initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 5, initArgs_2};
 static void initCallFunc_2(const iocshArgBuf *args) {
-  ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival);
+  ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival, args[4].dval);
 }
 
 void ecmcSafetyPlgRegister(void) {

startup.cmd

@@ -13,7 +13,7 @@
 #-
 #################################################################################
 
-#- Print discalimer
+#- Print disclaimer
 #
 # !!!!!!!!!!!!!  IMPORTANT  !!!!!!!!! 
 # This plugin has _NO_ safety rated functionalities.