From 19427d54ee3a1eda178ed7e461745f6aa5ec6fea Mon Sep 17 00:00:00 2001
From: VulcanixFR <vulcanix.gamingfr@gmail.com>
Date: Thu, 23 May 2024 09:50:22 +0200
Subject: [PATCH] Update Readme and Fix Latency windows

---
 README.md                         | 58 ++++++++++++++++++--
 main.py                           | 18 +++++--
 robot/KRL/Python_Gripper.sub      | 37 +++++++++++++
 robot/KRL/_Axis_MAIN_dataset.dat  | 21 ++++++++
 robot/KRL/a1.dat                  | 24 +++++++++
 robot/KRL/a1.src                  | 26 +++++++++
 robot/KRL/a2.dat                  | 26 +++++++++
 robot/KRL/a2.src                  | 31 +++++++++++
 robot/KRL/a3.dat                  | 23 ++++++++
 robot/KRL/a3.src                  | 27 ++++++++++
 robot/KRL/a4.dat                  | 23 ++++++++
 robot/KRL/a4.src                  | 28 ++++++++++
 robot/KRL/a5.dat                  | 23 ++++++++
 robot/KRL/a5.src                  | 28 ++++++++++
 robot/KRL/a6.dat                  | 23 ++++++++
 robot/KRL/a6.src                  | 29 ++++++++++
 robot/configurations/$config.dat  | 59 --------------------
 robot/configurations/12_ms_v2.xml | 89 +++++++++++++++++++++++++++++++
 ui/measure_latency.py             |  2 +
 19 files changed, 528 insertions(+), 67 deletions(-)
 create mode 100644 robot/KRL/Python_Gripper.sub
 create mode 100644 robot/KRL/_Axis_MAIN_dataset.dat
 create mode 100644 robot/KRL/a1.dat
 create mode 100644 robot/KRL/a1.src
 create mode 100644 robot/KRL/a2.dat
 create mode 100644 robot/KRL/a2.src
 create mode 100644 robot/KRL/a3.dat
 create mode 100644 robot/KRL/a3.src
 create mode 100644 robot/KRL/a4.dat
 create mode 100644 robot/KRL/a4.src
 create mode 100644 robot/KRL/a5.dat
 create mode 100644 robot/KRL/a5.src
 create mode 100644 robot/KRL/a6.dat
 create mode 100644 robot/KRL/a6.src
 delete mode 100644 robot/configurations/$config.dat
 create mode 100644 robot/configurations/12_ms_v2.xml

diff --git a/README.md b/README.md
index b041c3c..944d4cf 100644
--- a/README.md
+++ b/README.md
@@ -130,15 +130,67 @@ class methods instead of global variables and functions.
 
 This section describes how to deploy the data collection on a cell.
 
-**TODO**: Finish this part, add the files to the repo
-
 ### Global variables
 
 The following declarations must be added in the `System/$config.dat` file 
 on the robot KUKA workspace :
 
 ```
-...
+;FOLD ------DIAGNOSTIKA GLOBAL PROMENNE-------
+;Program control
+DECL BOOL PyRUN=FALSE
+DECL BOOL PyDONE=FALSE
+DECL INT PySPEED=30
+DECL INT PyITER[6]
+DECL BOOL PyOPEN_GRIPPER=FALSE
+DECL BOOL PyCLOSE_GRIPPER=FALSE
+DECL INT PyKNUCKLE
+DECL REAL SPEED
+
+;Data collection control
+DECL BOOL ColRUN=FALSE
+DECL BOOL ColRESET=FALSE
+DECL BOOL ColBUFFER_FULL=FALSE
+DECL BOOL ColKEEPING_UP=FALSE
+DECL BOOL ColRESET_DONE=TRUE
+DECL INT ColSAMPLING=12
+DECL INT ColBUFFER_SIZE=20000
+
+;Data communication buffers and flags
+DECL INT SAMPLE_READ=772
+DECL INT SAMPLE_NUMBER=772
+DECL REAL __TAB_1[36]
+
+DECL INT __PYTHON_HAS_READ=771 ; 
+DECL BOOL __PyResetTimer=FALSE ; 
+
+;Data collection buffers
+DECL REAL ColBUFFER_TQ_A1[20000]
+DECL REAL ColBUFFER_TQ_A2[20000]
+DECL REAL ColBUFFER_TQ_A3[20000]
+DECL REAL ColBUFFER_TQ_A4[20000]
+DECL REAL ColBUFFER_TQ_A5[20000]
+DECL REAL ColBUFFER_TQ_A6[20000]
+DECL REAL ColBUFFER_TEMP_A1[20000]
+DECL REAL ColBUFFER_TEMP_A2[20000]
+DECL REAL ColBUFFER_TEMP_A3[20000]
+DECL REAL ColBUFFER_TEMP_A4[20000]
+DECL REAL ColBUFFER_TEMP_A5[20000]
+DECL REAL ColBUFFER_TEMP_A6[20000]
+DECL REAL ColBUFFER_CURR_A1[20000]
+DECL REAL ColBUFFER_CURR_A2[20000]
+DECL REAL ColBUFFER_CURR_A3[20000]
+DECL REAL ColBUFFER_CURR_A4[20000]
+DECL REAL ColBUFFER_CURR_A5[20000]
+DECL REAL ColBUFFER_CURR_A6[20000]
+DECL REAL ColBUFFER_TIME[20000]
+DECL REAL ColBUFFER_ANALOG[20000]
+
+DECL E6AXIS __LAST_POS_ACT
+DECL E6AXIS ColBUFFER_POS_ACT[20000]
+DECL E6AXIS __LAST_POS_MEAS
+DECL E6AXIS ColBUFFER_POS_MEAS[20000]
+;ENDFOLD
 ```
 
 ### `Data_collector.sub`
