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

helicascan: working generation of motion program!

Browse Source
This commit is contained in:
zamofing_t
2017-12-08 14:28:36 +01:00
parent b78bfacbc6
commit fec82fb362
3 changed files with 310 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files

434
python/helicalscan.py
View File

@@ -39,6 +39,7 @@ import matplotlib.animation as anim
from matplotlib.widgets import Slider
import subprocess as sprc
from utilities import *
import telnetlib
d2r=2*np.pi/360
@@ -147,7 +148,6 @@ class HelicalScan:
def test_coord_trf(self):
self.calcParam()
param = self.param
dx, dz, w, y, = (0.2,0.3,0.1,3.3)
print 'input : dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
@@ -171,7 +171,6 @@ class HelicalScan:
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
ax.view_init(elev=14., azim=10)
self.calcParam()
param=self.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
ctr=param[:,0:3].mean(0)[::-1]
@@ -210,8 +209,8 @@ class HelicalScan:
ofs=(p[1]+p[0])/2. # = center of the cristal
m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m)
plt.show()
plt.show()
def update_cx_cz_w_fy(self,val):
cx = self.sldCx.val
@@ -234,7 +233,7 @@ class HelicalScan:
m= m.dot(Trf.rotY(w*d2r))
self.hOrig = self.pltOrig(m,self.hOrig)
else:
self.hCrist,pt=self.pltCrist(fy,cx,cz,w*d2r,self.hCrist)
self.hCrist,pt=self.pltCrist(cx,cz,w*d2r,fy,self.hCrist)
#l.set_ydata(amp * np.sin(2 * np.pi * freq * t))
self.fig.canvas.draw_idle()
@@ -244,7 +243,6 @@ class HelicalScan:
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
ax.view_init(elev=14., azim=10)
self.calcParam()
param=self.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
@@ -293,9 +291,72 @@ class HelicalScan:
w=w*d2r
(cx,cz,w,fy)=self.inv_transform(dx,dz,w,y)
#print (cx,cz,w,fy)
self.hCrist,pt=self.pltCrist(-fy,-cx,-cz,w,self.hCrist)
self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
self.fig.canvas.draw_idle()
def interactive_anim(self):
fig = plt.figure()
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
ax.view_init(elev=14., azim=10)
param=self.param
fnLoc='/tmp/gather.txt'
#meta = self.meta
#pts = self.points # X,Y array
self.rec = np.genfromtxt(fnLoc, delimiter=' ')
#if fnOut:
# np.savez_compressed(fnOut, rec=rec, pts=pts, meta=meta)
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
ctr=(0,0,0)
self.axSetCenter(ctr,10)
axFrm=plt.axes([0.1, 0.01, 0.8, 0.02])
lx=[-1,1];ly=[0,1];lz=[-1,1]
ly = param[:,1]
self.sldFrm=sFrm=Slider(axFrm, 'frm', 0, self.rec.shape[0]-1, valinit=0)
sFrm.on_changed(self.update_anim)
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
p=np.ndarray((param.shape[0], 3))
for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i]
p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
p[i,1]=y_i # y= y_i
p[i,2]=z_i+r_i*np.cos(phi_i) # z= z_i+r_i*sin(phi_i*w)
ofs=(p[1]+p[0])/2. # = center of the cristal
print 'p, ofs',p,ofs
m=Trf.trans(0,0,0); self.hOrig=self.pltOrig(m)
hCrist,pt=self.pltCrist(cx=-ofs[0],fy=-ofs[1],cz=-ofs[2])
self.hCrist=hCrist;self.fig=fig
animCnt=100
self.step=self.rec.shape[0]/animCnt
a = anim.FuncAnimation(fig, self.anim_gather_data, animCnt, fargs=(), interval=20, repeat=False, blit=False)
plt.show()
def update_anim(self,val):
frm = self.sldFrm.val
(cx, cz, w, fy)=self.rec[int(frm),:]
#data/=. #scale from um to mm
w*=d2r/1000 # scale from deg to rad
print (cx,cz,w,fy)
self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
self.fig.canvas.draw_idle()
def anim_gather_data(self,idx):
(cx, cz, w, fy)=self.rec[int(idx*self.step),:]
w*=d2r/1000 # scale from deg to rad
self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
#data=self.rec[int(idx*self.step),:]
#self.hCrist,pt=self.pltCrist(*data,h=self.hCrist)
def fwd_transform(self,cx,cz,w,fy):
#cx,cy: coarse stage
@@ -391,7 +452,7 @@ class HelicalScan:
return h
def pltCrist(self,fy=0,cx=0,cz=0,w=0,h=None):
def pltCrist(self,cx=0,cz=0,w=0,fy=0,h=None):
#h are the handles
if h is None:
h=[] #handels
@@ -507,7 +568,7 @@ open forward
DZ=qCZ-p0_z
Y=qFY
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y
P1001+=1
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close
''')
@@ -544,6 +605,7 @@ open inverse
qCZ=DZ+p0_z
qFY=Y
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
P1002+=1
close
''')
@@ -556,159 +618,213 @@ close
fh.write('\n'.join(prg))
fh.close()
if host is not None:
cmd = '/home/zamofing_t/scripts/gpasciiCommander --host ' + host + ' ' + file
cmd = 'gpasciiCommander --host ' + host + ' ' + file
print(cmd)
p = sprc.Popen(cmd, shell=True) # , stdout=sprc.PIPE, stderr=sprc.STDOUT)
# res=p.stdout.readlines(); print res
retval = p.wait()
# gather -u /var/ftp/gather/out.txt
def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
'''
kwargs:
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
pt2pt_time : time to move from one point to the next point
'''
prg=[]
acq_per=kwargs.get('acq_per',1)
gather={"MaxSamples":1000000, "Period":acq_per}
#Sys.ServoPeriod is dependent of !common() macro
ServoPeriod= .2 #0.2ms
#ServoPeriod = .05
self.meta = {'timebase': ServoPeriod*gather['Period']}
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
channels=["Motor[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].ActPos"]
prg.append('Gather.Enable=0')
prg.append('Gather.Items=%d'%len(channels))
for k,v in gather.iteritems():
prg.append('Gather.%s=%d'%(k,v))
for i,c in enumerate(channels):
prg.append('Gather.Addr[%d]=%s.a'%(i,c))
def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
'''
kwargs:
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
pt2pt_time : time to move from one point to the next point
'''
prg=[]
acq_per=kwargs.get('acq_per',1)
gather={"MaxSamples":1000000, "Period":acq_per}
#Sys.ServoPeriod is dependent of !common() macro
ServoPeriod= .2 #0.2ms
#ServoPeriod = .05
self.meta = {'timebase': ServoPeriod*gather['Period']}
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
# CX CZ W FY
channels=["Motor[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].ActPos"]
prg.append('Gather.Enable=0')
prg.append('Gather.Items=%d'%len(channels))
for k,v in gather.iteritems():
prg.append('Gather.%s=%d'%(k,v))
for i,c in enumerate(channels):
prg.append('Gather.Addr[%d]=%s.a'%(i,c))
prg.append('open prog %d'%(prgId))
# this uses Coord[1].Tm and limits with MaxSpeed
if mode==-1: #### jog a 10mm square
pos=self.points
prg.append(' linear abs')
prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
prg.append('open prog %d'%(prgId))
prg.append(' P1000=0')
# this uses Coord[1].Tm and limits with MaxSpeed
if mode==-1: #### jog all motors 10000um (or 10000 mdeg)
pos=np.array([[0,0,0,0],])
prg.append(' linear abs')
#prg.append('X(%g) Z(%g) B(%g) Y(%g)' % tuple(pos[0, :]))
prg.append(' jog1,2,7,8=0')
prg.append(' dwell 10')
prg.append(' Gather.Enable=2')
prg.append(' jog7:10000')
prg.append(' dwell 100')
prg.append(' jog8:10000')
prg.append(' dwell 100')
prg.append(' jog1:10000')
prg.append(' dwell 100')
prg.append(' jog2:10000')
prg.append(' dwell 100')
prg.append(' jog1,2,7,8=0')
prg.append(' dwell 100')
prg.append(' Gather.Enable=0')
elif mode==0: #### linear motion
#y=2.3 6.2
#dx=0, dz=0
#w=0..3600000 # 10 rev
pos=np.array([[0, 0, 0, 2.300],
[0, 0, 360000, 6.200],])
cnt=13
pos=np.zeros((cnt,4))
pos[:,2]= np.linspace(0,3600000,cnt)
pos[:,3]= np.linspace(2.3,6.2,cnt)
#[0, 0, 3600000, 6.200],])
#prg.append('Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics
prg.append(' linear abs')
prg.append(' X(%g) Z(%g) B(%g) Y(%g)' % tuple(pos[0, :]))
prg.append(' dwell 100')
prg.append(' Gather.Enable=2')
for i in range(pos.shape[0]):
prg.append(' X(%g) Z(%g) B(%g) Y(%g)' % tuple(pos[i, :]))
prg.append(' dwell 100')
prg.append(' Gather.Enable=0')
elif mode==1: #### pvt motion
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.
try:
cnt=kwargs['cnt'] #move path multiple times
except KeyError:
cnt=1
try:
pt=self.ptsCorr
except AttributeError:
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')
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]):
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)]))
if cnt>1:
prg.append('dwell 10')
prg.append('Gather.Enable=2')
prg.append('jog2:10000')
prg.append('dwell 100')
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: #### linear motion
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: #### pvt motion
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.
try:
cnt=kwargs['cnt'] #move path multiple times
except KeyError:
cnt=1
try:
pt=self.ptsCorr
except AttributeError:
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('P100=P100-1')
prg.append('if(P100>0)')
prg.append('{')
prg.append(' linear abs')
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
prg.append('dwell 10')
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]):
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)]))
if cnt>1:
prg.append('dwell 10')
prg.append('P100=P100-1')
prg.append('if(P100>0)')
prg.append('{')
prg.append(' linear abs')
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
prg.append('dwell 100')
prg.append('goto 100')
prg.append('}')
else:
prg.append('dwell 1000')
prg.append('Gather.Enable=0')
elif mode==2: #### spline motion
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.
pos=self.points
pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
pcor[(0,-1),:]=pos[(0,-1),:]
pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
#pcor=pos
prg.append(' linear abs')
prg.append('X(%g) Y(%g)' % tuple(pcor[0, :]))
prg.append('dwell 10')
prg.append('Gather.Enable=2')
prg.append(' spline%g abs'%pt2pt_time) #100ms to next position
for idx in range(pcor.shape[0]):
prg.append('X%g Y%g'%tuple(pcor[idx,:]))
prg.append('dwell 100')
prg.append('Gather.Enable=0')
prg.append('goto 100')
prg.append('}')
else:
prg.append('dwell 1000')
prg.append('Gather.Enable=0')
elif mode==2: #### spline motion
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.
pos=self.points
pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
pcor[(0,-1),:]=pos[(0,-1),:]
pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
#pcor=pos
prg.append(' linear abs')
prg.append('X(%g) Y(%g)' % tuple(pcor[0, :]))
prg.append('dwell 10')
prg.append('Gather.Enable=2')
prg.append(' spline%g abs'%pt2pt_time) #100ms to next position
for idx in range(pcor.shape[0]):
prg.append('X%g Y%g'%tuple(pcor[idx,:]))
prg.append('dwell 100')
prg.append('Gather.Enable=0')
prg.append('close')
prg.append('&1\nb%dr\n'%prgId)
if self.args.verbose & 4:
for ln in prg:
print(ln)
prg.append(' P1000=1')
prg.append('close')
prg.append('&1\nb%dr\n'%prgId)
if self.args.verbose & 4:
for ln in prg:
print(ln)
if file is not None:
fh=open(file,'w')
fh.write('\n'.join(prg))
fh.close()
if host is not None:
cmd ='gpasciiCommander --host '+host+' '+ file
print(cmd)
p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
#res=p.stdout.readlines(); print res
retval = p.wait()
#gather -u /var/ftp/gather/out.txt
cmd ='PBGatherPlot -m24 -v7 --host '+host
print(cmd)
p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
retval = p.wait()
self.prg=prg
if file is not None:
fh=open(file,'w')
fh.write('\n'.join(prg))
fh.close()
if host is not None:
# ***download and start the program***
cmd ='gpasciiCommander --host '+host+' '+ file
print(cmd)
p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
#res=p.stdout.readlines(); print res
retval = p.wait()
# ***wait program finished P1000=1***
com=GpasciiCommunicator().connect(host,prompt='# ')
ack=GpasciiCommunicator.gpascii_ack
sys.stdout.write('wait execution...');sys.stdout.flush()
while(True):
com.write('P1000\n')
val=com.read_until(ack)
val=int(val[val.find('=')+1:].rstrip(ack))
if val==1:break
time.sleep(.2)
sys.stdout.write('.');sys.stdout.flush()
fnRmt = '/var/ftp/gather/out.txt'
fnLoc = '/tmp/gather.txt'
print('\ngather data to %s...' % fnRmt)
p = sprc.Popen(('ssh', 'root@' + host, 'gather ', '-u', fnRmt), shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE,
stderr=sprc.PIPE)
res = p.wait()
print('transfer data to %s...' % fnLoc)
p = sprc.Popen(('scp', 'root@' + host + ':' + fnRmt, fnLoc), shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE,
stderr=sprc.PIPE)
res = p.wait()
class GpasciiCommunicator():
'''Communicates with the Delta Tau gpascii programm
'''
gpascii_ack="\x06\r\n"
gpascii_inp='Input\r\n'
def connect(self, host, username='root', password='deltatau',prompt='ppmac# ',verbose=0):
p=telnetlib.Telnet(host)
s=p.read_until('login: ')
if verbose: print(s)
p.write(username+'\n')
s =p.read_until('Password: ')
if verbose: print(s)
p.write(password+'\n')
s =p.read_until(prompt) # command prompt
if verbose: print(s)
p.write('gpascii -2\n') # execute gpascii command
s=p.read_until(self.gpascii_inp)
if verbose: print(s)
return p
if __name__=='__main__':
@@ -747,11 +863,31 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
args.other=other
hs=HelicalScan(args)
hs.args.verbose = 255
#hs.sequencer()
hs.args.verbose=255;hs.calcParam();hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
hs.test_find_rot_ctr()
hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
return
hs.calcParam()
hs.test_coord_trf()
#hs.interactive_dx_dz_w_y()
#hs.interactive_cx_cz_w_fy()
#hs.interactive_dx_dz_w_y()
hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
#hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=-1)
hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0)
hs.interactive_anim()
return
hs.interactive_dx_dz_w_y()
hs.interactive_cx_cz_w_fy()
hs.test_coord_trf()
return
hs.test_find_rot_ctr()
hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
hs.interactive_cx_cz_w_fy()