From a11a3bce7926e0e51591275d27326793dd8ff6a8 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Thu, 17 Jan 2019 15:56:18 +0100 Subject: [PATCH] non adapting triggering with triggerSync.c --- python/MXMotion.py | 6 +- python/PBMotionAnalyzer/PBMotionAnalyzer.py | 16 ++-- python/shapepath.py | 88 ++++++++++++++------- src/triggerSync/triggerSync.c | 53 ++++++++----- 4 files changed, 106 insertions(+), 57 deletions(-) diff --git a/python/MXMotion.py b/python/MXMotion.py index 941a169..473cac2 100644 --- a/python/MXMotion.py +++ b/python/MXMotion.py @@ -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) diff --git a/python/PBMotionAnalyzer/PBMotionAnalyzer.py b/python/PBMotionAnalyzer/PBMotionAnalyzer.py index 13dee84..95701a7 100755 --- a/python/PBMotionAnalyzer/PBMotionAnalyzer.py +++ b/python/PBMotionAnalyzer/PBMotionAnalyzer.py @@ -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 diff --git a/python/shapepath.py b/python/shapepath.py index 6853504..ff8a94b 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -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) diff --git a/src/triggerSync/triggerSync.c b/src/triggerSync/triggerSync.c index f131da4..64369e7 100644 --- a/src/triggerSync/triggerSync.c +++ b/src/triggerSync/triggerSync.c @@ -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(); printf("Wait for trigger:\n"); 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); pshm->Coord[1].Q[0]=-9; - if(!pshm->Gather.Enable) - pshm->Gather.Enable=2; //start record at flag0' trigger while(!FLAG1) 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); rtStart=rtLast=rt_timer_read(); scStart=scLast=pshm->ServoCount; @@ -81,7 +83,6 @@ void trigsync_func(void *arg) while(FLAG1) rt_task_wait_period(NULL); - while(pshm->Gather.Enable) for(i=0;pshm->Gather.Enable;i++) { while(!FLAG1) @@ -119,33 +120,45 @@ void trigsim_func(void *arg) curtask = rt_task_self(); rt_task_inquire(curtask, &curtaskinfo); //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 - 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; - RTIME rtStart; + RTIME rtStart,rtSlice; + printf("sizeof RTTIME %d\n",sizeof(rtStart)); pshm = GetSharedMemPtr(); 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; - 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; - 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); } if(pshm->Gather.Enable) { - pshm->Gather.Enable=0; + //pshm->Gather.Enable=0; } pshm->P[1]=1; - rt_task_sleep(1*1000*1000); //in ns + rt_task_sleep(1*1E6); //in ns pshm->P[1]=0; printf("trigsim_func done:\n"); } @@ -176,11 +189,13 @@ void trigsync_run() 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"); - rt_task_join (&trigsync_task); - printf("trigsync_task done\n"); printf("rt_task ended\n");