diff --git a/main.py b/main.py
index 896bd9d..c6bc7b2 100644
--- a/main.py
+++ b/main.py
@@ -9,6 +9,7 @@ from threading import Semaphore, Thread
 from typing import List, Any
 import pandas as pd
 import matplotlib.pyplot as plt
+import os
 
 class MainProgram (MainWindow):
     """Wrapper for the main program. Classes are more reliable than global variables.
@@ -20,7 +21,7 @@ class MainProgram (MainWindow):
 
     ### ---- Robots ---- ###
     robot_handlers: List[KUKA_Handler] = [ None ] * 3
-    robot_windows = [ ]
+    robot_windows: List[Measure_robot] = [ ]
     
     ### ---- Sync Mechanism ---- ###
     sync_done = 0
@@ -80,9 +81,14 @@ class MainProgram (MainWindow):
         """        
 
         self.sync_done += 1
-        if self.sync_done == self.sync_number:
+
+        sync_number = 0
+        for w in self.robot_windows:
+            sync_number += 1 if not w.collecting_data_done else 0
+
+        if self.sync_done == sync_number:
             self.sync_done = 0
-            self.sync_sem.release(self.sync_number)
+            self.sync_sem.release(sync_number)
 
     def close (self):
         """Closes the main program
@@ -105,6 +111,8 @@ class MainProgram (MainWindow):
 
         # Creating the file names
         file_path = values["-user_path-"] + "/" + values["-dataset_name-"]
+        if not os.path.exists(values["-user_path-"]):
+            os.mkdir(values["-user_path-"])
 
         # Creating the windows used to collect data
         self.robot_windows = [ ]
@@ -148,13 +156,13 @@ class MainProgram (MainWindow):
         """        
 
         try:
-            window_latency = []
+            self.window_latency = []
             for i in range(len(self.robot_handlers)):
                 r = self.robot_handlers[i]
                 if r is not None:
                     w = Measure_latency(f"Latency measurement for Robot {i + 1}")
                     w._poll()
-                    window_latency.append(w)
+                    self.window_latency.append(w)
                     t = Thread(target=w.measure_latency, args=[r], daemon=False)
                     t.start()
 
