SFELPHOTON-1128: debug stup-and-go motion(3)

python/shapepath.py

@@ -717,7 +717,7 @@ class ShapePath(MotionBase):
     #scan=0 # snake motion X fast, Y slow
     scan=1 # snake motion Y fast, X slow (default)
 
-    prg=f'close all buffers\nopen prog {prgId}\n'
+    prg=f'close all buffers\nopen prog {prgId}\n  Coord[1].DesTimeBase=Sys.ServoPeriod\n'
 
     verb=self.verbose
     comm=self.comm
@@ -801,7 +801,8 @@ class ShapePath(MotionBase):
           ox, oy=(0,0)
           px, py=(1,1)
           self.mot_pts=(np.hstack((pt, np.ones((pt.shape[0], 1))))*np.asmatrix(trf)).A  # pt*trf
-        prg+=f'  linear abs\n  X{ox:g} Y{oy:g}\n'
+        #prg+=f'  linear abs\n  X{ox:g} Y{oy:g}\n'
+        prg+=f'  linear abs\n  X{ox-px:g} Y{oy-py:g}\n' # start one position out of grid
       prg+='  dwell 10\n'
       try: prg+=self.sync_prg
       except AttributeError:
@@ -810,8 +811,8 @@ class ShapePath(MotionBase):
       if cnt>1:
         prg+='  P100=%d\n'%cnt
         prg+='N100:\n'
-      prg+='  pvt%g abs\n'%pt2pt_time #100ms to next position
       if mode in (1,3):
+        prg+='  pvt%g abs\n'%pt2pt_time  # 100ms to next position
         for idx in range(1,pv.shape[0]):
           #prg+=f'N{idx} ' + 'X%g:%g Y%g:%g\n'%tuple(pv[idx,(0,2,1,3)])
           prg+=f'  X%g:%g Y%g:%g\n'%tuple(pv[idx, (0, 2, 1, 3)])
@@ -825,6 +826,7 @@ class ShapePath(MotionBase):
           vy=py/(pt2pt_time)*scale*CoordFeedTime #scaling for Deltatau
 
           prg+=f'''\
+  pvt{pt2pt_time} abs
   //grid pvt motion
   L1=0 //slow loop x
   L0=0 //fast loop y
@@ -870,13 +872,106 @@ class ShapePath(MotionBase):
   X({ox}+L1*{px}):{0:g} Y({oy}+L0*{py}):{0:g}
 
 '''
-      else:  # mode=5
+
+      elif mode==5:
         if scan==0:
           raise Exception('scan=0 not supported')
           pass
         else:  # scan=1
+          if self.meta['sync_mode']==2:
+            _log.error('sync_mode=2 not allowed for stop-and-go motion !')
+          pt2tp_felpulse=kwargs['pt2tp_felpulse']
           tmove=kwargs['tmove']
