first running sync with rt-C-code

This commit is contained in:
2019-01-22 14:41:16 +00:00
parent 7c2a2c5fe6
commit 66e2d038ba
5 changed files with 217 additions and 136 deletions

View File

@@ -45,6 +45,11 @@ class MotionBase:
self.sync_wait and self.sync_run
sync_wait can be put in the program to force a timing sync
sync_run are the commands to run the whole program
flag=0 : real start and frame trigger
flag=1 : simulated start and real frame trigger
flag=2 : real start and simulated frame trigger
flag=3 : simulated start and frame trigger
pt2pt_time : time point to point (needed sor sync code)
'''
if mode==0:
try:
@@ -53,15 +58,15 @@ class MotionBase:
pass
self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, crdId=crdId)
elif mode==1:
# code block to insert in the program
# - waits untis <flag> is false
# - waits raising edge of <flag> and the sets DesTimeBase=ServoPeriod
# flag ='PowerBrick[0].GpioData[0].0.1==1'
flag0='Gate3[1].Chan[0].UserFlag'
flag1='Gate3[1].Chan[1].UserFlag'
flag0='P0';flag1='P1'
prg = '''
elif mode in(1,2):
#frequence jitter 50Hz Swissgrid:
#https://www.swissgrid.ch/de/home/operation/grid-data/current-data.html#
flag=kwargs.get('flag',0);
pt2pt_time=kwargs.get('pt2pt_time',40);
flag0='Coord[1].Q[10]' if flag&1 else 'Gate3[1].Chan[0].UserFlag'
flag1='Coord[1].Q[11]' if flag&2 else 'Gate3[1].Chan[1].UserFlag'
if mode==1:
prg = '''
Coord[1].Q[1]=-2
Coord[1].TimeBaseSlew=1 //1E-4 is default
Coord[1].DesTimeBase=0
@@ -71,36 +76,41 @@ class MotionBase:
while({flag1}==0){{}}
Coord[1].DesTimeBase=Sys.ServoPeriod
'''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
else:
prg = '''
//Gather.Enable=2 is done in the sync program
Coord[1].TimeBaseSlew=1 //1E-4 is default
Coord[1].DesTimeBase=0
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)
elif mode==2:
#frequence jitter 50Hz Swissgrid:
#https://www.swissgrid.ch/de/home/operation/grid-data/current-data.html#
flag0='Gate3[1].Chan[0].UserFlag'
flag1='Gate3[1].Chan[1].UserFlag'
flag0='P0';flag1='P1'
prg='''
//Gather.Enable=2 is done in the sync program
Coord[1].TimeBaseSlew=1 //1E-4 is default
Coord[1].DesTimeBase=0
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)
#download and start triggerSync code
comm=self.comm
sftp=comm.sftp
dst='/tmp/triggerSync'
src=os.path.abspath(os.path.join(os.path.dirname(__file__), '../src/triggerSync/triggerSync'))
sftp.put(src, dst)
sftp.chmod(dst, 0o755)
self.cmdSync=cmd='LD_LIBRARY_PATH=/opt/ppmac/libppmac/ '+dst
#self.syncShell=comm.shell_channel(cmd)
self.syncChan=sc=comm._client.exec_command(cmd)
#ch=sc[1].channel
#ch.settimeout(1)
print ('starting '+cmd)
trigMode=0;
if mode==2:trigMode+=1 #synchronize
trigMode+=flag*2 #simulated or real triggers
trigMode+=8 # verbose
if mode==2 or trigMode&4:
sftp = comm.sftp
dst = '/tmp/triggerSync'
src = os.path.abspath(os.path.join(os.path.dirname(__file__), '../src/triggerSync/triggerSync'))
sftp.put(src, dst)
sftp.chmod(dst, 0o755)
cmd = 'LD_LIBRARY_PATH=/opt/ppmac/libppmac/ ' + dst
cmd+=' %g %d'%(pt2pt_time,trigMode)
self.cmdSync = cmd
print ('starting '+cmd)
self.syncShell=comm.shell_channel(cmd)
#self.syncChan=sc=comm._client.exec_command(cmd)
#ch=sc[1].channel
#ch.settimeout(1)
#sc[1].channel.setblocking(False)
#sc[1].read(100)
if flag&1:
print('set Coord[1].Q[10]=1 to start motion')
def run(self):
'runs the code sync_run which has been generated with setup_sync()'
@@ -112,17 +122,4 @@ class MotionBase:
raise 'Need to call setup sync before'
gpascii.send_block(cmd)
dst='/tmp/triggerSync'
src=os.path.abspath(os.path.join(os.path.dirname(__file__), '../src/triggerSync/triggerSync'))
cmd='LD_LIBRARY_PATH=/opt/ppmac/libppmac/ '+dst
# self.syncShell=comm.shell_channel(cmd)
try:
cmd=self.cmdSync
except AttributeError: pass
else:
import time
time.sleep(1)
self.syncChan=sc=comm._client.exec_command(cmd)