diff --git a/robot/KRL/Python_Gripper.sub b/robot/KRL/Python_Gripper.sub
new file mode 100644
index 0000000..86b057e
--- /dev/null
+++ b/robot/KRL/Python_Gripper.sub
@@ -0,0 +1,37 @@
+&ACCESS RVP
+&COMMENT USER specified PLC program
+DEF  Python_Gripper ( )
+;FOLD DECLARATIONS
+  ;FOLD USER DECL
+  ; Please insert user defined declarations
+
+  ;ENDFOLD (USER DECL)
+;ENDFOLD (DECLARATIONS)
+;FOLD INI
+  ;FOLD USER INIT
+  ; Please insert user defined initialization commands
+
+  ;ENDFOLD (USER INIT)
+;ENDFOLD (INI)
+
+LOOP
+;FOLD USER PLC
+;Make your modifications here
+WAIT FOR PyOPEN_GRIPPER OR PyCLOSE_GRIPPER
+IF PyOPEN_GRIPPER THEN
+   PULSE ( $OUT[7], true, 0.5 )
+   PyOPEN_GRIPPER=FALSE
+ENDIF
+
+IF PyCLOSE_GRIPPER THEN
+   PULSE ( $OUT[8], true, 0.5 )
+   PyCLOSE_GRIPPER=FALSE
+ENDIF
+;ENDFOLD (USER PLC)
+ENDLOOP
+END
+;ENDFOLD
+;FOLD USER SUBROUTINE
+;Integrate your user defined subroutines
+
+;ENDFOLD (USER SUBROUTINE)
\ No newline at end of file
diff --git a/robot/KRL/_Axis_MAIN_dataset.dat b/robot/KRL/_Axis_MAIN_dataset.dat
new file mode 100644
index 0000000..28b3271
--- /dev/null
+++ b/robot/KRL/_Axis_MAIN_dataset.dat
@@ -0,0 +1,21 @@
+&ACCESS RVP
+&REL 8
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Diagnostika
+DEFDAT  _Axis_MAIN_dataset
+;FOLD EXTERNAL DECLARATIONS;%{PE}%MKUKATPBASIS,%CEXT,%VCOMMON,%P
+;FOLD BASISTECH EXT;%{PE}%MKUKATPBASIS,%CEXT,%VEXT,%P
+EXT  BAS (BAS_COMMAND  :IN,REAL  :IN )
+
+;ENDFOLD (BASISTECH EXT)
+;FOLD USER EXT;%{E}%MKUKATPUSER,%CEXT,%VEXT,%P
+;Make your modifications here
+DECL INT counter
+DECL AXIS DOMOV={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+DECL FDAT FP1={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
+DECL PDAT PPDAT1={VEL 100.000,ACC 100.000,APO_DIST 500.000,APO_MODE #CDIS,GEAR_JERK 100.000,EXAX_IGN 0}
+;ENDFOLD (USER EXT)
+;ENDFOLD (EXTERNAL DECLARATIONS)
+
+ENDDAT
\ No newline at end of file
diff --git a/robot/KRL/a1.dat b/robot/KRL/a1.dat
new file mode 100644
index 0000000..8a06145
--- /dev/null
+++ b/robot/KRL/a1.dat
@@ -0,0 +1,24 @@
+&ACCESS RVO1
+&REL 253
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEFDAT  A1
+;FOLD EXTERNAL DECLARATIONS;%{PE}%MKUKATPBASIS,%CEXT,%VCOMMON,%P
+;FOLD BASISTECH EXT;%{PE}%MKUKATPBASIS,%CEXT,%VEXT,%P
+EXT  BAS (BAS_COMMAND  :IN,REAL  :IN )
+DECL INT SUCCESS
+;ENDFOLD (BASISTECH EXT)
+;FOLD USER EXT;%{E}%MKUKATPUSER,%CEXT,%VEXT,%P
+;Make your modifications here
+
+;ENDFOLD (USER EXT)
+;ENDFOLD (EXTERNAL DECLARATIONS)
+DECL AXIS DOMOV={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+DECL FDAT FP1={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
+DECL PDAT PPDAT1={VEL 100.000,ACC 100.000,APO_DIST 500.000,APO_MODE #CDIS,GEAR_JERK 100.000,EXAX_IGN 0}
+DECL AXIS POHYB={A1 -45.0000,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+
+
+ENDDAT
+
diff --git a/robot/KRL/a1.src b/robot/KRL/a1.src
new file mode 100644
index 0000000..89c1c17
--- /dev/null
+++ b/robot/KRL/a1.src
@@ -0,0 +1,26 @@
+&ACCESS RVO1
+&REL 253
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEF A1( )
+;FOLD INI;%{PE}
+  ;FOLD BASISTECH INI
+    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
+    INTERRUPT ON 3 
+    BAS (#INITMOV,0 )
+  ;ENDFOLD (BASISTECH INI)
+  ;FOLD USER INI
+    ;Make your modifications here
+
+  ;ENDFOLD (USER INI)
+;ENDFOLD (INI)
+
+
+
+
+POHYB.A1 = DOMOV.A1+45 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+POHYB.A1 = DOMOV.A1-45 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+END
diff --git a/robot/KRL/a2.dat b/robot/KRL/a2.dat
new file mode 100644
index 0000000..b686d3f
--- /dev/null
+++ b/robot/KRL/a2.dat
@@ -0,0 +1,26 @@
+&ACCESS RVO1
+&REL 225
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEFDAT  A2
+;FOLD EXTERNAL DECLARATIONS;%{PE}%MKUKATPBASIS,%CEXT,%VCOMMON,%P
+;FOLD BASISTECH EXT;%{PE}%MKUKATPBASIS,%CEXT,%VEXT,%P
+EXT  BAS (BAS_COMMAND  :IN,REAL  :IN )
+DECL INT SUCCESS
+;ENDFOLD (BASISTECH EXT)
+;FOLD USER EXT;%{E}%MKUKATPUSER,%CEXT,%VEXT,%P
+;Make your modifications here
+
+;ENDFOLD (USER EXT)
+;ENDFOLD (EXTERNAL DECLARATIONS)
+DECL AXIS DOMOV={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+DECL FDAT FP1={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
+DECL PDAT PPDAT1={VEL 100.000,ACC 100.000,APO_DIST 500.000,APO_MODE #CDIS,GEAR_JERK 100.000,EXAX_IGN 0}
+DECL MODULEPARAM_T LAST_TP_PARAMS={PARAMS[] "Kuka.PointName=P31; Kuka.FrameData.base_no=0; Kuka.FrameData.tool_no=1; Kuka.FrameData.ipo_frame=#BASE; Kuka.isglobalpoint=False; Kuka.MoveDataPtpName=PDAT31; Kuka.MovementDataPdat.apo_mode=#CDIS; Kuka.MovementDataPdat.apo_dist=500; Kuka.MovementData.vel=100; Kuka.MovementData.acc=100; Kuka.MovementData.exax_ign=0; Kuka.VelocityPtp=100; Kuka.BlendingEnabled=True; Kuka.CurrentCDSetIndex=0; Kuka.VelocityFieldEnabled=False; Kuka.ColDetectFieldEnabled=False; Kuka.MovementParameterFieldEnabled=False; Kuka.IsAngleEnabled=False                     "}
+DECL AXIS POHYB={A1 0.0,A2 -135.000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+DECL E6POS XP31={X 285.604095,Y -0.640600,Z 520.018494,A 180.000,B 66.6905289,C -180.000,S 2,T 2,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
+DECL FDAT FP31={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
+DECL PDAT PPDAT31={VEL 100.000,ACC 100.000,APO_DIST 500.000,APO_MODE #CDIS,GEAR_JERK 100.000,EXAX_IGN 0}
+ENDDAT
+
diff --git a/robot/KRL/a2.src b/robot/KRL/a2.src
new file mode 100644
index 0000000..70a0b51
--- /dev/null
+++ b/robot/KRL/a2.src
@@ -0,0 +1,31 @@
+&ACCESS RVO1
+&REL 225
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEF A2( )
+;FOLD INI;%{PE}
+  ;FOLD BASISTECH INI
+    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
+    INTERRUPT ON 3 
+    BAS (#INITMOV,0 )
+  ;ENDFOLD (BASISTECH INI)
+  ;FOLD USER INI
+    ;Make your modifications here
+
+  ;ENDFOLD (USER INI)
+;ENDFOLD (INI)
+
+
+
+
+POHYB.A2 = DOMOV.A2+15
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+POHYB.A2 = DOMOV.A2-45 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+
+
+
+
+END
diff --git a/robot/KRL/a3.dat b/robot/KRL/a3.dat
new file mode 100644
index 0000000..deca5a4
--- /dev/null
+++ b/robot/KRL/a3.dat
@@ -0,0 +1,23 @@
+&ACCESS RVO1
+&REL 223
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEFDAT  A3
+;FOLD EXTERNAL DECLARATIONS;%{PE}%MKUKATPBASIS,%CEXT,%VCOMMON,%P
+;FOLD BASISTECH EXT;%{PE}%MKUKATPBASIS,%CEXT,%VEXT,%P
+EXT  BAS (BAS_COMMAND  :IN,REAL  :IN )
+DECL INT SUCCESS
+;ENDFOLD (BASISTECH EXT)
+;FOLD USER EXT;%{E}%MKUKATPUSER,%CEXT,%VEXT,%P
+;Make your modifications here
+
+;ENDFOLD (USER EXT)
+;ENDFOLD (EXTERNAL DECLARATIONS)
+DECL AXIS DOMOV={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+DECL FDAT FP1={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
+DECL PDAT PPDAT1={VEL 100.000,ACC 100.000,APO_DIST 500.000,APO_MODE #CDIS,GEAR_JERK 100.000,EXAX_IGN 0}
+DECL MODULEPARAM_T LAST_TP_PARAMS={PARAMS[] "Kuka.PointName=P31; Kuka.FrameData.base_no=0; Kuka.FrameData.tool_no=1; Kuka.FrameData.ipo_frame=#BASE; Kuka.isglobalpoint=False; Kuka.MoveDataPtpName=PDAT31; Kuka.MovementDataPdat.apo_mode=#CDIS; Kuka.MovementDataPdat.apo_dist=500; Kuka.MovementData.vel=100; Kuka.MovementData.acc=100; Kuka.MovementData.exax_ign=0; Kuka.VelocityPtp=100; Kuka.BlendingEnabled=False; Kuka.CurrentCDSetIndex=0; Kuka.VelocityFieldEnabled=False; Kuka.ColDetectFieldEnabled=False; Kuka.MovementParameterFieldEnabled=False; Kuka.IsAngleEnabled=False                  "}
+DECL AXIS POHYB={A1 0.0,A2 -90.0000,A3 60.0000,A4 0.0,A5 60.0000,A6 0.0}
+ENDDAT
+
diff --git a/robot/KRL/a3.src b/robot/KRL/a3.src
new file mode 100644
index 0000000..ca5ca2b
--- /dev/null
+++ b/robot/KRL/a3.src
@@ -0,0 +1,27 @@
+&ACCESS RVO1
+&REL 223
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEF A3( )
+;FOLD INI;%{PE}
+  ;FOLD BASISTECH INI
+    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
+    INTERRUPT ON 3 
+    BAS (#INITMOV,0 )
+  ;ENDFOLD (BASISTECH INI)
+  ;FOLD USER INI
+    ;Make your modifications here
+
+  ;ENDFOLD (USER INI)
+;ENDFOLD (INI)
+
+
+
+POHYB.A3 = DOMOV.A3+25 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+POHYB.A3 = DOMOV.A3-30 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+END
diff --git a/robot/KRL/a4.dat b/robot/KRL/a4.dat
new file mode 100644
index 0000000..2267b94
--- /dev/null
+++ b/robot/KRL/a4.dat
@@ -0,0 +1,23 @@
+&ACCESS RVO1
+&REL 224
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEFDAT  A4
+;FOLD EXTERNAL DECLARATIONS;%{PE}%MKUKATPBASIS,%CEXT,%VCOMMON,%P
+;FOLD BASISTECH EXT;%{PE}%MKUKATPBASIS,%CEXT,%VEXT,%P
+EXT  BAS (BAS_COMMAND  :IN,REAL  :IN )
+DECL INT SUCCESS
+;ENDFOLD (BASISTECH EXT)
+;FOLD USER EXT;%{E}%MKUKATPUSER,%CEXT,%VEXT,%P
+;Make your modifications here
+
+;ENDFOLD (USER EXT)
+;ENDFOLD (EXTERNAL DECLARATIONS)
+DECL AXIS DOMOV={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+DECL FDAT FP1={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
+DECL PDAT PPDAT1={VEL 100.000,ACC 100.000,APO_DIST 500.000,APO_MODE #CDIS,GEAR_JERK 100.000,EXAX_IGN 0}
+DECL MODULEPARAM_T LAST_TP_PARAMS={PARAMS[] "Kuka.PointName=P31; Kuka.FrameData.base_no=0; Kuka.FrameData.tool_no=1; Kuka.FrameData.ipo_frame=#BASE; Kuka.isglobalpoint=False; Kuka.MoveDataPtpName=PDAT31; Kuka.MovementDataPdat.apo_mode=#CDIS; Kuka.MovementDataPdat.apo_dist=500; Kuka.MovementData.vel=100; Kuka.MovementData.acc=100; Kuka.MovementData.exax_ign=0; Kuka.VelocityPtp=100; Kuka.BlendingEnabled=False; Kuka.CurrentCDSetIndex=0; Kuka.VelocityFieldEnabled=False; Kuka.ColDetectFieldEnabled=False; Kuka.MovementParameterFieldEnabled=False; Kuka.IsAngleEnabled=False                  "}
+DECL AXIS POHYB={A1 0.0,A2 -90.0000,A3 90.0000,A4 -50.0000,A5 60.0000,A6 0.0}
+ENDDAT
+
diff --git a/robot/KRL/a4.src b/robot/KRL/a4.src
new file mode 100644
index 0000000..a84ab3c
--- /dev/null
+++ b/robot/KRL/a4.src
@@ -0,0 +1,28 @@
+&ACCESS RVO1
+&REL 224
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEF A4( )
+;FOLD INI;%{PE}
+  ;FOLD BASISTECH INI
+    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
+    INTERRUPT ON 3 
+    BAS (#INITMOV,0 )
+  ;ENDFOLD (BASISTECH INI)
+  ;FOLD USER INI
+    ;Make your modifications here
+
+  ;ENDFOLD (USER INI)
+;ENDFOLD (INI)
+
+
+
+POHYB.A4 = DOMOV.A4+50 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+POHYB.A4 = DOMOV.A4-50 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+
+END
diff --git a/robot/KRL/a5.dat b/robot/KRL/a5.dat
new file mode 100644
index 0000000..10645ba
--- /dev/null
+++ b/robot/KRL/a5.dat
@@ -0,0 +1,23 @@
+&ACCESS RVO1
+&REL 211
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEFDAT  A5
+;FOLD EXTERNAL DECLARATIONS;%{PE}%MKUKATPBASIS,%CEXT,%VCOMMON,%P
+;FOLD BASISTECH EXT;%{PE}%MKUKATPBASIS,%CEXT,%VEXT,%P
+EXT  BAS (BAS_COMMAND  :IN,REAL  :IN )
+DECL INT SUCCESS
+;ENDFOLD (BASISTECH EXT)
+;FOLD USER EXT;%{E}%MKUKATPUSER,%CEXT,%VEXT,%P
+;Make your modifications here
+
+;ENDFOLD (USER EXT)
+;ENDFOLD (EXTERNAL DECLARATIONS)
+DECL AXIS DOMOV={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+DECL FDAT FP1={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
+DECL PDAT PPDAT1={VEL 100.000,ACC 100.000,APO_DIST 500.000,APO_MODE #CDIS,GEAR_JERK 100.000,EXAX_IGN 0}
+DECL MODULEPARAM_T LAST_TP_PARAMS={PARAMS[] "Kuka.PointName=P31; Kuka.FrameData.base_no=0; Kuka.FrameData.tool_no=1; Kuka.FrameData.ipo_frame=#BASE; Kuka.isglobalpoint=False; Kuka.MoveDataPtpName=PDAT31; Kuka.MovementDataPdat.apo_mode=#CDIS; Kuka.MovementDataPdat.apo_dist=500; Kuka.MovementData.vel=100; Kuka.MovementData.acc=100; Kuka.MovementData.exax_ign=0; Kuka.VelocityPtp=100; Kuka.BlendingEnabled=False; Kuka.CurrentCDSetIndex=0; Kuka.VelocityFieldEnabled=False; Kuka.ColDetectFieldEnabled=False; Kuka.MovementParameterFieldEnabled=False; Kuka.IsAngleEnabled=False                  "}
+DECL AXIS POHYB={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 25.0000,A6 0.0}
+ENDDAT
+
diff --git a/robot/KRL/a5.src b/robot/KRL/a5.src
new file mode 100644
index 0000000..7a27359
--- /dev/null
+++ b/robot/KRL/a5.src
@@ -0,0 +1,28 @@
+&ACCESS RVO1
+&REL 211
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEF A5( )
+;FOLD INI;%{PE}
+  ;FOLD BASISTECH INI
+    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
+    INTERRUPT ON 3 
+    BAS (#INITMOV,0 )
+  ;ENDFOLD (BASISTECH INI)
+  ;FOLD USER INI
+    ;Make your modifications here
+
+  ;ENDFOLD (USER INI)
+;ENDFOLD (INI)
+
+
+
+POHYB.A5 = DOMOV.A5+30 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+POHYB.A5 = DOMOV.A5-35 
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+
+END
diff --git a/robot/KRL/a6.dat b/robot/KRL/a6.dat
new file mode 100644
index 0000000..b3ecf45
--- /dev/null
+++ b/robot/KRL/a6.dat
@@ -0,0 +1,23 @@
+&ACCESS RVO1
+&REL 207
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEFDAT  A6
+;FOLD EXTERNAL DECLARATIONS;%{PE}%MKUKATPBASIS,%CEXT,%VCOMMON,%P
+;FOLD BASISTECH EXT;%{PE}%MKUKATPBASIS,%CEXT,%VEXT,%P
+EXT  BAS (BAS_COMMAND  :IN,REAL  :IN )
+DECL INT SUCCESS
+;ENDFOLD (BASISTECH EXT)
+;FOLD USER EXT;%{E}%MKUKATPUSER,%CEXT,%VEXT,%P
+;Make your modifications here
+
+;ENDFOLD (USER EXT)
+;ENDFOLD (EXTERNAL DECLARATIONS)
+DECL AXIS DOMOV={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 0.0}
+DECL FDAT FP1={TOOL_NO 0,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
+DECL PDAT PPDAT1={VEL 100.000,ACC 100.000,APO_DIST 500.000,APO_MODE #CDIS,GEAR_JERK 100.000,EXAX_IGN 0}
+DECL MODULEPARAM_T LAST_TP_PARAMS={PARAMS[] "Kuka.PointName=P31; Kuka.FrameData.base_no=0; Kuka.FrameData.tool_no=1; Kuka.FrameData.ipo_frame=#BASE; Kuka.isglobalpoint=False; Kuka.MoveDataPtpName=PDAT31; Kuka.MovementDataPdat.apo_mode=#CDIS; Kuka.MovementDataPdat.apo_dist=500; Kuka.MovementData.vel=100; Kuka.MovementData.acc=100; Kuka.MovementData.exax_ign=0; Kuka.VelocityPtp=100; Kuka.BlendingEnabled=False; Kuka.CurrentCDSetIndex=0; Kuka.VelocityFieldEnabled=False; Kuka.ColDetectFieldEnabled=False; Kuka.MovementParameterFieldEnabled=False; Kuka.IsAngleEnabled=False                  "}
+DECL AXIS POHYB={A1 0.0,A2 -90.0000,A3 90.0000,A4 0.0,A5 60.0000,A6 -90.0000}
+ENDDAT
+
diff --git a/robot/KRL/a6.src b/robot/KRL/a6.src
new file mode 100644
index 0000000..8c406a8
--- /dev/null
+++ b/robot/KRL/a6.src
@@ -0,0 +1,29 @@
+&ACCESS RVO1
+&REL 207
+&PARAM EDITMASK = *
+&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
+&PARAM DISKPATH = KRC:\R1\Program\Diagnostika
+DEF A6( )
+;FOLD INI;%{PE}
+  ;FOLD BASISTECH INI
+    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
+    INTERRUPT ON 3 
+    BAS (#INITMOV,0 )
+  ;ENDFOLD (BASISTECH INI)
+  ;FOLD USER INI
+    ;Make your modifications here
+
+  ;ENDFOLD (USER INI)
+;ENDFOLD (INI)
+
+
+
+
+POHYB.A6 = DOMOV.A6+90
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+POHYB.A6 = DOMOV.A6-90
+SPTP POHYB WITH $VEL_AXIS[1] = SVEL_JOINT(PySPEED), $TOOL = STOOL2(FP1), $BASE = SBASE(FP1.BASE_NO), $IPO_MODE = SIPO_MODE(FP1.IPO_FRAME), $LOAD = SLOAD(FP1.TOOL_NO), $ACC_AXIS[1] = SACC_JOINT(PPDAT1), $APO = SAPO_PTP(PPDAT1), $GEAR_JERK[1] = SGEAR_JERK(PPDAT1), $COLLMON_TOL_PRO[1] = USE_CM_PRO_VALUES(0)
+
+
+END
diff --git a/robot/configurations/$config.dat b/robot/configurations/$config.dat
deleted file mode 100644
index 8a3266a..0000000
--- a/robot/configurations/$config.dat
+++ /dev/null
@@ -1,59 +0,0 @@
-; Global variables needed for the execution of the data collector ;
-
-;FOLD ------DIAGNOSTIKA GLOBAL PROMENNE-------
-;Program control
-DECL BOOL PyRUN=FALSE
-DECL BOOL PyDONE=FALSE
-DECL INT PySPEED=30
-DECL INT PyITER[6]
-DECL BOOL PyOPEN_GRIPPER=FALSE
-DECL BOOL PyCLOSE_GRIPPER=FALSE
-DECL INT PyKNUCKLE
-DECL REAL SPEED
-
-;Data collection control
-DECL BOOL ColRUN=FALSE
-DECL BOOL ColRESET=FALSE
-DECL BOOL ColBUFFER_FULL=FALSE
-DECL BOOL ColKEEPING_UP=FALSE
-DECL BOOL ColRESET_DONE=TRUE
-DECL INT ColSAMPLING=12
-DECL INT ColBUFFER_SIZE=20000
-
-;Data communication buffers and flags
-DECL INT SAMPLE_READ=772
-DECL INT SAMPLE_NUMBER=772
-DECL REAL __TAB_1[36]
-
-DECL INT __PYTHON_HAS_READ=771 ; 
-DECL BOOL __PyResetTimer=FALSE ; 
-
-;Data collection buffers
-DECL REAL ColBUFFER_TQ_A1[20000]
-DECL REAL ColBUFFER_TQ_A2[20000]
-DECL REAL ColBUFFER_TQ_A3[20000]
-DECL REAL ColBUFFER_TQ_A4[20000]
-DECL REAL ColBUFFER_TQ_A5[20000]
-DECL REAL ColBUFFER_TQ_A6[20000]
-DECL REAL ColBUFFER_TEMP_A1[20000]
-DECL REAL ColBUFFER_TEMP_A2[20000]
-DECL REAL ColBUFFER_TEMP_A3[20000]
-DECL REAL ColBUFFER_TEMP_A4[20000]
-DECL REAL ColBUFFER_TEMP_A5[20000]
-DECL REAL ColBUFFER_TEMP_A6[20000]
-DECL REAL ColBUFFER_CURR_A1[20000]
-DECL REAL ColBUFFER_CURR_A2[20000]
-DECL REAL ColBUFFER_CURR_A3[20000]
-DECL REAL ColBUFFER_CURR_A4[20000]
-DECL REAL ColBUFFER_CURR_A5[20000]
-DECL REAL ColBUFFER_CURR_A6[20000]
-DECL REAL ColBUFFER_TIME[20000]
-DECL REAL ColBUFFER_ANALOG[20000]
-
-DECL E6AXIS __LAST_POS_ACT
-DECL E6AXIS ColBUFFER_POS_ACT[20000]
-DECL E6AXIS __LAST_POS_MEAS
-DECL E6AXIS ColBUFFER_POS_MEAS[20000]
-;ENDFOLD
-
-...
\ No newline at end of file
diff --git a/robot/configurations/12_ms_v2.xml b/robot/configurations/12_ms_v2.xml
new file mode 100644
index 0000000..3feb955
--- /dev/null
+++ b/robot/configurations/12_ms_v2.xml
@@ -0,0 +1,89 @@
+<?xml version="1.0"?>
+<Trace xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:xsd="http://www.w3.org/2001/XMLSchema">
+  <Time>600</Time>
+  <PreTrigger>0</PreTrigger>
+  <Basename>12_ms_v2</Basename>
+  <Modules>
+    <Module>
+      <Name>KRCIpo</Name>
+      <Rate>12000</Rate>
+      <Channel>500</Channel>
+    </Module>
+    <Module>
+      <Name>NextGenDrive#1</Name>
+      <Rate>12000</Rate>
+      <Channel>1</Channel>
+      <Channel>2</Channel>
+      <Channel>3</Channel>
+      <Channel>8</Channel>
+      <Channel>14</Channel>
+      <Channel>117</Channel>
+      <Channel>200</Channel>
+      <Channel>202</Channel>
+    </Module>
+    <Module>
+      <Name>NextGenDrive#2</Name>
+      <Rate>12000</Rate>
+      <Channel>1</Channel>
+      <Channel>2</Channel>
+      <Channel>3</Channel>
+      <Channel>8</Channel>
+      <Channel>14</Channel>
+      <Channel>117</Channel>
+      <Channel>200</Channel>
+      <Channel>202</Channel>
+    </Module>
+    <Module>
+      <Name>NextGenDrive#3</Name>
+      <Rate>12000</Rate>
+      <Channel>1</Channel>
+      <Channel>2</Channel>
+      <Channel>3</Channel>
+      <Channel>8</Channel>
+      <Channel>14</Channel>
+      <Channel>117</Channel>
+      <Channel>200</Channel>
+      <Channel>202</Channel>
+    </Module>
+    <Module>
+      <Name>NextGenDrive#4</Name>
+      <Rate>12000</Rate>
+      <Channel>1</Channel>
+      <Channel>2</Channel>
+      <Channel>3</Channel>
+      <Channel>8</Channel>
+      <Channel>14</Channel>
+      <Channel>117</Channel>
+      <Channel>200</Channel>
+      <Channel>202</Channel>
+    </Module>
+    <Module>
+      <Name>NextGenDrive#5</Name>
+      <Rate>12000</Rate>
+      <Channel>1</Channel>
+      <Channel>2</Channel>
+      <Channel>3</Channel>
+      <Channel>8</Channel>
+      <Channel>14</Channel>
+      <Channel>117</Channel>
+      <Channel>200</Channel>
+      <Channel>202</Channel>
+    </Module>
+    <Module>
+      <Name>NextGenDrive#6</Name>
+      <Rate>12000</Rate>
+      <Channel>1</Channel>
+      <Channel>2</Channel>
+      <Channel>3</Channel>
+      <Channel>8</Channel>
+      <Channel>14</Channel>
+      <Channel>117</Channel>
+      <Channel>200</Channel>
+      <Channel>202</Channel>
+    </Module>
+  </Modules>
+  <Trigger>
+    <ModuleName>KRCIpo</ModuleName>
+    <ID>1</ID>
+  </Trigger>
+</Trace>
\ No newline at end of file
diff --git a/ui/measure_latency.py b/ui/measure_latency.py
index 96a423a..3353eda 100644
--- a/ui/measure_latency.py
+++ b/ui/measure_latency.py
@@ -55,6 +55,8 @@ class Measure_latency(sg.Window):
             robot (KUKA_Handler): The robot handler
         """        
 
+        print("Measuring latency with " + str(robot.ipAddress))
+
         # Preparing the buffers
         self.acc_time_M = np.zeros(self.N)
         self.acc_time_1 = np.zeros(self.N)
-- 
GitLab