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

pvt motion

Browse Source
This commit is contained in:
zamofing_t
2017-01-04 13:51:18 +01:00
parent 85f20ef491
commit a265d91f2f
7 changed files with 141 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
python/shapepath.py
View File

@@ -64,9 +64,19 @@ class ShapePath:
#cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
#cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=0.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg")','plot_gather()']}
#cfg = {"sequencer": ['gen_rand_points(n=107, scale=1000)', 'sort_points()','plot_gather()']}
cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg")','plot_gather()']}
#cfg={"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
#cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
#cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg")','plot_gather()']}
#fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')
#cfg={"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']}
#cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
#cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1)', 'plot_gather()']}
#cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=100,rnd=0)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
#cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
#cfg = {"points": [[0, 0],[100, 0],[200, 0],[300, 0],[400, 0],[400, 100],[300, 100],[200, 100],[100, 100],[0, 100],[10, 200],[100, 200],[200, 200],[300, 200],[400, 200],[410, 300],[300, 300],[200, 300],[100, 300],[0, 300],[0, 400],[100, 400],[200, 400],[300, 400],[400, 400]],"sequencer": ['gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
#cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=-1)', 'plot_gather()']}
#cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=1000)', 'plot_gather()']}
cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather()']}
#cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather()']}
self.cfg=dotdict(cfg)
self.args=args
@@ -103,9 +113,11 @@ class ShapePath:
pts+=(np.random.rand(pts.shape[0],2)*(rnd*pitch))
self.points=pts
def gen_prog(self,prgId=2,file=None,host=None,mode=0):
def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
prg=[]
gather={"MaxSamples":100000, "Period":10 }
gather={"MaxSamples":100000, "Period":1 }
#Sys.ServoPeriod= 0.2
self.meta = {'timebase': 0.2*gather['Period']}
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos","Motor[1].DesPos","Motor[2].DesPos","Motor[3].DesPos"]
prg.append('Gather.Enable=0')
@@ -118,16 +130,64 @@ class ShapePath:
prg.append('open prog %d'%(prgId))
# this uses Coord[1].Tm and limits with MaxSpeed
prg.append('Gather.Enable=2')
if mode==0:
if mode==-1:
pos=self.points
prg.append(' linear abs')
data=self.points
for idx in range(data.shape[0]):
prg.append('X(%f) Y(%f)'%tuple(data[idx,:]))
prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
prg.append('dwell 10')
prg.append('Gather.Enable=2')
prg.append('jog2:10000')
prg.append('dwell 100')
if mode==1:
pass
prg.append('Gather.Enable=0')
prg.append('jog3:10000')
prg.append('dwell 100')
prg.append('jog2:-10000')
prg.append('dwell 100')
prg.append('jog3:-10000')
prg.append('dwell 100')
prg.append('Gather.Enable=0')
elif mode==0:
pos=self.points
prg.append(' linear abs')
prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
prg.append('dwell 10')
prg.append('Gather.Enable=2')
prg.append(' linear abs')
for idx in range(pos.shape[0]):
prg.append('X%g Y%g'%tuple(pos[idx,:]))
prg.append('dwell 100')
prg.append('Gather.Enable=0')
elif mode==1:
try:
pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
except KeyError:
print 'missing pt2pt_time, use default=100ms'
pt2pt_time=100.
pt=self.points
vel=pt[2:,:]-pt[:-2,:]
#pv is an array of posx posy velx vely
pv=np.ndarray(shape=(pt.shape[0]+2,4),dtype=pt.dtype)
pv[:]=np.NaN
#pv[ 0,(0,1)]=2*pt[0,:]-pt[1,:]
pv[ 0,(0,1)]=pt[0,:]
pv[ 1:-1,(0,1)]=pt
#pv[ -1,(0,1)]=2*pt[-1,:]-pt[-2,:]
pv[ -1,(0,1)]=pt[-1,:]
pv[(0,0,-1,-1),(2,3,2,3)]=0
dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
pv[ 1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time)
prg.append(' linear abs')
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
prg.append('dwell 10')
prg.append('Gather.Enable=2')
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
for idx in range(1,pv.shape[0]):
prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)]))
prg.append('X%g Y%g' % tuple(pv[-1, (0,1)]))
prg.append('dwell 1000')
prg.append('Gather.Enable=0')
prg.append('close')
prg.append('&1\nb%dr\n'%prgId)
if self.args.verbose & 4:
@@ -200,11 +260,17 @@ class ShapePath:
self.ax=ax
self.hl=hl
def plot_gather(self,fnLoc='/tmp/gather.txt',fnOut='/tmp/shapepath.npz'):
def plot_gather(self,fnOut='/tmp/shapepath.npz',fnLoc='/tmp/gather.txt'):
meta=self.meta
pts=self.points
rec = np.genfromtxt(fnLoc, delimiter=' ')
if fnOut:
np.savez_compressed(fnOut, rec=rec, pts=pts)
# time base:
# Gather.Period=10
# Sys.ServoPeriod=0.2 -> 5kHz
# -> aquired data all 10*0.2ms= 2 ms -> 30000 points = 60 sec
np.savez_compressed(fnOut, rec=rec, pts=pts, meta=meta)
fig=plt.figure()
ax = fig.add_subplot(1,1,1)
#hl=ax[0].plot(x, y, color=col)
@@ -223,7 +289,7 @@ class ShapePath:
ax = fig.add_subplot(1, 1, 1)
err=np.sqrt((rec[:,1]-rec[:,4])**2+(rec[:,1]-rec[:,4])**2)
hl = ax.plot(err, 'r-')
ax.xaxis.set_label_text('time (10x servo cycle)')
ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)' % meta['timebase'])
ax.yaxis.set_label_text('pos-error um')
plt.show()