sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
zamofing_t
2024-05-23 14:13:17 +02:00
parent 5a26b7f47c
commit a0352036a0
1 changed files with 121 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

141
python/shapepath.py
View File

@@ -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