##############################################################################
## 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
##

require ecmccfg "ENG_MODE=1,MASTER_ID=0"

##############################################################################
## Load components lib
#
require ecmccomp

##############################################################################
## Configure hardware

epicsEnvSet("DRV_SLAVE",              "7")
${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",              "2")
${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(ENC_SLAVE), HW_DESC=EL5042"
${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"
# This hardware box need to set BO6 to get power
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${BO_SLAVE},binaryOutput06,1)"

epicsEnvSet("BI_SLAVE",              "6")
${SCRIPTEXEC} ${ecmccfg_DIR}addSlave.cmd, "SLAVE_ID=$(BI_SLAVE), HW_DESC=EL1008"

#Apply hardware configuration
ecmcConfigOrDie "Cfg.EcApplyConfig(1)"

##############################################################################
## AXIS 1
#
epicsEnvSet("DEV",      "$(IOC)")
${SCRIPTEXEC} ${ECMC_CONFIG_ROOT}loadYamlAxis.cmd, "FILE=./cfg/axis.yaml"

##############################################################################
## Load safety plugin
#
require ecmc_plugin_safety

# Create SS1 group
#-   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_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 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:
#
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)