From a0352036a0d2cfe92093efde0be8b1c7830beded Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Thu, 23 May 2024 14:13:17 +0200 Subject: [PATCH] SFELPHOTON-1128: debug stup-and-go motion(3) --- python/shapepath.py | 141 +++++++++++++++++++++++++++++++++++++------- 1 file changed, 121 insertions(+), 20 deletions(-) diff --git a/python/shapepath.py b/python/shapepath.py index f0ad30f..7907fba 100755 --- a/python/shapepath.py +++ b/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 - X({ox}+L1*{px}):0 Y({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: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 - pt2pt_time=tmove+twait + tmove=sp.meta['tmove']=20 # time to move in ms + 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