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' flag0='P0';flag1='P1'
prg = ''' prg = '''
Coord[1].Q[1]=-2 Coord[1].Q[1]=-2
Coord[1].TimeBaseSlew=1 //1E-4 is default
Coord[1].DesTimeBase=0
while({flag0}==0){{}} while({flag0}==0){{}}
Coord[1].Q[1]=-1 Coord[1].Q[1]=-1
Gather.Enable=2 Gather.Enable=2
while({flag1}==0){{}} while({flag1}==0){{}}
Coord[1].DesTimeBase=Sys.ServoPeriod
'''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1) '''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
self.sync_prg = prg self.sync_prg = prg
self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId) 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' flag0='Gate3[1].Chan[0].UserFlag'
flag1='Gate3[1].Chan[1].UserFlag' flag1='Gate3[1].Chan[1].UserFlag'
flag0='P0';flag1='P1' flag0='P0';flag1='P1'
pt2pt_time=40
prg=''' prg='''
//Gather.Enable=2 if done in the sync program //Gather.Enable=2 if done in the sync program
Coord[1].TimeBaseSlew=1 //1E-4 is default
Coord[1].DesTimeBase=0 Coord[1].DesTimeBase=0
Coord[1].Q[1]=-1 Coord[1].Q[1]=-1
//while({flag0}==0){{}}
'''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1) '''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
self.sync_prg=prg self.sync_prg=prg
self.sync_run='&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId) 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] idxTrigger=rec[:,4]
doc.idxTrigger=idxTrigger=np.where(np.diff(idxTrigger)==1)[0]+1 doc.idxTrigger=idxTrigger=np.where(np.diff(idxTrigger)==1)[0]+1
idxInPos=[] #first point at idx 0 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): 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 l2=l[:,0]**2+l[:,1]**2
try: try:
ofs=l2.argmin() ofs=l2.argmin()
@@ -153,7 +155,7 @@ class MAMainFrame(wx.Frame):
break#print(l2[ofs]) break#print(l2[ofs])
idx+=ofs idx+=ofs
idxInPos.append(idx) idxInPos.append(idx)
idx+=rng/2 #idx+=rng/2
doc.idxInPos=idxInPos=np.array(idxInPos) doc.idxInPos=idxInPos=np.array(idxInPos)
idx=min(idxInPos.shape[0],idxTrigger.shape[0])-1 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') # ax.legend(loc='best')
# plt.show(block=False) # plt.show(block=False)
return (tt,ppx,ppy)
class ShapePath(MotionBase): class ShapePath(MotionBase):
def __init__(self,comm, gather, verbose): def __init__(self,comm, gather, verbose):
@@ -209,6 +209,15 @@ class ShapePath(MotionBase):
pts+=ofs pts+=ofs
self.points=pts 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): 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' '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 p=pt.T.copy() #copy
k=p.shape[1] k=p.shape[1]
stp=((p[:,-1]-p[:,0])/(k-1)) #calculate steepness point to point stp=((p[:,-1]-p[:,0])/(k-1)) #calculate steepness point to point
#stp*=0
p[0,:]-=stp[0]*np.arange(k) p[0,:]-=stp[0]*np.arange(k)
p[1,:]-=stp[1]*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) pf=np.fft.fft(p)
pfd=pf*f*1j # differentiate in fourier pfd=pf*f*1j # differentiate in fourier
pd=np.fft.ifft(pfd) 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 pv[ 1:-1,(2,3)] = v*scale
verb=self.verbose verb=self.verbose
if verb&16: if verb&16:
debugplot_pvt(pv, self.meta) self.pvt=debugplot_pvt(pv, self.meta)
plt.show() plt.show()
pv[1:-1, (2, 3)]*=CoordFeedTime #scaling for Deltatau pv[1:-1, (2, 3)]*=CoordFeedTime #scaling for Deltatau
@@ -554,31 +564,37 @@ class ShapePath(MotionBase):
ax = fig.add_subplot(1,1,1) ax = fig.add_subplot(1,1,1)
ax.invert_xaxis();ax.invert_yaxis() ax.invert_xaxis();ax.invert_yaxis()
#hl=ax[0].plot(x, y, color=col) #hl=ax[0].plot(x, y, color=col)
hl=ax.plot(pts[:,0],pts[:,1],'r.') hl=ax.plot(pts[:,0],pts[:,1],'r.',label='points')
hl=ax.plot(pts[:,0],pts[:,1],'y--') hl+=ax.plot(pts[:,0],pts[:,1],'y--',label='direct')
hl=ax.plot(rec[:,3],rec[:,2], 'b-') # desired path hl+=ax.plot(rec[:,3],rec[:,2], 'b-',label='DesPos') # desired path
hl=ax.plot(rec[:,1],rec[:,0],'g-') # actual path hl+=ax.plot(rec[:,1],rec[:,0],'g-',label='ActPos') # actual path
ax.xaxis.set_label_text('x-pos um') try: pvt=self.pvt
ax.yaxis.set_label_text('y-pos um') except AttributeError: pass
ax.axis('equal') else:
cid = fig.canvas.mpl_connect('button_press_event', self.onclick) hl=ax.plot(pvt[1], pvt[2], 'c--',label='SimPos') # simulated path
fig.obj=self
self.ax=ax
self.hl=hl 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 = rec[:, 4]
idxTrigger = np.where(np.diff(idxTrigger) == 1)[0] + 1 idxTrigger = np.where(np.diff(idxTrigger) == 1)[0] + 1
if idxTrigger.shape[0]>0: 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.xaxis.set_label_text('x-pos um')
ax = fig.add_subplot(1, 1, 1) ax.yaxis.set_label_text('y-pos um')
hl += ax.plot(rec[:,2], 'r-',label='desPos Mot1') ax.axis('equal')
hl += ax.plot(rec[:,3], 'g-',label='desPos Mot2') ax.legend(loc='best')
hl += ax.plot(rec[:, 4], 'b-',label='trigger') cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
ax.legend(loc='best') fig.obj=self
plt.show(block=False)
ax2.legend(loc='best')
plt.show(block=False)
if mode&2: if mode&2:
fig = plt.figure('position error') fig = plt.figure('position error')
@@ -791,17 +807,29 @@ if __name__=='__main__':
#print(sp.points[:,0]) #print(sp.points[:,0])
#sp.gen_swissmx_points(width=1000, ofs=(-500, 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.]])
sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,0.],
[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.], [1., 0.], [0., -1.], [-1., 0.],
[0., 1.], [1., 0.], [0., -1.], [-1., 0.], [0., 1.]]) # [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=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.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=0)
#sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=1) #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_sync(mode=2) #sync with timing system and PLC to sync speed (PROG)
sp.setup_coord_trf() # reset to shape path system sp.setup_coord_trf() # reset to shape path system
sp.setup_motion(fnPrg=fn + '.prg', mode=3, pt2pt_time=40,scale=1,dwell=10) sp.setup_motion(fnPrg=fn + '.prg', mode=3, pt2pt_time=40,scale=1,dwell=10)

