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

non adapting triggering with triggerSync.c

Browse Source
This commit is contained in:
zamofing_t
2019-01-17 15:56:18 +01:00
parent 3e9485ee25
commit a11a3bce79
4 changed files with 106 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
python/MXMotion.py
View File

@@ -63,10 +63,13 @@ class MotionBase:
flag0='P0';flag1='P1'
prg = '''
Coord[1].Q[1]=-2
Coord[1].TimeBaseSlew=1 //1E-4 is default
Coord[1].DesTimeBase=0
while({flag0}==0){{}}
Coord[1].Q[1]=-1
Gather.Enable=2
while({flag1}==0){{}}
Coord[1].DesTimeBase=Sys.ServoPeriod
'''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
self.sync_prg = prg
self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId)
@@ -76,11 +79,12 @@ class MotionBase:
flag0='Gate3[1].Chan[0].UserFlag'
flag1='Gate3[1].Chan[1].UserFlag'
flag0='P0';flag1='P1'
pt2pt_time=40
prg='''
//Gather.Enable=2 if done in the sync program
Coord[1].TimeBaseSlew=1 //1E-4 is default
Coord[1].DesTimeBase=0
Coord[1].Q[1]=-1
//while({flag0}==0){{}}
'''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
self.sync_prg=prg
self.sync_run='&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId)

16
python/PBMotionAnalyzer/PBMotionAnalyzer.py
View File

@@ -138,14 +138,16 @@ class MAMainFrame(wx.Frame):
idxTrigger=rec[:,4]
doc.idxTrigger=idxTrigger=np.where(np.diff(idxTrigger)==1)[0]+1
idxInPos=[] #first point at idx 0
try:# find approximate distance of 2 points
rng =idxTrigger[2]-idxTrigger[1]
except IndexError:
rng = int(lenRec / lenPts)
idx=rng/2
#try:# find approximate distance of 2 points
# rng =idxTrigger[2]-idxTrigger[1]
#except IndexError:
# rng = int(lenRec / lenPts)
#idx=rng/2
idx=0
for i in range(lenPts):
l=rec[idx:idx+rng,(3,2)]-pts[i,:]
l=rec[idx:,(3,2)]-pts[i,:]
l2=l[:,0]**2+l[:,1]**2
try:
ofs=l2.argmin()
@@ -153,7 +155,7 @@ class MAMainFrame(wx.Frame):
break#print(l2[ofs])
idx+=ofs
idxInPos.append(idx)
idx+=rng/2
#idx+=rng/2
doc.idxInPos=idxInPos=np.array(idxInPos)
idx=min(idxInPos.shape[0],idxTrigger.shape[0])-1

88
python/shapepath.py
View File

