From aae435f0bd3d1d5e5400f63e23b680cd5fdfc80d Mon Sep 17 00:00:00 2001
From: Henrique Garcia Rodrigues <garcia_h@sls-lc9.psi.ch>
Date: Thu, 20 Aug 2026 14:26:38 +0200
Subject: [PATCH] update

---
 ECMC/cfg/axis.yaml |  16 ++---
 ECMC/cfg/code.plc  | 153 +++++++++++++++++++++++++++++++++++++++++++++
 ECMC/startup.cmd   |  10 +++
 3 files changed, 171 insertions(+), 8 deletions(-)
 create mode 100644 ECMC/cfg/code.plc

diff --git a/ECMC/cfg/axis.yaml b/ECMC/cfg/axis.yaml
index fc44276..718d5ad 100644
--- a/ECMC/cfg/axis.yaml
+++ b/ECMC/cfg/axis.yaml
@@ -1,10 +1,3 @@
-#plc:
-  #id: 1
-  #enable: yes
-  # rateMilliseconds: 10
-  #file: test.plc
-  #externalCommands: true
-
 axis:
   id: ${AXIS_ID=1}                                    # Axis id
   #feedSwitchesOutput: ec0.s${BO_SID}.binaryOutput${BO_CH=01} # Ethercat entry for feed switches
@@ -13,6 +6,13 @@ axis:
     disableTimeout: 15.0
   tweakDist: 1.0
 
+plc:
+  id: 1
+  enable: yes
+    #rateMilliseconds: 10
+  file: ./cfg/code.plc
+  externalCommands: true
+
 epics:
   name: ${AX_NAME=M1}                                 # Axis name
   precision: 3                                        # Decimal count
@@ -101,5 +101,5 @@ monitoring:
       drive: 400                                      # Time allowed outside max velo before system disables drive
   target:
     enable: true                                      # Enable at target monitoring (needs to be enabled if using motor record)
-    tolerance: 0.002                                   # Allowed tolerance
+    tolerance: 0.003                                   # Allowed tolerance
     time: 10                                          # Filter time inside tolerance to be at target
diff --git a/ECMC/cfg/code.plc b/ECMC/cfg/code.plc
new file mode 100644
index 0000000..69d6262
--- /dev/null
+++ b/ECMC/cfg/code.plc
@@ -0,0 +1,153 @@
+#############
+# PLC multi-position motion sequence
+#
+# Motion parameters
+#
+
+var VEL := 2;
+var ACC := 3;
+var DEC := 3;
+var DWELL_TIME := 12;
+var N_POS := 5;   # number of scan positions, count with the 0 position :)
+var MAX_CYCLES := 5;
+
+
+VAR
+  myAxis      : ax${AX_ID=1};
+  step        : static.seqStep;
+  cycles      : static.cycleCounter;
+  doMotion    : static.doMotion;
+  doMotionOld : static.doMotionOld;
+  posIndex    : static.posIndex;
+  direction   : static.direction;
+  dwellCnt    : static.dwellCnt;
+END_VAR
+
+
+var errorCode := 0;
+var pos := 0;
+
+
+# --------------------------------------------------
+# Initialization
+# --------------------------------------------------
+
+if(${SELF}.firstscan){
+  step := 0;
+  cycles := 0;
+  posIndex := 0;
+  direction := 1;
+  dwellCnt := 0;
+};
+
+
+# --------------------------------------------------
+# Trigger start
+# --------------------------------------------------
+
+if(doMotion and not(doMotionOld)){
+  println('PLC position sequence started');
+  step := 1;
+  posIndex := 0;
+  direction := 1;
+};
+
+
+# --------------------------------------------------
+# Position lookup
+# --------------------------------------------------
+
+pos := 0;
+
+if(posIndex==0){pos:=1.0;}
+if(posIndex==1){pos:=19.542;}
+if(posIndex==2){pos:=57.342;}
+if(posIndex==3){pos:=78.853;}
+if(posIndex==4){pos:=99.0;}
+
+
+
+# --------------------------------------------------
+# STEP 1 – move to next position
+# --------------------------------------------------
+
+if(step==1 and doMotion){
+
+  errorCode := mc_move_abs(${AX_ID=1},1,pos,VEL,ACC,DEC);
+
+  if(errorCode){
+    println('mc_move_abs error:',errorCode);
+    ${SELF}.error := errorCode;
+  };
+
+  if(not(mc_get_busy(${AX_ID=1})) and myAxis.mon.attarget){
+
+    mc_move_abs(${AX_ID=1},0,pos,VEL,ACC,DEC);
+
+    dwellCnt := 0;
+    step := 2;
+
+    println('Move done, index:',posIndex);
+  };
+};
+
+
+# --------------------------------------------------
+# STEP 2 – dwell
+# --------------------------------------------------
+
+if(step==2 and doMotion){
+
+  dwellCnt := dwellCnt + 1;
+
+  if(dwellCnt >= DWELL_TIME){
+
+    posIndex := posIndex + direction;
+
+    # reached end of list
+    if(posIndex >= N_POS){
+      direction := -1;
+      posIndex := N_POS - 2;
+    };
+
+    # reached start of list
+	if(posIndex < 0){
+
+  		direction := 1;
+  		posIndex := 1;
+
+  		cycles := cycles + 1;
+
+  		println('Cycle completed:',cycles);
+
+  		if(cycles >= MAX_CYCLES){
+    		println('Sequence completed');
+    		doMotion := 0;
+    		step := 0;
+  		};
+	};
+
+	if(step<>0){
+  		step := 1;
+	};
+  };
+};
+
+
+doMotionOld := doMotion;
+
+
+# --------------------------------------------------
+# Safety
+# --------------------------------------------------
+
+if(not(myAxis.drv.enabled)){
+  return [];
+};
+
+if(((not(ax1.mon.lowlim) or not(ax1.mon.highlim))) or errorCode){
+  mc_power(${AX_ID=1},0);
+  println('Sequence ended by limit switch violation or errorcode:', errorCode);
+  step := 0;
+  ${SELF}.enable := 0;
+};
diff --git a/ECMC/startup.cmd b/ECMC/startup.cmd
index 2b78f79..bf0fd67 100644
--- a/ECMC/startup.cmd
+++ b/ECMC/startup.cmd
@@ -52,3 +52,13 @@ epicsEnvSet(DRV_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" 
 #, BO_SID=${BO_SID}"
 ${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlEnc.cmd,    "FILE=./cfg/enc_open_loop.yaml,      DEV=${IOC}, DRV_SID=${DRV_SID}"
+
+# PLC configuration --------------------------------------
+${SCRIPTEXEC} ${ecmccfg_DIR}loadPLCFile.cmd     "FILE=./cfg/code.plc, SAMPLE_RATE_MS=500, PLC_MACROS='AX_ID=1'"
+dbLoadRecords("ecmcPlcAnalog.db","P=$(IOC):,PORT=MC_CPU1,ASYN_NAME=plcs.plc${ECMC_PLC_ID}.static.seqStep,REC_NAME=-M${AX_ID=1}-State")
+dbLoadRecords("ecmcPlcBinary.db","P=$(IOC):,PORT=MC_CPU1,ASYN_NAME=plcs.plc${ECMC_PLC_ID}.static.doMotion,REC_NAME=-M${AX_ID=1}-DoMtn")
+
+# Start motion with " caput aik-motion-pc5:Set-M1-DoMtn-RB 1 "
+# Stop motion with  " caput aik-motion-pc5:Set-M1-DoMtn-RB 0 "
+
+