#!/usr/bin/env python # *-----------------------------------------------------------------------* # | | # | Copyright (c) 2017 by Paul Scherrer Institute (http://www.psi.ch) | # | | # | Author Thierry Zamofing (thierry.zamofing@psi.ch) | # *-----------------------------------------------------------------------* ''' tools to setup and execute a helical scan of a cristal motors CX CZ RY FY 7 8 1 2 Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step Mot 2: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step Mot 3: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step Mot/Enc 4: camera base plate X Mot/Enc 5: camera base plate Y Mot 6: Backlight 2.3A Mot 7: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) Mot 8: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) verbose bits: #1 basic info #2 plot sorting steps 4 list program #4 upload progress #8 plot gather path ''' import os, sys, json import numpy as np import matplotlib as mpl import matplotlib.pyplot as plt import mpl_toolkits.mplot3d as plt3d import matplotlib.animation as anim from matplotlib.widgets import Slider import subprocess as sprc from utilities import * d2r=2*np.pi/360 #ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') #plot coordinates: X Y Z #data Z X Y class Trf: #https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector @staticmethod def rotZ(rad): """ Return matrix for rotating about the z-axis by 'radians' radians """ c = np.cos(rad) s = np.sin(rad) m=np.identity(4) m[0:2,0:2]=((c, -s),(s, c)) return m @staticmethod def rotY(rad): """ Return matrix for rotating about the z-axis by 'radians' radians """ c = np.cos(rad) s = np.sin(rad) m=np.identity(4) m[np.ix_((0,2),(0,2))]=((c, s),(-s, c)) return m @staticmethod def trans(x,y,z): m=np.identity(4) m[0:3, 3] =(x,y,z) return m class HelicalScan: def __init__(self,args): if args.cfg: fh=open(args.cfg,'r') s=fh.read() cfg=json.loads(s, object_hook=ConvUtf8) s=json.dumps(cfg, indent=2, separators=(',', ': '));print(s) else: fn='/tmp/shapepath4' #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_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['test_find_rot_ctr()']} #cfg = {"sequencer": ['test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)']} cfg = {"sequencer": ['test_coord_trf()']} self.cfg=dotdict(cfg) self.args=args def sequencer(self): print('args='+str(self.args)) print('cfg='+str(self.cfg)) #try: # self.points=np.array(self.cfg.points) #except AttributeError: # pass try: sequencer= self.cfg.pop('sequencer') except KeyError: print('no command sequence to execute') else: dryrun=self.args.dryrun for cmd in sequencer: print('>'*5+' '+cmd+' '+'<'*5) if not dryrun: eval('self.' + cmd) @staticmethod def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37): # find the rotation center, amplitude out of n (niminum 3) measurements # n number of equidistant measurements # per number of periods (full rotation of all measurements nut be a interger value for precise measurements) # phi phase # bias bias value # ampl amplitude t = np.arange(n) w=2*np.pi*per/n*t y=ampl*np.cos(w+phi*d2r)+bias plt.figure(1) plt.subplot(311) plt.plot(t,y,'b.-') plt.subplot(312) f = np.fft.fft(y) plt.step(t, f.real,'b.-', t, f.imag,'r.-', where='mid') (bias,ampl,phase)=HelicalScan.meas_rot_ctr(y, per) print('bias: '+str(bias)) print('amplitude: '+str(ampl)) print('phase: '+str(phase*360./2/np.pi)) plt.subplot(313) t2 = np.linspace(0,2*np.pi,64) y2=ampl*np.cos(t2+phase)+bias plt.plot(t2,y2,'g-') plt.stem(w,y,'b-') plt.show() pass def test_coord_trf(self): self.calcParam() param = self.param dx, dz, w, y, = (0.2,0.3,0.1,4.3) print 'input : dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y) (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y) print 'inv_trf: cx:%.3g cz:%.3g w:%.3g fy:%.3g' % (cx,cz,w/d2r,fy) (dx, dz,w,y) = self.fwd_transform(cx,cz,w,fy) print 'fwd_trf: dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y) # plt.ion() # fig = plt.figure() # a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(horig,pt),interval=20,blit=False) # plt.show() # def my_anim_func3(self,idx): # self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist) def interactive_cx_cz_w_fy(self): fig = plt.figure() self.manip=False#True#False 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] self.axSetCenter(ctr,10) axCx=plt.axes([0.1, 0.01, 0.8, 0.02]) axCz=plt.axes([0.1, 0.04, 0.8, 0.02]) axW =plt.axes([0.1, 0.07, 0.8, 0.02]) axFy=plt.axes([0.1, 0.10, 0.8, 0.02]) if self.manip: lz=ax.get_xlim() lx=ax.get_ylim() ly=ax.get_zlim() else: lx = ly=lz=[-5,5] self.sldCx=sCx=Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.) self.sldCz=sCz=Slider(axCz, 'cz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.) self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0) self.sldFy=sFy=Slider(axFy, 'fy', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.) sCx.on_changed(self.update_cx_cz_w_fy) sCz.on_changed(self.update_cx_cz_w_fy) sW.on_changed(self.update_cx_cz_w_fy) sFy.on_changed(self.update_cx_cz_w_fy) hCrist,pt=self.pltCrist() self.hCrist=hCrist;self.fig=fig # 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) print p ofs=(p[1]+p[0])/2. # = center of the cristal m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m) plt.show() def update_cx_cz_w_fy(self,val): cx = self.sldCx.val cz = self.sldCz.val w = self.sldW.val fy = self.sldFy.val if self.manip: param = self.param # 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.cos(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.sin(phi_i) # z= z_i+r_i*sin(phi_i*w) print p ofs = (p[1] + p[0]) / 2. # = center of the cristal m = Trf.trans(cx,fy,cz) 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) #l.set_ydata(amp * np.sin(2 * np.pi * freq * t)) self.fig.canvas.draw_idle() def interactive_dx_dz_w_y(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) self.calcParam() param=self.param # param[i]=(z_i, y_i, x_i, r_i,phi_i) ctr=(0,0,0) self.axSetCenter(ctr,10) axDx=plt.axes([0.1, 0.01, 0.8, 0.02]) axDz=plt.axes([0.1, 0.04, 0.8, 0.02]) axW =plt.axes([0.1, 0.07, 0.8, 0.02]) axY =plt.axes([0.1, 0.10, 0.8, 0.02]) lx=[-1,1];ly=[0,1];lz=[-1,1] ly = param[:,1] self.sldDx=sDx=Slider(axDx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.) self.sldDz=sDz=Slider(axDz, 'dz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.) self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0) self.sldY =sY =Slider(axY, 'y', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.) sDx.on_changed(self.update_dx_dz_w_y) sDz.on_changed(self.update_dx_dz_w_y) sW.on_changed(self.update_dx_dz_w_y) sY.on_changed(self.update_dx_dz_w_y) # 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 plt.show() def update_dx_dz_w_y(self,val): dx = self.sldDx.val dz = self.sldDz.val w = self.sldW.val y = self.sldY.val 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.fig.canvas.draw_idle() def fwd_transform(self,cx,cz,w,fy): #cx,cy: coarse stage #input: cx,cz,w,fy #output: dx,dz,w,y # param[i]=(z_i, y_i, x_i, r_i,phi_i) param=self.param 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+w) # 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+w) # z= z_i+r_i*sin(phi_i*w) v=p[1]-p[0] #for y = 0..1: #v=v*y #for y = y_0..y_1: y_0=param[0,1];y_1=param[1,1];v=v*(fy-y_0)/(y_1-y_0) v=v+p[0] dx=cx-v[0] dz=cz-v[2] y=v[1] res=(dx,dz,w,y) return res def inv_transform(self,dx,dz,w,y): #input: dx,dz,w,y #output: cx,cz,w,fy #dx,dy: deviation from cristal center line param=self.param 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+w) # 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+w) # z= z_i+r_i*sin(phi_i*w) v=p[1]-p[0] #for y = 0..1: #v=v*y #for y = y_0..y_1: y_0=param[0,1];y_1=param[1,1];v=v*(y-y_0)/(y_1-y_0) v=v+p[0] cx=dx+v[0] cz=dz+v[2] fy=v[1] res=(cx,cz,w,fy) return res def calcParam(self): n = 3.; per = 1.; w = 2 * np.pi * per / n * np.arange(n) p = ((2.3, .71, 4.12, 10.6 * d2r),(6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi) self.param = param = np.ndarray((len(p), 5)) z = 14.5 # fix z position for i in range(2): (y, bias, ampl, phi) = p[i] x = ampl * np.cos(w + phi) + bias print('yMeas_%d=' % i + str(y) + ' xMeas_%d=' % i + str(x)) # param[i]=(z_i, y_i, x_i, r_i,phi_i) param[i, 0] = z param[i, 1] = y param[i, 2:] = HelicalScan.meas_rot_ctr(x) # (bias,ampl,phase) (bias, ampl, phase) = param[i][2:] print param def pltOrig(self,m,h=None): ax=self.ax # m is a 4x4 matrix. the transformed matrix r=m[:3,0] #1st g=m[:3,1] #2nd b=m[:3,2] #3rd o=m[:3,3] #origin if h is None: hr=ax.plot((o[2],o[2]+r[2]), (o[0],o[0]+r[0]), (o[1],o[1]+r[1]), 'r') hg=ax.plot((o[2],o[2]+g[2]), (o[0],o[0]+g[0]), (o[1],o[1]+g[1]), 'g') hb=ax.plot((o[2],o[2]+b[2]), (o[0],o[0]+b[0]), (o[1],o[1]+b[1]), 'b') return hr + hg + hb # this is a list else: hr, hg, hb = h hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0])) hr.set_3d_properties((o[1], o[1] + r[1])) hg.set_data((o[2], o[2] + g[2]), (o[0], o[0] + g[0])) hg.set_3d_properties((o[1], o[1] + g[1])) hb.set_data((o[2], o[2] + b[2]), (o[0], o[0] + b[0])) hb.set_3d_properties((o[1], o[1] + b[1])) return h def pltCrist(self,fy=0,cx=0,cz=0,w=0,h=None): #h are the handles if h is None: h=[] #handels ax=self.ax param=self.param pt = np.ndarray((4, 3)) for i in range(2): (z, y, x, r, phi) = param[i] x+=cx;y+=fy;z+=cz;phi+=w pt[i] = (x, y, z) pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi)) obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3)) h1=ax.add_patch(obj) h2=plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z") h.append(obj) #hs=ax.scatter(pt[:, 2], pt[:, 0], pt[:, 1]) hs=ax.plot(pt[:, 2], pt[:, 0], pt[:, 1],'.') hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1]) h+=(hs[0],hp[0]) else: ax=self.ax param=self.param pt = np.ndarray((4, 3)) for i in range(2): (z, y, x, r, phi) = param[i] x+=cx;y+=fy;z+=cz;phi+=w pt[i] = (x, y, z) pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi)) h[i].remove() obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3)) ax.add_patch(obj) plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z") h[i]=obj h[2].set_data(pt[:, 2], pt[:, 0])#, pt[:, 1])) h[2].set_3d_properties(pt[:, 1]) h[3].set_data(pt[2:, 2], pt[2:, 0])#, pt[:, 1])) h[3].set_3d_properties(pt[2:, 1]) #hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0])) #hr.set_3d_properties((o[1], o[1] + r[1])) #h[3].set_3d_properties(zs=pt[:, 1])) #h[4].set_data((pt[2:, 2], pt[2:, 0]))#, pt[2:, 1])) #hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1], label='zs=0, zdir=z') #h+=(hs,hp[0]) return (h,pt) @staticmethod def meas_rot_ctr(y,per=1): # find the amplitude bias and phase of an equidistant sampled sinus # it needs at least 3 measurements e.g. at 0,120 240 deg or 0 90 180 270 deg # per is the number of persiods, default is 1 period =360 deg n=len(y) f = np.fft.fft(y) idx=int(per) bias=np.absolute(f[0]/n) phase=np.angle(f[idx]) ampl=np.absolute(f[idx])*2/n return (bias,ampl,phase) def axSetCenter(self,v,l): ax=self.ax #v=center vector, l= length of each axis l2=l/2. ax.set_xlim(v[2]-l2, v[2]+l2); ax.set_ylim(v[0]-l2, v[0]+l2); ax.set_zlim(v[1]-l2, v[1]+l2) def gen_coord_trf_code(self,file=None,host=None): param=self.param prg = [] prg.append(''' // Set the motors as inverse kinematic axes in CS 1 //motors CX CZ RY FY // 7 8 1 2 &1 #7->I #8->I #1->I #2->I open forward define(qCX='L7', qCZ='L8', qW='L1', qFY='L2') define(DX='C6', DZ='C8', W='C1', Y='C7') //coord X Z B Y define(p0_x='L10', p0_y='L11', p0_z='L12') define(p1_x='L13', p1_y='L14', p1_z='L15') define(f='L16') send 1"forward kinematic\\n"''') for i in range(2): #https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i] prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)"%tuple(l)) prg.append(''' p0_x=x_0+r_0*sin(phi_0+W) p1_x=x_1+r_1*sin(phi_1+W) p0_y=y_0 p1_y=y_1 p0_z=z_0+r_0*cos(phi_0+W) p1_z=z_1+r_1*cos(phi_0+W) f=(qFY-y_0)/(y_1-y_0) p0_x=p0_x+f*(p1_x-p0_x) p0_y=p0_y+f*(p1_y-p0_y) p0_z=p0_z+f*(p1_z-p0_z) DX=qCX-p0_x DZ=qCZ-p0_z Y=qFY W=qW D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 close ''') prg.append(''' open inverse define(DX='C6', DZ='C8', W='C1', Y='C7') //coord X Z B Y //D0 is set to $000001c2 define(qCX='L7', qCZ='L8', qW='L1', qFY='L2') define(p0_x='L10', p0_y='L11', p0_z='L12') define(p1_x='L13', p1_y='L14', p1_z='L15') define(f='L16') send 1"inverse kinematic\\n"''') for i in range(2): # https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i] prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)" % tuple(l)) prg.append(''' p0_x=x_0+r_0*sin(phi_0+W) p1_x=x_1+r_1*sin(phi_1+W) p0_y=y_0 p1_y=y_1 p0_z=z_0+r_0*cos(phi_0+W) p1_z=z_1+r_1*cos(phi_0+W) f=(qFY-y_0)/(y_1-y_0) p0_x=p0_x+f*(p1_x-p0_x) p0_y=p0_y+f*(p1_y-p0_y) p0_z=p0_z+f*(p1_z-p0_z) qCX=DX+p0_x qCZ=DZ+p0_z qFY=Y qW=W close ''') 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 = '/home/zamofing_t/scripts/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)) 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('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(' 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('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 __name__=='__main__': from optparse import OptionParser, IndentedHelpFormatter class MyFormatter(IndentedHelpFormatter): 'helper class for formating the OptionParser' def __init__(self): IndentedHelpFormatter.__init__(self) def format_epilog(self, epilog): if epilog: return epilog else: return "" def parse_args(): 'main command line interpreter function' #usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg (h, t)=os.path.split(sys.argv[0]);cmd='\n '+(t if len(h)>3 else sys.argv[0])+' ' exampleCmd=('-n', '-v15' ) epilog=__doc__+''' Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' fmt=MyFormatter() parser=OptionParser(epilog=epilog, formatter=fmt) parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0) parser.add_option('-n', '--dryrun', action='store_true', help='dryrun to stdout') parser.add_option('--xy', action='store_true', help='sort x,y instead y,x') parser.add_option('--cfg', help='config file containing json configuration structure') (args, other)=parser.parse_args() args.other=other hs=HelicalScan(args) #hs.sequencer() hs.args.verbose=255;hs.calcParam();hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485') #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.test_coord_trf() hs.interactive_cx_cz_w_fy() hs.interactive_dx_dz_w_y() #------------------ Main Code ---------------------------------- #ssh_test() ret=parse_args() exit(ret)