@@ -136,7 +136,7 @@ def debugplot_pvt(pv, meta):
#
# ax.legend(loc='best')
# plt.show(block=False)
return (tt,ppx,ppy)
class ShapePath(MotionBase):
def __init__(self,comm, gather, verbose):
@@ -209,6 +209,15 @@ class ShapePath(MotionBase):
pts+=ofs
self.points=pts
def gen_spiral_points(self,rStart=1.,rInc=.2,numSeg=4,numCir=6, ofs=(0, 0)):
#rInc radius increment per circle
r=rStart+np.arange(numSeg*numCir)*(float(rInc)/numSeg)
ang=2.*np.pi/numSeg*np.arange(numSeg*numCir)
pts=np.vstack((np.sin(ang)*r,np.cos(ang)*r)).T
pts+=ofs
self.points=pts
def gen_closed_shifted(self,pitch=100,shift=5,mult=3):
'from the given points, close the path, and runs 9 times with small pitch'
@@ -390,17 +399,18 @@ class ShapePath(MotionBase):
p=pt.T.copy() #copy
k=p.shape[1]
stp=((p[:,-1]-p[:,0])/(k-1)) #calculate steepness point to point
#stp*=0
p[0,:]-=stp[0]*np.arange(k)
p[1,:]-=stp[1]*np.arange(k)
f=np.fft.fftfreq(k, d=1./k)
f=np.fft.fftfreq(k, d=1.)
pf=np.fft.fft(p)
pfd=pf*f*1j # differentiate in fourier
pd=np.fft.ifft(pfd)
v=pd.real.T/(k*2*np.pi)+stp/pt2pt_time
v=pd.real.T/pt2pt_time*np.pi*2+stp/pt2pt_time
pv[ 1:-1,(2,3)] = v*scale
verb=self.verbose
if verb&16:
debugplot_pvt(pv, self.meta)
self.pvt=debugplot_pvt(pv, self.meta)
plt.show()
pv[1:-1, (2, 3)]*=CoordFeedTime #scaling for Deltatau
@@ -554,31 +564,37 @@ class ShapePath(MotionBase):
ax = fig.add_subplot(1,1,1)
ax.invert_xaxis();ax.invert_yaxis()
#hl=ax[0].plot(x, y, color=col)
hl=ax.plot(pts[:,0],pts[:,1],'r.')
hl=ax.plot(pts[:,0],pts[:,1],'y--')
hl=ax.plot(rec[:,3],rec[:,2], 'b-') # desired path
hl=ax.plot(rec[:,1],rec[:,0],'g-') # actual path
ax.xaxis.set_label_text('x-pos um')
ax.yaxis.set_label_text('y-pos um')
ax.axis('equal')
cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
fig.obj=self
self.ax=ax
self.hl=hl
hl=ax.plot(pts[:,0],pts[:,1],'r.',label='points')
hl+=ax.plot(pts[:,0],pts[:,1],'y--',label='direct')
hl+=ax.plot(rec[:,3],rec[:,2], 'b-',label='DesPos') # desired path
hl+=ax.plot(rec[:,1],rec[:,0],'g-',label='ActPos') # actual path
try: pvt=self.pvt
except AttributeError: pass
else:
hl=ax.plot(pvt[1], pvt[2], 'c--',label='SimPos') # simulated path
fig2=plt.figure('time line')
ax2=fig2.add_subplot(1, 1, 1)
hl2=ax2.plot(rec[:, 2], 'r-', label='desPos Mot1')
hl2+=ax2.plot(rec[:, 3], 'g-', label='desPos Mot2')
idxTrigger = rec[:, 4]
idxTrigger = np.where(np.diff(idxTrigger) == 1)[0] + 1
if idxTrigger.shape[0]>0:
hl=ax.plot(rec[idxTrigger,1],rec[idxTrigger,0],'xr') # actual path
hl+=ax.plot(rec[idxTrigger,1],rec[idxTrigger,0],'xr',label='trig') # actual path
hl2 += ax2.plot(rec[:, 4], 'b-',label='trigger')
fig = plt.figure('trigger')
ax = fig.add_subplot(1, 1, 1)
hl += ax.plot(rec[:,2], 'r-',label='desPos Mot1')
hl += ax.plot(rec[:,3], 'g-',label='desPos Mot2')
hl += ax.plot(rec[:, 4], 'b-',label='trigger')
ax.legend(loc='best')
plt.show(block=False)
ax.xaxis.set_label_text('x-pos um')
ax.yaxis.set_label_text('y-pos um')
ax.axis('equal')
ax.legend(loc='best')
cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
fig.obj=self
ax2.legend(loc='best')
plt.show(block=False)
if mode&2:
fig = plt.figure('position error')
@@ -791,17 +807,29 @@ if __name__=='__main__':
#print(sp.points[:,0])
#sp.gen_swissmx_points(width=1000, ofs=(-500, 0));
#sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,0.]])
sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,0.],
[0., 1.], [1., 0.], [0., -1.], [-1., 0.],
[0., 1.], [1., 0.], [0., -1.], [-1., 0.],
[0., 1.], [1., 0.], [0., -1.], [-1., 0.],
[0., 1.], [1., 0.], [0., -1.], [-1., 0.],
[0., 1.], [1., 0.], [0., -1.], [-1., 0.], [0., 1.]])
#sp.points=np.array([[0.,1. ],[1. ,0.],[0.,-1. ],[-1. ,0.],
# [0., 1.], [1., 0.], [0., -1.], [-1., 0.],
# [0., 1.], [1., 0.], [0., -1.], [-1., 0.],
# [0., 1.], [1., 0.], [0., -1.], [-1., 0.],
# [0., 1.], [1., 0.], [0., -1.], [-1., 0.],
# ])
# [0.,1.2],[1.2,0.],[0.,-1.2],[-1.2,0.],
# [0.,1.4],[1.4,0.],[0.,-1.4],[-1.4,0.],
# [0.,1.6],[1.6,0.],[0.,-1.6],[-1.6,0.],
# [0.,1.8],[1.8,0.],[0.,-1.8],[-1.8,0.],
# [0.,2. ],[2. ,0.],[0.,-2. ],[-2. ,0.],
# [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_closed_shifted()
#sp.gen_grid_points(w=10,h=10,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False);
#sp.gen_grid_points(w=1,h=10,pitch=100,rnd=0,ofs=(0,0))
#sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=0)
#sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=1)
sp.setup_gather(acq_per=2)
sp.setup_gather(acq_per=1)
sp.setup_sync(mode=2) #sync with timing system and PLC to sync speed (PROG)
sp.setup_coord_trf() # reset to shape path system
sp.setup_motion(fnPrg=fn + '.prg', mode=3, pt2pt_time=40,scale=1,dwell=10)