From f32f7723bf735eb175ce2573597912be93f4e4a8 Mon Sep 17 00:00:00 2001
From: Thierry Zamofing <thierry.zamofing@psi.ch>
Date: Thu, 17 Jan 2019 17:03:13 +0100
Subject: [PATCH] running sync !!!

---
 python/shapepath.py           |  2 +-
 src/triggerSync/triggerSync.c | 71 +++++++++++++++++++----------------
 2 files changed, 39 insertions(+), 34 deletions(-)

diff --git a/python/shapepath.py b/python/shapepath.py
index ff8a94b..c1c1b4a 100755
--- a/python/shapepath.py
+++ b/python/shapepath.py
@@ -822,7 +822,7 @@ if __name__=='__main__':
     #                    [0., 1.]])
     #sp.points*=100
     #sp.gen_spiral_points(rStart=100,rInc=10,numSeg=4,numCir=60, ofs=(0, 0))
-    sp.gen_spiral_points(rStart=100,rInc=10,numSeg=32,numCir=12, ofs=(0, 0))
+    sp.gen_spiral_points(rStart=100,rInc=10,numSeg=8,numCir=12, ofs=(0, 0))
 
     #sp.gen_closed_shifted()
     #sp.gen_grid_points(w=10,h=10,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False);
diff --git a/src/triggerSync/triggerSync.c b/src/triggerSync/triggerSync.c
index 64369e7..cf642a4 100644
--- a/src/triggerSync/triggerSync.c
+++ b/src/triggerSync/triggerSync.c
@@ -41,58 +41,60 @@ void trigsync_func(void *arg)
   rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD);
 
   int i;
-  RTIME rtStart,rtLast,rtCur;
-  unsigned scStart,scLast,diff,maxDiff;
+  RTIME rtStart,rtLast,rtCur,rtDiff; //rt* is real time of the rt-process
+  unsigned int scStart,scLast,scCur,scDiff; //sc* are servo counts of the motion
+  float srvPer;
+  float mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed
+
+  float mtPt2Pt=40.f; //motion point to point time
+
+  unsigned int maxDiff;
   struct GateArray3* gate3_1=GetGate3MemPtr(1);
 
   pshm = GetSharedMemPtr();
-
-
-  /*
-  //Start the task loop
-  rtStart = rt_timer_read();
-  for(i=0;i<20000;i++)
-  {
-    if (i%10==0)
-      printf("Loop count: %d, Loop time: %.5f ms\n", i, (rt_timer_read() - tStart)/1000000.0);
-    rt_task_wait_period(NULL);
-    //int rt_task_sleep_until
-  }
-  */
+  srvPer=pshm->ServoPeriod;
 
   if(pshm->Gather.Enable==1)
     pshm->Gather.Enable=2;  //start record at flag0' trigger
-  sleep(1);
+  usleep(1000000); //wait 1 sec(record before motion)
   rtStart=rt_timer_read();
-  printf("Wait for trigger:\n");
+  printf("Wait for 'arm' event:\n");
   pshm->Coord[1].Q[0]=-10;
   while(!FLAG0)
     rt_task_wait_period(NULL);
-  printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0);
+  printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1E6);
 
   pshm->Coord[1].Q[0]=-9;
   while(!FLAG1)
     rt_task_wait_period(NULL);
-  //if(pshm->Gather.Enable==1)
-  //  pshm->Gather.Enable=2;  //start record at flag1' trigger
-  printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0);
+  printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1E6);
   rtStart=rtLast=rt_timer_read();
   scStart=scLast=pshm->ServoCount;
+  mtAct=0.f;
   pshm->Coord[1].Q[0]=1;
-  pshm->Coord[1].DesTimeBase=pshm->ServoPeriod;  //start motion at default speed
-  while(FLAG1)
-    rt_task_wait_period(NULL);
+  pshm->Coord[1].DesTimeBase=srvPer;  //start motion at default speed
 
-  for(i=0;pshm->Gather.Enable;i++)
+  for(i=1;pshm->Gather.Enable;i++)
   {
-    while(!FLAG1)
-      rt_task_wait_period(NULL);
-    rtLast = rt_timer_read();
-    scLast=pshm->ServoCount;
-    pshm->Coord[1].Q[0]++;
     while(FLAG1)
       rt_task_wait_period(NULL);
-    printf("Trigger count: %d, time: %.5f ms   ServoCount %d\n", i, (rtLast - rtStart)/1000000.0,scLast-scStart);
+
+    while(!FLAG1)
+      rt_task_wait_period(NULL);
+    //FEL shot arrived
+    rtCur = rt_timer_read();
+    scCur=pshm->ServoCount;
+    rtDiff=rtCur-rtLast;
+    scDiff=scCur-scLast;
+
+    mtAct+=scDiff*srvPer;
+    mtDes=i*mtPt2Pt;
+    srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff;
+    pshm->Coord[1].DesTimeBase=srvPer;
+    pshm->Coord[1].Q[0]++;
+    printf("Trigger count: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.3f\n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer);
+    rtLast=rtCur;
+    scLast=scCur;
   }
 
   /*  float ang,pos;
@@ -132,9 +134,12 @@ void trigsim_func(void *arg)
 
   rtStart=rt_timer_read();
 
-  for(i=0;;i++)
+  for(i=0,rtSlice=rtStart;;i++)
   {
-    rtSlice=rtStart+i*40*1E6; // a slice is 40 ms
+    //rtSlice=rtStart+i*40*1E6; // a slice is 40 ms
+    rtSlice=rtStart+i*40.2*1E6; // a slice is 40 ms
+    //rtSlice+=(rand()%(int)(1*1E6)); //0.1ms jitter
+
     rt_task_sleep_until(rtSlice); //in ns
     pshm->P[1]=1;
     //while(rt_timer_read()-rtStart<1E6*40*i)