-          twait=kwargs['twait']
+          syncPlc=f'''\
+close all buffers
+disable plc 2
+open plc 2
+  Sys.Udata[0]=0
+  Sys.Udata[1]=0
+  while(1) {{
+    while (Gate3[1].Chan[1].UserFlag==0){{}}
+    Sys.Udata[0]=Sys.Udata[0]+1
+    if (Sys.Udata[0]>={pt2tp_felpulse}) {{
+      Sys.Udata[1]=Sys.Udata[1]+1
+      Sys.Udata[0]=0
+    }}
+    while (Gate3[1].Chan[1].UserFlag==1){{}}
+  }}
+close
+enable plc 2
+'''
+          comm.gpascii.send_block(syncPlc, verb&0x08)
+
+#          prg+=f'''\
+#  linear abs
+#  Motor[1].JogTa=-1 // closer to 0 harder acceleration
+#  Motor[2].JogTa=-1
+#  Motor[5].JogTa=-10
+#  Coord[1].Ta=00000.1 // 455
+#  Coord[1].Td=00000.1
+#  Coord[1].Ts=0
+#  jog1=-1080
+#  dwell 50
+#  jog1=-1200
+#  dwell 50
+#  jog1=-1080
+#  dwell 50
+#  jog1=-1200
+#  dwell 50
+#  //grid stop-and-go motion
+#  L1=0 //slow loop x
+#  L0=0 //fast loop y
+#  while(L1<{nx})
+#  {{
+#    //send 1"A:move X%g Y%g",{ox}+L1*{px},{oy}+L0*{py}
+#    X({ox}+L1*{px}) Y({oy}+L0*{py})
+#    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
+#    L0+=1
+#    while(L0<{ny}-1)
+#    {{
+#      //send 1"B:move X%g Y%g",{ox}+L1*{px},{oy}+L0*{py}
+#      X({ox}+L1*{px}) Y({oy}+L0*{py})
+#      while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
+#      L0+=1
+#    }}
+#    if(L1>={nx}-1)
+#    {{
+#      break
+#    }}
+#    //send 1"C:move X%g Y%g",{ox}+L1*{px},{oy}+L0*{py}
+#    X({ox}+L1*{px}) Y({oy}+L0*{py})
+#    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
+#    L1+=1
+#    //send 1"D:move X%g Y%g",{ox}+L1*{px},{oy}+L0*{py}
+#    X({ox}+L1*{px}) Y({oy}+L0*{py})
+#    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
+#    L0-=1
+#    while(L0>=1)
+#    {{
+#      //send 1"E:move X%g Y%g",{ox}+L1*{px},{oy}+L0*{py}
+#      X({ox}+L1*{px}) Y({oy}+L0*{py})
+#      while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
+#      L0-=1
+#    }}
+#    if(L1>={nx}-1)
+#    {{
+#      break
+#    }}
+#    //send 1"F:move X%g Y%g",{ox}+L1*{px},{oy}+L0*{py}
+#    X({ox}+L1*{px}) Y({oy}+L0*{py})
+#    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
+#    L1+=1
+#    //send 1"G:move X%g Y%g",{ox}+L1*{px},{oy}+L0*{py}
+#    X({ox}+L1*{px}) Y({oy}+L0*{py})
+#    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
+#    L0+=1
+#  }}
+#  //send 1"H:move X%g Y%g",{ox}+L1*{px},{oy}+L0*{py}
+#  X({ox}+L1*{px}) Y({oy}+L0*{py})
+#
+#'''
+
+
           prg+=f'''\
   pvt{tmove} abs
   //grid pvt motion
@@ -886,13 +981,13 @@ class ShapePath(MotionBase):
   {{
     //send 1"A:move X%g:%g Y%g:%g",{ox}+L1*{px},0,{oy}+L0*{py},0
     X({ox}+L1*{px}):0 Y({oy}+L0*{py}):0
-    delay{twait}
+    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
     L0+=1
     while(L0<{ny}-1)
     {{
       //send 1"B:move X%g:%g Y%g:%g",{ox}+L1*{px},0,{oy}+L0*{py},0
       X({ox}+L1*{px}):0 Y({oy}+L0*{py}):0
-      delay{twait}
+      while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
       L0+=1
     }}
     if(L1>={nx}-1)
@@ -901,17 +996,17 @@ class ShapePath(MotionBase):
     }}
     //send 1"C:move X%g:%g Y%g:%g",{ox}+L1*{px},0,{oy}+L0*{py},0
     X({ox}+L1*{px}):0 Y({oy}+L0*{py}):0
-    delay{twait}
+    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
     L1+=1
     //send 1"D:move X%g:%g Y%g:%g",{ox}+L1*{px},0,{oy}+L0*{py},0
     X({ox}+L1*{px}):0 Y({oy}+L0*{py}):0
-    delay{twait}
+    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
     L0-=1
     while(L0>=1)
     {{
       //send 1"E:move X%g:%g Y%g:%g",{ox}+L1*{px},0,{oy}+L0*{py},0
       X({ox}+L1*{px}):0 Y({oy}+L0*{py}):0
-      delay{twait}
+      while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
       L0-=1
     }}
     if(L1>={nx}-1)
@@ -920,17 +1015,21 @@ class ShapePath(MotionBase):
     }}
     //send 1"F:move X%g:%g Y%g:%g",{ox}+L1*{px},0,{oy}+L0*{py},0
     X({ox}+L1*{px}):0 Y({oy}+L0*{py}):0
-    delay{twait}
+    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
     L1+=1
     //send 1"G:move X%g:%g Y%g:%g",{ox}+L1*{px},0,{oy}+L0*{py},0
     X({ox}+L1*{px}):0 Y({oy}+L0*{py}):0
-    delay{twait}
+    while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
     L0+=1
   }}
-  //send 1"H:move X%g:%g Y%g:%g",{ox}+L1*{px},0,{oy}+L0*{py},0
+  //send 1"H:move X%g:%g Y%g:%g",{ox}+L1*{px},{0:g},{oy}+L0*{py},{0:g}
-  X({ox}+L1*{px}):0 Y({oy}+L0*{py}):0
+  X({ox}+L1*{px}):{0:g} Y({oy}+L0*{py}):{0:g}
+  while(Sys.Udata[2]==Sys.Udata[1]){{}};Sys.Udata[2]=Sys.Udata[1] // wait motion trigger
 
 '''
