enhance sync part 2

Readme.md

@@ -1309,9 +1309,6 @@ start debug tools
 PPMAC=SAR-CPPM-EXPMX1
 PBInspect --host=$PPMAC&
 gpasciiCommander --host $PPMAC -i
-ssh root@$PPMAC
-ssh root@$PPMAC rm /tmp/gather_server /tmp/triggerSync
-ssh root@$PPMAC ls -l /tmp
 ```
 
 restart IOC
@@ -1323,7 +1320,6 @@ telnet $PPMAC 50001
 Ctrl-X
 dbgf SAR-CPPM-EXPMX1:MOD_VER
 
-caget SAR-CPPM-EXPMX1:MOD_VER
 
 ```
 
@@ -1331,6 +1327,7 @@ checking versions
 -----------------
 ```
 git: 7a968aac967
+caget SAR-CPPM-EXPMX1:MOD_VER
 asyn             427.0.2
 motorBase        alpha_220518
 asynMotor        alpha_220518
@@ -1461,6 +1458,8 @@ EncTable[20].index5 = 0
 EncTable[20].index5 = 0
 EncTable[20].ScaleFactor=1E-3
 
+Sys.Udata[0];EncTable[20].PrevEnc
+
 
 2.1 trigger zu spät
 1.9 trigger zu früh

python/shapepath.py

@@ -771,8 +771,8 @@ EncTable[20].ScaleFactor=1E-3
           prg.append('N100:')
         prg.append('  pvt%g abs'%pt2pt_time) #100ms to next position
         for idx in range(1,pv.shape[0]):
-          prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)]))
-          #if idx%78==10: #sync data all 256 points
+          prg.append(f'N{idx} ' + 'X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)]))
+          #if idx%256==4: #sync data all 256 points
           #  prg.append('Coord[1].Q[12]=Sys.ServoCount')
           #  #prg.append(f'Coord[1].Q[13]={idx}')
           #  prg.append(f'Coord[1].Q[13]=Motor[1].DesPos')

src/triggerSync/triggerSync.c

@@ -76,8 +76,8 @@ void trigsync_func(void *arg)
   int i;
   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
+  double srvPer,srvPer2;
+  double mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed
 
   unsigned int maxDiff;
   struct GateArray3* gate3_1=GetGate3MemPtr(1);
@@ -90,6 +90,7 @@ void trigsync_func(void *arg)
   }
 
   //unsigned int lastQ12=0; //Q12 value for further sync
+  double mvTimer;
   double mxActPos,mxDesPos,mxHomePos=pshm->Motor[2].HomePos;
   double myActPos,myDesPos,myHomePos=pshm->Motor[1].HomePos;
 
@@ -98,6 +99,7 @@ void trigsync_func(void *arg)
     rt_task_wait_period(NULL);
   pshm->Coord[1].Q[0]=-2;
   srvPer=pshm->ServoPeriod;
+  srvPer2=pshm->ServoPeriod;
 
   rtStart=rt_timer_read();
   printf("Wait for 'arm' event...\n");
@@ -193,13 +195,27 @@ void trigsync_func(void *arg)
     mtDes=i*mtPt2Pt+timeOfs;
     srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff;
     //srvPer=pshm->ServoPeriod; //default speed
-    pshm->Coord[1].DesTimeBase=srvPer;
+
+    // mvTimer > 0 motion leading FEL -> decrease srvPer
+    // mvTimer < 0 motion lagging FEL -> increase srvPer
+    // rtDiff > mtPt2Pt -> increase srvPer
+    // rtDiff < mtPt2Pt -> decrease srvPer
+    mvTimer=pshm->Motor[1].MoveTimer; //time in current section
+    if(mvTimer>mtPt2Pt/2) //lagging
+      mvTimer-=mtPt2Pt;
+    //srvPer2*=(rtDiff/1E6-mvTimer)/mtPt2Pt;
+    srvPer2=pshm->ServoPeriod*(rtDiff/1E6-mvTimer)/mtPt2Pt;
+
+    pshm->Coord[1].DesTimeBase=srvPer2;
     pshm->Coord[1].Q[0]++;
     if(mode&8)
     {
       mxActPos=pshm->Motor[2].ActPos;mxDesPos=pshm->Motor[2].DesPos;
       myActPos=pshm->Motor[1].ActPos;myDesPos=pshm->Motor[1].DesPos;
-      printf(" X,Y ActPos:(%.5g %.5g) DesPos:(%.5g %.5g) ", mxActPos-mxHomePos, myActPos-myHomePos, mxDesPos-mxHomePos, myDesPos-myHomePos);
+      //printf(" X,Y ActPos:(%.5g %.5g) DesPos:(%.5g %.5g) ", mxActPos-mxHomePos, myActPos-myHomePos, mxDesPos-mxHomePos, myDesPos-myHomePos);
+      //printf(" X,Y Nsync:(%d) ", pshm->Coord[1].Nsync);
+      //printf(" X,Y MoveTimer:(%.5g %.5g) ", pshm->Motor[1].MoveTimer,pshm->Motor[2].MoveTimer);
+      printf(" Nsync:%d, MoveTimer:(%.5g) srvPer2:%.6f ", pshm->Coord[1].Nsync,mvTimer,srvPer2);
       printf("Trigger count:%d, rtDiff:%.3fms scDiff:%d mtAct:%.3f mtDes:%.3f srvPer:%.6f ENCVAL:%u \n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer,ENCVAL);
     }
     rtLast=rtCur;