diff --git a/python/MXMotion.py b/python/MXMotion.py index 00085b6..941a169 100644 --- a/python/MXMotion.py +++ b/python/MXMotion.py @@ -62,10 +62,11 @@ class MotionBase: flag1='Gate3[1].Chan[1].UserFlag' flag0='P0';flag1='P1' prg = ''' - Coord[1].Q[1]=1 + Coord[1].Q[1]=-2 while({flag0}==0){{}} + Coord[1].Q[1]=-1 + Gather.Enable=2 while({flag1}==0){{}} - Coord[1].Q[1]=2 '''.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) @@ -77,9 +78,9 @@ class MotionBase: flag0='P0';flag1='P1' pt2pt_time=40 prg=''' - Coord[1].Q[1]=1 + //Gather.Enable=2 if done in the sync program Coord[1].DesTimeBase=0 - Coord[1].Q[1]=2 + Coord[1].Q[1]=-1 '''.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/shapepath.py b/python/shapepath.py index 8b0bd42..6853504 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -287,6 +287,9 @@ class ShapePath(MotionBase): gt=self.gather gt.set_phasemode(False) address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Gate3[1].Chan[1].UserFlag") + address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Sys.P[1]") + #address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Coord[1].Q[1]") + gt.set_address(*address) gt.set_property(MaxSamples=1000000, Period=acq_per) ServoPeriod= .2 #0.2ms #Sys.ServoPeriod is dependent of !common() macro @@ -318,13 +321,16 @@ class ShapePath(MotionBase): mode=-1 jog a 10mm square mode=0 linear motion mode=1 pvt motion - kwargs: scale: scaling velocity (default=1. value=0 would stop at the point + kwargs: + pt2pt_time : time to move from one point to the next point + sync_frq : synchronization mark all n points (default=10) + scale : scaling velocity (default=1. value=0 would stop at the point + cnt : move path multiple times (default=1) + dwell : dwell time at end (default=100ms) + mode=2 spline motion mode=3 pvt motion using inverse fft velocity - kwargs: scale: scaling velocity (default=1. value=0 would stop at the point - kwargs: - pt2pt_time : time to move from one point to the next point - sync_frq : synchronization mark all n points + kwargs: same as pvt motion ''' prg=['close all buffers','open prog %d'%(prgId)] comm=self.comm @@ -364,6 +370,7 @@ class ShapePath(MotionBase): self.meta['pt2pt_time']=pt2pt_time cnt=kwargs.get('cnt', 1) # move path multiple times sync_frq=kwargs.get('sync_frq', 10) # synchronization mark all n points + dwell=kwargs.get('dwell', 100) # synchronization mark all n points CoordFeedTime=1000. #Defaut deltatau value try: pt=self.ptsCorr @@ -403,17 +410,17 @@ class ShapePath(MotionBase): try: prg.extend(self.sync_prg.split('\n')) except AttributeError: #print('no sync code available') - pass - prg.append('Gather.Enable=2') + prg.append('Gather.Enable=2') if cnt>1: prg.append('P100=%d'%cnt) prg.append('N100:') prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position for idx in range(1,pv.shape[0]): if sync_frq is not None and idx%sync_frq==0: - prg.append('Coord[1].Q[0]=%d'%(idx)) + prg.append('Coord[1].Q[1]=%d'%(idx)) prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)])) - prg.append('Coord[1].Q[0]=%d' % (idx)) + if sync_frq is not None: + prg.append('Coord[1].Q[1]=%d' % (idx)) prg.append('X%g Y%g' % tuple(pv[-1, (0,1)])) if cnt>1: prg.append('dwell 10') @@ -426,10 +433,7 @@ class ShapePath(MotionBase): prg.append('goto 100') prg.append('}') else: - prg.append('dwell 1000') - if sync_frq is not None: - prg.append('Coord[1].Q[0]=-1') - prg.append('Coord[1].Q[1]=0') + prg.append('dwell %d'%dwell) prg.append('Gather.Enable=0') elif mode==2: #### spline motion try: @@ -562,6 +566,20 @@ class ShapePath(MotionBase): self.ax=ax self.hl=hl + 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 + + 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) + + if mode&2: fig = plt.figure('position error') ax = fig.add_subplot(1, 1, 1) @@ -588,6 +606,7 @@ class ShapePath(MotionBase): # idxTrigger=np.hstack(([0],rec[:,4])) # doc.idxTrigger=idxTrigger=np.where(np.diff(idxTrigger)==1)[0] idxTrigger = rec[:, 4] + idxTrigger = np.where(np.diff(idxTrigger) == 1)[0] + 1 idxInPos = [] # first point at idx 0 try: # find approximate distance of 2 points @@ -785,7 +804,7 @@ if __name__=='__main__': sp.setup_gather(acq_per=2) 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) + sp.setup_motion(fnPrg=fn + '.prg', mode=3, pt2pt_time=40,scale=1,dwell=10) sp.run() sp.gather_upload(fnRec=fn+'.npz') sp.plot_gather(mode=11) diff --git a/src/triggerSync/triggerSync.c b/src/triggerSync/triggerSync.c index 90c8f05..f131da4 100644 --- a/src/triggerSync/triggerSync.c +++ b/src/triggerSync/triggerSync.c @@ -1,9 +1,9 @@ // >>>>>>> https://www.ashwinnarayan.com/post/xenomai-realtime-programming-part-2/ // >>>>>>> https://xenomai.org/documentation/xenomai-2.4/html/api/group__task.html -#include -#include -#include +#include +#include +#include #include #include #include @@ -11,12 +11,18 @@ #include -extern struct SHM *pshm; +extern struct SHM *pshm; #define CLOCK_RES 1e-9 //Clock resolution is 1 ns by default -#define LOOP_PERIOD 1e7 //Expressed in ticks +#define LOOP_PERIOD 1e4 //Expressed in ticks //RTIME period = 10000000; +//#define FLAG0 (gate3_1->Chan[0].Status&0x8000000) +//#define FLAG1 (gate3_1->Chan[1].Status&0x8000000) +#define FLAG0 (pshm->P[0]) +#define FLAG1 (pshm->P[1]) + + RT_TASK trigsync_task; void trigsync_func(void *arg) { @@ -24,34 +30,73 @@ void trigsync_func(void *arg) RT_TASK_INFO curtaskinfo; int iret = 0; - RTIME tstart, now; curtask = rt_task_self(); rt_task_inquire(curtask, &curtaskinfo); //Print the info - printf("Starting task %s with period of 10 ms ....\n", curtaskinfo.name); + printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,LOOP_PERIOD/1000000.f); //Make the task periodic with a specified loop period rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD); - int ctr = 0; - unsigned srvStart,srvCnt,diff,maxDiff; + int i; + RTIME rtStart,rtLast,rtCur; + unsigned scStart,scLast,diff,maxDiff; + struct GateArray3* gate3_1=GetGate3MemPtr(1); - tstart = rt_timer_read(); - pshm = GetSharedMemPtr(); - srvStart=srvCnt=pshm->ServoCount; + pshm = GetSharedMemPtr(); + + /* //Start the task loop - while(ctr<200){ - printf("sLoop count: %d, Loop time: %.5f ms\n", ctr, (rt_timer_read() - tstart)/1000000.0); - ctr++; + rtStart = rt_timer_read(); + for(i=0;i<20000;i++) + { + if (i%10==0) + printf("Loop count: %d, Loop time: %.5f ms\n", i, (rt_timer_read() - tStart)/1000000.0); rt_task_wait_period(NULL); //int rt_task_sleep_until } - /* float ang,pos; + */ + + rtStart=rt_timer_read(); + printf("Wait for trigger:\n"); + pshm->Coord[1].Q[0]=-10; + while(!FLAG0) + rt_task_wait_period(NULL); + 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 + printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1000000.0); + rtStart=rtLast=rt_timer_read(); + scStart=scLast=pshm->ServoCount; + pshm->Coord[1].Q[0]=1; + pshm->Coord[1].DesTimeBase=pshm->ServoPeriod; //start motion at default speed + while(FLAG1) + rt_task_wait_period(NULL); + + while(pshm->Gather.Enable) + for(i=0;pshm->Gather.Enable;i++) + { + while(!FLAG1) + rt_task_wait_period(NULL); + rtLast = rt_timer_read(); + scLast=pshm->ServoCount; + pshm->Coord[1].Q[0]++; + while(FLAG1) + rt_task_wait_period(NULL); + printf("Trigger count: %d, time: %.5f ms ServoCount %d\n", i, (rtLast - rtStart)/1000000.0,scLast-scStart); + } + + /* float ang,pos; int i; - float srvStart,srvCnt,diff,maxDiff,tmp; + float srvStart,srvCnt,diff,maxDiff,tmp; srvStart=srvCnt=pshm->ServoCount; for(maxDiff=0,i=0;i<10000000;i++) @@ -65,35 +110,95 @@ void trigsync_func(void *arg) printf("srvCnt %d diff %f maxDiff %f\n",(int)(srvCnt-srvStart),diff, maxDiff);*/ } -void trigsync_run() +RT_TASK trigsim_task; +void trigsim_func(void *arg) { - char str[20]; - //Lock the memory to avoid memory swapping for this program - mlockall(MCL_CURRENT | MCL_FUTURE); - printf("Starting cyclic task...\n"); - //Create the real time task - sprintf(str, "cyclic_task"); - rt_task_create(&trigsync_task, str, 0, 50, T_JOINABLE); - //Since task starts in suspended mode, start task - rt_task_start(&trigsync_task, &trigsync_func, 0); - //Wait for Ctrl-C - printf("Wait for Ctrl-C\n"); - rt_task_join (&trigsync_task); + RT_TASK *curtask; + RT_TASK_INFO curtaskinfo; + int iret = 0; + 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); + //Make the task periodic with a specified loop period + rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD); + int i; + RTIME 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++) + { + pshm->P[1]=1; + rt_task_sleep(1*1000*1000); //in ns + pshm->P[1]=0; + rt_task_sleep(39*1000*1000); //in ns + putchar('.');fflush(stdout); + } + if(pshm->Gather.Enable) + { + pshm->Gather.Enable=0; + } + pshm->P[1]=1; + rt_task_sleep(1*1000*1000); //in ns + pshm->P[1]=0; + printf("trigsim_func done:\n"); } -int main(int argc, char *argv[]) -{ - int err; + + +void trigsync_run() +{ + const char* strSync="trigsync_task"; + const char* strSim="trigsim_task"; + pshm->P[0]=0; + pshm->P[1]=0; + pshm->Coord[1].Q[0]=0; + //if(!pshm->Gather.Enable) + // pshm->Gather.Enable=1; + + //Lock the memory to avoid memory swapping for this program + mlockall(MCL_CURRENT | MCL_FUTURE); + printf("Starting rt task...\n"); + //Create the real time task + rt_task_create(&trigsync_task, strSync, 0, 50, T_JOINABLE); + //Since task starts in suspended mode, start task + rt_task_start(&trigsync_task, &trigsync_func, 0); + + rt_task_create(&trigsim_task, strSim, 0, 50, T_JOINABLE); + rt_task_start(&trigsim_task, &trigsim_func, 0); + + printf("Wait end of rt_task\n"); + + 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"); + +} + + +int main(int argc, char *argv[]) +{ + int err; int initialized=0; char s[256]; int i; - - if ((err = InitLibrary()) != 0) { - abort(); - } - initialized = 1; - pshm = GetSharedMemPtr(); + + if ((err = InitLibrary()) != 0) { + abort(); + } + initialized = 1; + pshm = GetSharedMemPtr(); printf("P:%g\n",pshm->P[1011]); printf("pshm->MaxRtPlc:%d\n",pshm->MaxRtPlc); @@ -116,12 +221,12 @@ int main(int argc, char *argv[]) //loop_task_run(); - CloseLibrary(); - return !err; - - if (initialized) - CloseLibrary(); - - return 0; - -} + CloseLibrary(); + return !err; + + if (initialized) + CloseLibrary(); + + return 0; + +} diff --git a/src/triggerSync/triggerSync.layout b/src/triggerSync/triggerSync.layout index a846b78..f8c0742 100644 --- a/src/triggerSync/triggerSync.layout +++ b/src/triggerSync/triggerSync.layout @@ -2,14 +2,14 @@ - - - - - - + + + + + +