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

wip

Browse Source
This commit is contained in:
zamofing_t
2019-01-16 14:29:29 +01:00
parent ee70ff070a
commit 3e9485ee25
4 changed files with 197 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
python/MXMotion.py
View File

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

47
python/shapepath.py
View File

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

201
src/triggerSync/triggerSync.c
View File

@@ -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 <stdio.h>
#include <gplib.h>
#include <math.h>
#include <stdio.h>
#include <gplib.h>
#include <math.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>
@@ -11,12 +11,18 @@
#include <native/timer.h>
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;
}

12
src/triggerSync/triggerSync.layout
View File

@@ -2,14 +2,14 @@
<CodeBlocks_layout_file>
<FileVersion major="1" minor="0" />
<ActiveTarget name="Debug" />
<File name="Makefile" open="1" top="1" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="853" topLine="12" />
</Cursor>
</File>
<File name="triggerSync.c" open="1" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="835" topLine="9" />
<Cursor1 position="952" topLine="9" />
</Cursor>
</File>
<File name="Makefile" open="1" top="1" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="692" topLine="1" />
</Cursor>
</File>
</CodeBlocks_layout_file>