View File

@@ -286,7 +286,7 @@ class ShapePath(MotionBase):
self.ptsCorr=ptsCorr
print(ptsCorr)
def setup_gather(self,acq_per=1):
def setup_gather(self,acq_per=1,sim=False):
'''
setup the channels to gather
kwargs:
@@ -295,9 +295,10 @@ class ShapePath(MotionBase):
comm=self.comm
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]")
if sim:
address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Coord[1].Q[11]")
else:
address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Gate3[1].Chan[1].UserFlag")
gt.set_address(*address)
gt.set_property(MaxSamples=1000000, Period=acq_per)
@@ -492,9 +493,6 @@ class ShapePath(MotionBase):
else:
print(syncShell.sync())
del self.syncShell
syncShell.close()
pts=self.points
ofsy=-rec[0,2]+pts[0,1]
@@ -832,17 +830,37 @@ if __name__=='__main__':
# [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=8,numCir=12, ofs=(0, 0))
sp.gen_spiral_points(rStart=100,rInc=20,numSeg=8,numCir=32, 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)
#works: direct start
#sp.setup_gather(acq_per=1)
#sp.setup_sync(mode=0)
#works: simulated start and frame trigger no sync
#sp.setup_gather(acq_per=1,sim=True)
#sp.setup_sync(mode=1,flag=3,pt2pt_time=40)
#works: simulated start and frame trigger with sync
#sp.setup_gather(acq_per=1,sim=True)
#sp.setup_sync(mode=2,flag=3,pt2pt_time=40)
#works: simulated start real frame trigger no sync
#sp.setup_gather(acq_per=1)
#sp.setup_sync(mode=1,flag=1,pt2pt_time=40)
#works: simulated start real frame trigger with sync
sp.setup_gather(acq_per=1)
sp.setup_sync(mode=1) #sync with timing system and PLC to sync speed (PROG)
sp.setup_sync(mode=2,flag=1,pt2pt_time=40)
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=1, pt2pt_time=40,scale=0,dwell=10)
sp.run()
sp.gather_upload(fnRec=fn+'.npz')
sp.plot_gather(mode=11)

View File

@@ -1,31 +1,24 @@
//*-----------------------------------------------------------------------*
//| |
//| Copyright (c) 2019 by Paul Scherrer Institute (http://www.psi.ch) |
//| |
//| Author Thierry Zamofing (thierry.zamofing@psi.ch) |
//*-----------------------------------------------------------------------*
// >>>>>>> https://www.ashwinnarayan.com/post/xenomai-realtime-programming-part-2/
// >>>>>>> https://xenomai.org/documentation/xenomai-2.4/html/api/group__task.html
// Usage -> read the usage function
//samples: root@:/opt/ppmac#
// /tmp/triggerSync 40 11 trigger all 40 ms, simulated start, use real frame triggers, verbose
// /tmp/triggerSync 40 14 trigger all 40 ms, simulated start and frame triggers, no sync, verbose
// /tmp/triggerSync 40 15 trigger all 40 ms, simulated start and frame triggers, with sync, verbose
// /tmp/triggerSync 40 7 trigger all 40 ms, simulated start and frame triggers, with sync, minimal verbose
//mode:
// bit0: sync mode
// bit1: simulate mode
// bit2: verbose
//before start set: Gather.Enable=1
//to trigger a simulated start: Coord[1].Q[10]=1
// 0: useless
// 1: synchronize real triggers
// 2: simulate triggers (no sync)
// 3: synchronize simulate triggers
//in simulate mode:
//set pshm->Coord[1].Q[10]=1 to simulate a Jungfrau aquire start
//set pshm->Coord[1].Q[10]=2 to stop simulate trigger generation
//Coord[1].Q[11] is the simulated frame trigger
//in synchronize mode
//Coord[1].Q[0]=-2 : trigsync_func start, Wait for 'arm' trigger
//Coord[1].Q[0]=-1 : got 'arm' trigger, wait frame trigger
//Coord[1].Q[0]= 0 : got frame trigger 0
//Coord[1].Q[0] is incremented at each trigger
//sync task ends when Gather.Enable==0
//Gather.Enable=1
//the simulated frame trigger comes at 40.2 ms period.
#include <stdio.h>
#include <gplib.h>
@@ -45,10 +38,12 @@ static float mtPt2Pt=40.f; //motion point to point time
#define SIMFLAG0 (pshm->Coord[1].Q[10])
#define SIMFLAG1 (pshm->Coord[1].Q[11])
#define FLAG0 (gate3_1->Chan[0].Status&0x8000000)
#define FLAG1 (gate3_1->Chan[1].Status&0x8000000)
//#define SIMFLAG0 (pshm->P[10])
//#define SIMFLAG1 (pshm->P[11])
//Power PMAC Software Reference Manual.pdf page 760
#define FLAG0 (gate3_1->Chan[0].Status&0x800)
#define FLAG1 (gate3_1->Chan[1].Status&0x800)
RT_TASK trigsync_task;
void trigsync_func(void *arg)
{
RT_TASK *curtask;
@@ -72,12 +67,15 @@ void trigsync_func(void *arg)
float srvPer;
float mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed
float mtPt2Pt=40.f; //motion point to point time
unsigned int maxDiff;
struct GateArray3* gate3_1=GetGate3MemPtr(1);
pshm = GetSharedMemPtr();
if(!pshm->Gather.Enable)
{
printf("ending trigsync_func because Gather.Enable==0\n");
return;
}
pshm->Coord[1].Q[0]=-2;
srvPer=pshm->ServoPeriod;
@@ -94,8 +92,10 @@ void trigsync_func(void *arg)
rt_task_wait_period(NULL);
}
printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1E6);
pshm->Gather.Enable=2; //start gathering data
pshm->Coord[1].Q[0]=-1;
if(mode&2)
if(mode&4)
{
while(!SIMFLAG1)
rt_task_wait_period(NULL);
@@ -105,16 +105,24 @@ void trigsync_func(void *arg)
while(!FLAG1)
rt_task_wait_period(NULL);
}
//puts("waitFlag1");
//gate3_1->Chan[1].Status f057f77f
//gate3_1->Chan[1].Status f057ff7f
//printf("gate3_1->Chan[1].Status %x\n",gate3_1->Chan[1].Status);
printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1E6);
rtStart=rtLast=rt_timer_read();
scStart=scLast=pshm->ServoCount;
mtAct=0.f;
pshm->Coord[1].Q[0]=0;
pshm->Coord[1].DesTimeBase=srvPer; //start motion at default speed
if(mode&8)
printf("Start: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.6f\n", i, rtLast/1E6,scLast, mtAct,mtDes,srvPer);
for(i=1;pshm->Gather.Enable;i++)
{
if(mode&2)
if(mode&4)
{
while(SIMFLAG1)
rt_task_wait_period(NULL);
@@ -141,14 +149,14 @@ void trigsync_func(void *arg)
srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff;
pshm->Coord[1].DesTimeBase=srvPer;
pshm->Coord[1].Q[0]++;
if(mode&4)
printf("Trigger count: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.3f\n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer);
if(mode&8)
printf("Trigger count: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.6f\n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer);
rtLast=rtCur;
scLast=scCur;
}
printf("trigsync_func done\n");
}
RT_TASK trigsim_task;
void trigsim_func(void *arg)
{
RT_TASK *curtask;
@@ -162,11 +170,11 @@ void trigsim_func(void *arg)
//rt_task_set_periodic(NULL, TM_NOW, 10*1E6);
printf("Starting task %s \n", curtaskinfo.name);
int i;
int i,j=0;
RTIME rtStart,rtSlice;
printf("sizeof RTTIME %d\n",sizeof(rtStart));
//printf("sizeof RTTIME %d\n",sizeof(rtStart));
pshm = GetSharedMemPtr();
pshm->Coord[1].Q[10]=0;
SIMFLAG0=0;
rtStart=rt_timer_read();
for(i=0,rtSlice=rtStart;;i++)
@@ -175,25 +183,39 @@ void trigsim_func(void *arg)
rtSlice=rtStart+i*40.2*1E6; // a slice is 40 ms
//rtSlice+=(rand()%(int)(1*1E6)); //0.1ms jitter
rt_task_sleep_until(rtSlice); //in ns
pshm->Coord[1].Q[11]=1;
//while(rt_timer_read()-rtStart<1E6*40*i)
// rt_task_wait_period(NULL);
rt_task_sleep_until(rtSlice+5*1E6); //in ns
pshm->Coord[1].Q[11]=0;
//if(pshm->Coord[1].DesTimeBase==0)
if(pshm->Coord[1].Q[10]==1)
{
rt_task_sleep_until(rtSlice+10*1E6); //in ns
pshm->Coord[1].Q[11]=1;
rt_task_sleep_until(rtSlice+20*1E6); //in ns
pshm->Coord[1].Q[11]=0;
pshm->Coord[1].Q[10]=0;
}
if(pshm->Coord[1].Q[10]=2)
break;
if(mode&4)
putchar('.');fflush(stdout);
{
SIMFLAG1=1;
//while(rt_timer_read()-rtStart<1E6*40*i)
// rt_task_wait_period(NULL);
rt_task_sleep_until(rtSlice+5*1E6); //in ns
SIMFLAG1=0;
if(mode&8)
{putchar('.');fflush(stdout);}
}
//if(pshm->Coord[1].DesTimeBase==0)
if(SIMFLAG0==1)
{
if(j)
{
SIMFLAG0=0;j=0;
}
else if(mode&8)
{putchar('x');fflush(stdout);}
j++;
}
if(!pshm->Gather.Enable)
{
if((mode&4) && j<10)
{
putchar('o');
j++;
continue;
}
break;
}
//if(pshm->Coord[1].Q[10]==2)
// break;
}
printf("trigsim_func done\n");
}
@@ -203,6 +225,8 @@ void trigsim_func(void *arg)
void trigsync_run()
{
RT_TASK trigsync_task;
RT_TASK trigsim_task;
const char* strSync="trigsync_task";
const char* strSim="trigsim_task";
//mode:
@@ -241,40 +265,80 @@ void trigsync_run()
}
int usage(const char* cmd)
{
const char* s="\n\
mode:\n\
bit0:1: sync mode\n\
bit1:2: simulate start trigger\n\
bit2:4: simulate frame trigger\n\
bit3:8: verbose\n\
\n\
simulate start trigger:\n\
set pshm->Coord[1].Q[10]=1 to simulate a Jungfrau aquire start\n\
\n\
simulate frame trigger\n\
is output to pshm->Coord[1].Q[11]\n\
1: synchronize real frame and start triggers\n\
3: synchronize real frame and simulated start triggers\n\
6: simulated frame and start triggers (no sync)\n\
7: synchronize simulated frame and start triggers\n\
\n\
in simulate mode:\n\
set pshm->Coord[1].Q[10]=1 to simulate a Jungfrau aquire start\n\
set pshm->Coord[1].Q[10]=2 to stop simulate trigger generation\n\
Coord[1].Q[11] is the simulated frame trigger\n\
\n\
in synchronize mode\n\
Coord[1].Q[0]=-2 : trigsync_func start, Wait for 'arm' trigger\n\
Coord[1].Q[0]=-1 : got 'arm' trigger, wait frame trigger\n\
Coord[1].Q[0]= 0 : got frame trigger 0\n\
Coord[1].Q[0] is incremented at each trigger\n\
sync task ends when Gather.Enable==0\n\
";
printf("usage:\n%s pt2ptTime mode\n",cmd);
puts(s);
return -1;
}
int main(int argc, char *argv[])
{
int err;
int initialized=0;
char s[256];
int i;
char *s;
if(argc!=3)
return usage(argv[0]);
mtPt2Pt= strtof(argv[1], &s);
if (argv[1]==s)
return usage(argv[0]);
mode= (int)strtol(argv[2], &s, 10);
if (argv[2]==s)
return usage(argv[0]);
if ((err = InitLibrary()) != 0) {
abort();
}
initialized = 1;
pshm = GetSharedMemPtr();
printf("P:%g\n",pshm->P[1011]);
printf("pshm->MaxRtPlc:%d\n",pshm->MaxRtPlc);
for(i=0;i<8;i++) //MAX_MOTORS instead of 8
printf("pshm->Motor[%d].Ctrl:%p\n",i,pshm->Motor[i].Ctrl);
for(i=0;i<8;i++) //MAX_MOTORS instead of 8
printf("pshm->UserAlgo.ServoCtrlAddr[%d]:%d\n",i,pshm->UserAlgo.ServoCtrlAddr[i]);
for(i=0;i<MAX_PLC;i++) //MAX_MOTORS instead of 8
printf("pshm->UserAlgo.BgCplcPID[%d]:%d\n",i,pshm->UserAlgo.BgCplcPID[i]);
printf("pshm->UserAlgo.RtiCplcPID:%d\n",pshm->UserAlgo.RtiCplcPID);
printf("pshm->UserAlgo.RtiCplcMaxTime:%d\n",pshm->UserAlgo.RtiCplcMaxTime);
printf("pshm->UserAlgo.CaptCompFuncAddr:%p\n",pshm->UserAlgo.CaptCompFuncAddr);
//initialized = 1;
//pshm = GetSharedMemPtr();
//
//printf("P:%g\n",pshm->P[1011]);
//printf("pshm->MaxRtPlc:%d\n",pshm->MaxRtPlc);
//
//for(i=0;i<8;i++) //MAX_MOTORS instead of 8
// printf("pshm->Motor[%d].Ctrl:%p\n",i,pshm->Motor[i].Ctrl);
//
//for(i=0;i<8;i++) //MAX_MOTORS instead of 8
// printf("pshm->UserAlgo.ServoCtrlAddr[%d]:%d\n",i,pshm->UserAlgo.ServoCtrlAddr[i]);
//
//for(i=0;i<MAX_PLC;i++) //MAX_MOTORS instead of 8
// printf("pshm->UserAlgo.BgCplcPID[%d]:%d\n",i,pshm->UserAlgo.BgCplcPID[i]);
//
//printf("pshm->UserAlgo.RtiCplcPID:%d\n",pshm->UserAlgo.RtiCplcPID);
//printf("pshm->UserAlgo.RtiCplcMaxTime:%d\n",pshm->UserAlgo.RtiCplcMaxTime);
//printf("pshm->UserAlgo.CaptCompFuncAddr:%p\n",pshm->UserAlgo.CaptCompFuncAddr);
trigsync_run();
//trigsync_func(0);
//loop_task_run();
CloseLibrary();
return !err;

View File

@@ -25,12 +25,14 @@
<search_path add="/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac" />
<search_path add="/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac" />
<search_path add="/opt/eldk-4.2/ppc_4xxFP/usr/include" />
<search_path add="/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/include" />
</code_completion>
<lib_finder disable_auto="1" />
<debugger>
<search_path add="/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac" />
<search_path add="/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac" />
<search_path add="/opt/eldk-4.2/ppc_4xxFP/usr/include" />
<search_path add="/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/include" />
<remote_debugging>
<options conn_type="0" serial_baud="115200" ip_address="MOTTEST-CPPM-CRM0573" ip_port="2000" additional_cmds_before="file triggerSync" />
</remote_debugging>

View File

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