+
+      else:
+        raise ValueError('unsupported mode')
       #common code to repeat the motion multiple times
       if cnt>1:
         prg+=f'''\
@@ -1024,7 +1123,7 @@ if __name__=='__main__':
     #sp = ShapePath(comm, gather, args.verbose)
 
     # direct start
-    sp = ShapePath(comm, gather, args.verbose,fel_per=10,sync_mode=0)
+    #sp = ShapePath(comm, gather, args.verbose,fel_per=10,sync_mode=0)
 
     #simulated start and frame trigger no sync
     #sp = ShapePath(comm, gather, args.verbose,fel_per=10,sync_mode=1,sync_flag=3)
@@ -1033,7 +1132,7 @@ if __name__=='__main__':
     #sp = ShapePath(comm, gather, args.verbose,fel_per=10,sync_mode=2,sync_flag=3)
 
     #simulated start real frame trigger no sync
-    #sp = ShapePath(comm, gather, args.verbose,fel_per=10,sync_mode=1,sync_flag=1)
+    sp = ShapePath(comm, gather, args.verbose,fel_per=10,sync_mode=1,sync_flag=1)
 
     #simulated start real frame trigger with sync
     #sp = ShapePath(comm, gather, args.verbose,fel_per=10,sync_mode=2,sync_flag=1)
@@ -1092,10 +1191,12 @@ if __name__=='__main__':
     # mode:3  pvt motion using inverse fft velocity
     # mode:4  pvt motion short code using grid parameters
     # mode:5  pvt motion short code using grid parameters. Instead of continous motion it moves and waits as give in the parameter time
-    mode=3
+    #mode=3;scale=0
+    mode=5
     if mode==5:
-      tmove=20; twait=100
+      tmove=sp.meta['tmove']=20 # time to move in ms
-      pt2pt_time=tmove+twait
+      pt2tp_felpulse=15 #numper of fel pulses from point to point move e.g. 15puls at 100Hz=10ms -> 15*10ms=150ms
+      pt2pt_time=pt2tp_felpulse*sp.meta['fel_per']
     else:
       pt2pt_time=sp.meta['fel_per']
     sp.meta['pt2pt_time']=pt2pt_time
@@ -1130,7 +1231,7 @@ if __name__=='__main__':
     elif mode==4:
       sp.setup_motion(fnPrg=fn+'.prg',scale=1.,cnt=1,dwell=100,mode=4,grid=grid)
     elif mode==5:
-      sp.setup_motion(fnPrg=fn+'.prg',scale=1.,cnt=1,dwell=100,mode=5,tmove=tmove ,twait=twait, grid=grid)
+      sp.setup_motion(fnPrg=fn+'.prg',scale=1.,cnt=1,dwell=100,mode=5,tmove=tmove, pt2tp_felpulse=pt2tp_felpulse, grid=grid)
 
     sp.homing()      #homing if needed
     sp.run()         #start motion program