53
src/triggerSync/triggerSync.c
View File

@@ -60,6 +60,9 @@ void trigsync_func(void *arg)
} }
*/ */
if(pshm->Gather.Enable==1)
pshm->Gather.Enable=2; //start record at flag0' trigger
sleep(1);
rtStart=rt_timer_read(); rtStart=rt_timer_read();
printf("Wait for trigger:\n"); printf("Wait for trigger:\n");
pshm->Coord[1].Q[0]=-10; pshm->Coord[1].Q[0]=-10;
@@ -68,11 +71,10 @@ void trigsync_func(void *arg)
printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0); printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0);
pshm->Coord[1].Q[0]=-9; pshm->Coord[1].Q[0]=-9;
if(!pshm->Gather.Enable)
pshm->Gather.Enable=2; //start record at flag0' trigger
while(!FLAG1) while(!FLAG1)
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
//pshm->Gather.Enable=2; //start record at flag1' trigger //if(pshm->Gather.Enable==1)
// pshm->Gather.Enable=2; //start record at flag1' trigger
printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0); printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0);
rtStart=rtLast=rt_timer_read(); rtStart=rtLast=rt_timer_read();
scStart=scLast=pshm->ServoCount; scStart=scLast=pshm->ServoCount;
@@ -81,7 +83,6 @@ void trigsync_func(void *arg)
while(FLAG1) while(FLAG1)
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
while(pshm->Gather.Enable)
for(i=0;pshm->Gather.Enable;i++) for(i=0;pshm->Gather.Enable;i++)
{ {
while(!FLAG1) while(!FLAG1)
@@ -119,33 +120,45 @@ void trigsim_func(void *arg)
curtask = rt_task_self(); curtask = rt_task_self();
rt_task_inquire(curtask, &curtaskinfo); rt_task_inquire(curtask, &curtaskinfo);
//Print the info //Print the info
printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,LOOP_PERIOD/1000000.f); //printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,10*1E6/1E6);
//Make the task periodic with a specified loop period //Make the task periodic with a specified loop period
rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD); //rt_task_set_periodic(NULL, TM_NOW, 10*1E6);
printf("Starting task %s \n", curtaskinfo.name);
int i; int i;
RTIME rtStart; RTIME rtStart,rtSlice;
printf("sizeof RTTIME %d\n",sizeof(rtStart));
pshm = GetSharedMemPtr(); pshm = GetSharedMemPtr();
rtStart=rt_timer_read(); rtStart=rt_timer_read();
rt_task_sleep(1000*1000*1000); //in ns
pshm->P[0]=1;
rt_task_sleep(100*1000*1000); //in ns
pshm->P[1]=1;
for(i=0;i<100&&pshm->Gather.Enable;i++) for(i=0;;i++)
{ {
rtSlice=rtStart+i*40*1E6; // a slice is 40 ms
rt_task_sleep_until(rtSlice); //in ns
pshm->P[1]=1; pshm->P[1]=1;
rt_task_sleep(1*1000*1000); //in ns //while(rt_timer_read()-rtStart<1E6*40*i)
// rt_task_wait_period(NULL);
rt_task_sleep_until(rtSlice+5*1E6); //in ns
pshm->P[1]=0; pshm->P[1]=0;
rt_task_sleep(39*1000*1000); //in ns
if(pshm->Coord[1].Q[0]==-10)
{
rt_task_sleep_until(rtSlice+10*1E6); //in ns
pshm->P[0]=1;
rt_task_sleep_until(rtSlice+20*1E6); //in ns
pshm->P[0]=0;
}
if(pshm->Coord[1].Q[0]==-1)
break;
putchar('.');fflush(stdout); putchar('.');fflush(stdout);
} }
if(pshm->Gather.Enable) if(pshm->Gather.Enable)
{ {
pshm->Gather.Enable=0; //pshm->Gather.Enable=0;
} }
pshm->P[1]=1; pshm->P[1]=1;
rt_task_sleep(1*1000*1000); //in ns rt_task_sleep(1*1E6); //in ns
pshm->P[1]=0; pshm->P[1]=0;
printf("trigsim_func done:\n"); printf("trigsim_func done:\n");
} }
@@ -176,11 +189,13 @@ void trigsync_run()
printf("Wait end of rt_task\n"); printf("Wait end of rt_task\n");
rt_task_join (&trigsim_task); rt_task_join(&trigsync_task);
printf("trigsync_task done\n");
pshm->Coord[1].Q[0]=-1;
rt_task_join(&trigsim_task);
printf("trigsim_task done\n"); printf("trigsim_task done\n");
rt_task_join (&trigsync_task);
printf("trigsync_task done\n");
printf("rt_task ended\n"); printf("rt_task ended\n");