diff --git a/SAR-EXPMX1.subs b/SAR-EXPMX1.subs index e012ba3..3e9095f 100644 --- a/SAR-EXPMX1.subs +++ b/SAR-EXPMX1.subs @@ -16,12 +16,12 @@ file asyn.template {{P="$(P_M)1", PORT=$(PORT_M)}} file PPMACMotor.template { -pattern{ DESC , P , M , PORT , ADDR, DIR, VELO, HVEL, ACCL, JAR, MRES , PREC, EGU , DHLM, DLLM} - { "Sample F-Trans Y", "$(P_M)", "MOT_FY" , "$(PORT_M)", 1 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } - { "Sample F-Trans X", "$(P_M)", "MOT_FX" , "$(PORT_M)", 2 , 0 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } - { "Rotation Y" , "$(P_M)", "MOT_ROT_Y", "$(PORT_M)", 3 , 1 , 50 , 50 , 0.1 , 20 , -0.001, 3 , "deg", 0 , 0 } - { "Sample C-Trans X", "$(P_M)", "MOT_CX" , "$(PORT_M)", 4 , 0 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } - { "Sample C-Trans Z", "$(P_M)", "MOT_CZ" , "$(PORT_M)", 5 , 0 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } - { "Interfero Y" , "$(P_M)", "ENC_FY" , "$(PORT_M)", 6 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } - { "Interfero X" , "$(P_M)", "ENC_FX" , "$(PORT_M)", 7 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } +pattern{ DESC , P , M , PORT , ADDR, DIR, VELO, HVEL, ACCL, JAR, MRES , PREC, EGU , DHLM, DLLM} + { "Sample F-Trans Y", "$(P_M)", "MOT_FY" , "$(PORT_M)", 1 , 0 , 2 , 2 , 0.1 , 20 , 0.001, 3 , "mm" , 0 , 0 } + { "Sample F-Trans X", "$(P_M)", "MOT_FX" , "$(PORT_M)", 2 , 1 , 2 , 2 , 0.1 , 20 , 0.001, 3 , "mm" , 0 , 0 } + { "Rotation Y" , "$(P_M)", "MOT_ROT_Y", "$(PORT_M)", 3 , 0 , 50 , 50 , 0.1 , 20 , 0.001, 3 , "deg", 0 , 0 } + { "Sample C-Trans X", "$(P_M)", "MOT_CX" , "$(PORT_M)", 4 , 1 , 2 , 2 , 0.1 , 20 , 0.001, 3 , "mm" , 0 , 0 } + { "Sample C-Trans Z", "$(P_M)", "MOT_CZ" , "$(PORT_M)", 5 , 1 , 2 , 2 , 0.1 , 20 , 0.001, 3 , "mm" , 0 , 0 } + { "Interfero Y" , "$(P_M)", "ENC_FY" , "$(PORT_M)", 6 , 0 , 2 , 2 , 0.1 , 20 , 0.001, 3 , "mm" , 0 , 0 } + { "Interfero X" , "$(P_M)", "ENC_FX" , "$(PORT_M)", 7 , 0 , 2 , 2 , 0.1 , 20 , 0.001, 3 , "mm" , 0 , 0 } } diff --git a/python/MXMotion.py b/python/MXMotion.py index 210c996..0ebfbd7 100644 --- a/python/MXMotion.py +++ b/python/MXMotion.py @@ -44,7 +44,6 @@ class MotionBase: sync_wait can be put in the program to force a timing sync sync_run are the commands to run the whole program ''' - gpascii = self.comm.gpascii if mode == 0: try: del self.sync_prg @@ -83,7 +82,10 @@ while(1) disable plc {plcId} close '''.format(plcId=plcId, crdId=crdId, flag=flag) - gpascii.send_block(prg) + comm=self.comm + if comm is not None: + gpascii=comm.gpascii + gpascii.send_block(prg) self.sync_prg = 'Coord[{crdId}].DesTimeBase=0'.format(prgId=prgId, plcId=plcId, crdId=crdId) self.sync_run = 'enable plc {plcId};&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId) diff --git a/python/helicalscan.py b/python/helicalscan.py index a350c28..8cce73e 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -20,23 +20,42 @@ Mot 5: Stage Z Stada Stepper 670mA 200 poles 1 rev = 100*2 Enc 6: Interferometer Y Enc 7: Interferometer X - - - - - - verbose bits: #1 basic info #2 plot sorting steps 4 list program #4 upload progress #8 plot gather path - - - ''' +#motors CX CZ RY FY +# 4 5 3 1 +#Dir. - - + + + +#There are 2 coordinate systems +#rhs =right hand side coord: system osed in the python code +#pb = power brick coord: cx,cz have inverted directions + +#the structures have following conventions: + +# param -> pb coord +# rec -> pb coord +# code -> pb coord +# points -> pb coord + +#ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') +#plot coordinates: X Y Z +#data Z X Y + +#param: +#(z_i, y_i, x_i, r_i,phi_i) -> pb coord. be aware of inverted z_i,x_i signs + +#rec: +# cx,cz,w,fy -> pb coord. be aware of inverted cx,cz signs + +#points: +#dx,dz,w,fy -> pb coord. be aware of inverted dx,dz signs + import os, sys, json,re import numpy as np import matplotlib as mpl @@ -56,10 +75,7 @@ from pbtools.misc.gather import Gather from MXMotion import MotionBase 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 +np.set_printoptions(formatter={'float':lambda x:"{:12.6g}".format(x)}) class Trf: #https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector @@ -184,17 +200,17 @@ class HelicalScanGui(): if manip: y0=param[:, 1].mean() ly=sorted(param[:,1]) - x0=param[:, 2].mean() + x0=-param[:, 2].mean() lx=(-l/2+x0,l/2+x0) - z0=param[:, 0].mean() + z0=-param[:, 0].mean() lz=(-l/2+z0,l/2+z0) - self.axSetCenter((x0,y0,z0),l) + self.axSetCenter((-x0,-y0,-z0),l) else: self.axSetCenter((0, 0, 0), l) ly=sorted(param[:,1]) - x0=param[:, 2].mean() + x0=-param[:, 2].mean() lx=(-l/2+x0,l/2+x0) - z0=param[:, 0].mean() + z0=-param[:, 0].mean() lz=(-l/2+z0,l/2+z0) self.sldCx=sCx=Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.) @@ -225,15 +241,14 @@ class HelicalScanGui(): # param[i]=(z_i, y_i, x_i, r_i,phi_i) #rx=param[:,2].mean();rz=param[:,0].mean() #precise caldulation of rotation center dependent of y - (z0, y0, x0, r0, phi0)=param[0] - (z1, y1, x1, r1, phi1)=param[1] + (z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0 + (z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1 rx = x0+(x1-x0)/(y1-y0)*(fy-y0) rz = z0+(z1-z0)/(y1-y0)*(fy-y0) - m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz))) + m=Trf.trans(-rx,-fy,-rz).dot(Trf.rotY(-w).dot(Trf.trans(rx-cx,0,rz-cz))) self.pltOrig(m) else: - self.pltCrist(-cx, -cz, w, -fy) - #self.pltCrist(cx,cz,w*d2r,fy) + self.pltCrist(cx, cz, w, fy) self.fig.canvas.draw_idle() def interactive_dx_dz_w_y(self,manip=False): @@ -255,17 +270,15 @@ class HelicalScanGui(): if manip: y0=param[:, 1].mean() ly=sorted(param[:,1]) - x0=param[:, 2].mean() + x0=-param[:, 2].mean() lx=(-l/2,l/2) - z0=param[:, 0].mean() + z0=-param[:, 0].mean() lz=(-l/2,l/2) - self.axSetCenter((x0,y0,z0),l) + self.axSetCenter((-x0,-y0,-z0),l) else: self.axSetCenter((0, 0, 0), l) ly=sorted(param[:,1]) - x0=param[:, 2].mean() lx=(-l/2,l/2) - z0=param[:, 0].mean() lz=(-l/2,l/2) self.sldDx=sDx=Slider(axDx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.) @@ -296,14 +309,14 @@ class HelicalScanGui(): param = self.helScn.param # param[i]=(z_i, y_i, x_i, r_i,phi_i) #precise caldulation of rotation center dependent of y - (z0, y0, x0, r0, phi0)=param[0] - (z1, y1, x1, r1, phi1)=param[1] + (z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0 + (z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1 rx = x0+(x1-x0)/(y1-y0)*(fy-y0) rz = z0+(z1-z0)/(y1-y0)*(fy-y0) - m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz))) + m=Trf.trans(-rx,-fy,-rz).dot(Trf.rotY(-w).dot(Trf.trans(rx-cx,0,rz-cz))) self.pltOrig(m) else: - self.pltCrist(-cx,-cz,w,-fy) + self.pltCrist(cx,cz,w,fy) self.fig.canvas.draw_idle() def interactive_anim(self,manip=False): @@ -321,10 +334,10 @@ class HelicalScanGui(): axFrm=plt.axes([0.1, 0.01, 0.8, 0.02]) if manip: - y0=param[:, 1].mean() - x0=param[:, 2].mean() - z0=param[:, 0].mean() - self.axSetCenter((x0,y0,z0),l) + y0= param[:, 1].mean() + x0=-param[:, 2].mean() + z0=-param[:, 0].mean() + self.axSetCenter((-x0,-y0,-z0),l) else: self.axSetCenter((0, 0, 0), l) @@ -341,27 +354,24 @@ class HelicalScanGui(): def update_anim(self,frm): rec=self.helScn.rec - (cx, cz, w, fy)=rec[int(frm),:4] - #data/=. #scale from um to mm + (cx, cz, w, fy)=rec[int(frm),:4];cx=-cx;cz=-cz w*=d2r/1000 # scale from deg to rad if self.manip: param = self.helScn.param # param[i]=(z_i, y_i, x_i, r_i,phi_i) - #rx=param[:,2].mean();rz=param[:,0].mean() - #precise caldulation of rotation center dependent of y - (z0, y0, x0, r0, phi0)=param[0] - (z1, y1, x1, r1, phi1)=param[1] + (z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0 + (z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1 rx = x0+(x1-x0)/(y1-y0)*(fy-y0) rz = z0+(z1-z0)/(y1-y0)*(fy-y0) - m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz))) + m=Trf.trans(-rx,-fy,-rz).dot(Trf.rotY(-w).dot(Trf.trans(rx-cx,0,rz-cz))) self.pltOrig(m) else: - self.pltCrist(-cx,-cz,w,-fy) + self.pltCrist(cx,cz,w,fy) self.fig.canvas.draw_idle() def anim_gather_data(self,idx): rec=self.helScn.rec - (cx, cz, w, fy)=rec[int(idx*self.step),:] + (cx, cz, w, fy)=rec[int(idx*self.step),:];cx=-cx;cz=-cz 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),:] @@ -406,8 +416,8 @@ class HelicalScanGui(): except AttributeError: h=[] #handels for i in range(2): - (z, y, x, r, phi) = param[i] - x+=cx;y+=fy;z+=cz;phi+=w + (z, y, x, r, phi) = param[i];z=-z;x=-x + x=cx-x;y=fy-y;z=cz-z;phi=phi+w pt[i] = (z, x, y) pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y) obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('y' if i==0 else 'r', alpha=0.2)) @@ -420,7 +430,7 @@ class HelicalScanGui(): hp=ax.plot(pt[2:, 0], pt[2:, 1], pt[2:, 2]) h+=(hs[0],hp[0]) if hasattr(helScn,'points'): - lines = self.get_meas_lines(pt,cx,cz,fy,w) + lines = self.get_meas_lines(pt,w) p=tuple(lines[:,0,:].T) hl =plt3d.art3d.Line3D(*p,color='r',marker='.')#, *args[argi:], **kwargs) ax.add_artist(hl);h.append(hl) @@ -432,8 +442,8 @@ class HelicalScanGui(): self.hCrist=h else: for i in range(2): - (z, y, x, r, phi) = param[i] - x+=cx;y+=fy;z+=cz;phi+=w + (z, y, x, r, phi) = param[i];z=-z;x=-x + x=cx-x;y=fy-y;z=cz-z;phi=phi+w pt[i] = (z, x, y) pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y) h[i].remove() @@ -447,7 +457,7 @@ class HelicalScanGui(): h[3].set_data(pt[2:, 0], pt[2:, 1])#, pt[:, 1])) h[3].set_3d_properties(pt[2:, 2]) if hasattr(helScn,'points'): - lines = self.get_meas_lines(pt,cx,cz,fy,w) + lines = self.get_meas_lines(pt,w) hl=h[4] #hl.set_data(lines[:,0,0], lines[:,0,1]) # , pt[:, 1])) #hl.set_3d_properties(lines[:,0,2]) @@ -459,7 +469,11 @@ class HelicalScanGui(): return (h,pt) - def get_meas_lines(self,pt,cx,cz,fy,w,arwLen=None): + def get_meas_lines(self,pt,w,arwLen=None): + #pt are the rot-centers,starting,ending points of the crystal + #w is the rotation angle + #the measurement points are used from self.helScn.points + #params is used from self.helScn.param if arwLen is None: arwLen=self.scale*.05 # default arrow length=0.01 of crystal size param = self.helScn.param @@ -468,16 +482,13 @@ class HelicalScanGui(): dz_ = pts[:, 1] # add 0.2 to test w_ = pts[:, 2] * (d2r / 1000.) y_ = pts[:, 3] - - # self.inv_transform(self, dx, dz, w, y): - - f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1]) + f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1]) #part of y move: 0..1 lines = np.ndarray((pts.shape[0], 2, 3)) ofx=dx_*-np.sin(w-w_)+dz_*np.cos(w-w_) # 0.2=dx ofy=dx_*np.cos(w-w_)+dz_*np.sin(w-w_) lines[:, 0, 0] = pt[2, 0] + (pt[3, 0] - pt[2, 0]) * f +ofx # z data lines[:, 0, 1] = pt[2, 1] + (pt[3, 1] - pt[2, 1]) * f +ofy # x data - lines[:, 0, 2] = pts[:, 3] + fy # y data + lines[:, 0, 2] = param[0, 1]+pt[0, 2] -pts[:, 3] # y data lines[:, 1, 0] = lines[:, 0, 0] + np.cos(w-w_)*arwLen lines[:, 1, 1] = lines[:, 0, 1] + np.sin(w-w_)*arwLen lines[:, 1, 2] = lines[:, 0, 2] @@ -494,18 +505,17 @@ class HelicalScanGui(): class HelicalScanTests(): @staticmethod - def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37): + def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37*d2r): # 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 + # phi phase (in rad) # 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) + y=bias+ampl*np.sin(w+phi) + plt.figure('test_find_rot_ctr') plt.subplot(311) plt.plot(t,y,'b.-') @@ -513,19 +523,20 @@ class HelicalScanTests(): 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)) + inp=np.array((bias,ampl,phi)) + (bias_,ampl_,phi_)=out=HelicalScan.meas_rot_ctr(y, per) + print('input bias:{} amplitude:{} phase:{: .4f}'.format(bias,ampl,phi/d2r)) + print('output bias:{} amplitude:{} phase:{: .4f}'.format(bias_,ampl_,phi_/d2r)) + + assert(np.abs((inp-out)).max()<1E-10) plt.subplot(313) t2 = np.linspace(0,2*np.pi,64) - y2=ampl*np.cos(t2+phase)+bias + y2=bias+ampl*np.sin(t2+phi_) plt.plot(t2,y2,'g-') plt.stem(w,y,'b-') plt.show() - pass @staticmethod def test_coord_trf(helScn): @@ -575,31 +586,54 @@ class HelicalScanTests(): pass @staticmethod - def calcParamSim(): + def calcParamSim(hs): #simulated test values n = 3.; per = 1.; w = 2 * np.pi * per / n * np.arange(n) - #(z_i, y_i, x_i, r_i, phi_i)=param[i] + # param[i]=(z_i, y_i, x_i, r_i,phi_i) #paramSim = ((14.5, 2.3, .71, 4.12, 10.6 * d2r),(14.5, 6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi) #paramSim = ((14.5, 2.3, -100., 10, 10. * d2r),(14.5, 6.2, 100., 10., -10. * d2r)) # (y, bias, ampl, phi) - paramSim = ((14.5, 2.3, -100., 10, 0. * d2r),(14.5, 6.2, 100., 10., 90. * d2r)) # (y, bias, ampl, phi) + #paramSim = ((14.5, 2.3, 102., 15, 0. * d2r),(14.8, 16.2, 103.4, 10., 90. * d2r)) # (y, bias, ampl, phi) + paramSim = ((-945., 730., -102., 150, 10. * d2r),(-958., 327., -108, 202., 90. * d2r)) # (y, bias, ampl, phi) paramSim=np.array(paramSim) - param = np.ndarray(paramSim.shape) - for i in range(param.shape[0]): - (z,y,bias,ampl,phi) = paramSim[i] - measX = ampl * np.sin(w + phi) + bias - measZ = ampl * np.cos(w + phi) + bias - print('xMeas_%d=' % i + str(measX) + ' zMeas_%d=' % i + str(measZ)) - # 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(measX) # (bias,ampl,phase) - (bias, ampl, phase) = param[i][2:] - print(param) + measX=np.ndarray((paramSim.shape[0],w.size)) + measZ=np.ndarray((paramSim.shape[0],w.size)) + for i in range(paramSim.shape[0]): + (z_i,y_i,x_i,r_i,phi_i) = paramSim[i] + measX[i,:] = x_i+r_i*np.sin(w+phi_i) + measZ[i,:] = z_i+r_i*np.cos(w+phi_i) + if i==0: + print('measX='+str(measX[i,:])+' measZ='+str(measZ[i,:])) + else: + print(' '+str(measX[i,:])+' '+str(measZ[i,:])) + measY=paramSim[:, 1] + print('measY='+str(measY)) + hs.calcParam(measX,measY,measZ) + print(hs.param) print(paramSim) - assert((paramSim-param).max()<1E-10) - return param + assert(abs(paramSim-hs.param).max()<1E-10) + + inp=np.array(((0,0,0, measY[0]), + (0,0,np.pi*2/3, measY[0]), + (0,0,np.pi*4/3, measY[0]), + (0,0,0, measY[1]), + (0,0,np.pi*2/3, measY[1]), + (0,0,np.pi*4/3, measY[1]))) + + outCX=(measX[0,0],measX[0,1],measX[0,2],measX[1,0],measX[1,1],measX[1,2]) + outCZ=(measZ[0,0],measZ[0,1],measZ[0,2],measZ[1,0],measZ[1,1],measZ[1,2]) + + for i in range(inp.shape[0]): + (dx,dz,w_,y_) = inp[i,:] + print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w_/d2r*1000.,y_)) + (cx,cz,w,fy) = out = hs.inv_transform(dx,dz,w_,y_) + #assert(abs(cx-outCX[i])<1E-10) + #assert(abs(cz-outCZ[i])<1E-10) + print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)) + (dx,dz,w_,y_)=out= hs.fwd_transform(cx,cz,w,fy) + print('fwd_trf: dx:%.6g dz:%.6g w:%.6g y:%.6g' % (dx,dz,w_/d2r*1000.,y_)) + assert(np.abs(inp[i,:]-out).max()<1E-10) class HelicalScan(MotionBase): @@ -628,10 +662,10 @@ class HelicalScan(MotionBase): 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 + (z_i, y_i, x_i, r_i, phi_i)=param[i];z_i=-z_i;x_i=-x_i + p[i,0]=x_i-r_i*np.sin(w+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+w) # z= z_i+r_i*sin(phi_i*w) + p[i,2]=z_i-r_i*np.cos(w+phi_i) # z= z_i+r_i*sin(phi_i*w) v=p[1]-p[0] #for y = 0..1: #v=v*y @@ -652,10 +686,10 @@ class HelicalScan(MotionBase): 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 + (z_i, y_i, x_i, r_i, phi_i)=param[i];z_i=-z_i;x_i=-x_i + p[i,0]=x_i-r_i*np.sin(w+phi_i) 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) + p[i,2]=z_i-r_i*np.cos(w+phi_i) v=p[1]-p[0] #for y = 0..1: #v=v*y @@ -672,23 +706,11 @@ class HelicalScan(MotionBase): def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)), y=(575.,175.), z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))): -# #1,3,4,5p -# point 1 0,120,240 deg -# 575.5 0 -241.5 -1401.3 -# 575.5 120000 96.7 -1401.7 -# 575.5 240000 -53.8 -1802.4 -# -# point 2 0,120,240 deg -# 175.5 0 -162.3 -1802.5 -# 175.5 120000 -293.2 -1303.7 -# 175.5 240000 246.4 -1402.25 - #real measured values: #y : 2x1 array : y position were the measurements were taken #x : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1] #z : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1] # the z value is only used to find a rought bias of z - assert(len(y)==2) n = float(len(x[0])) #number of angles per = 1 #number of rotations @@ -700,35 +722,26 @@ class HelicalScan(MotionBase): param[i, 2:] = HelicalScan.meas_rot_ctr(x[i]) # (bias,ampl,phase) (bias, ampl, phase) = param[i][2:] - #check correctness of center: - w = 2 * np.pi * per / n * np.arange(n) - x_ = ampl * np.cos(w + phase) + bias - print(x_) - (dx,dz,w,y_) = (0,0,0,y[0]) - print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y_)) - (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y_) - print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)) - (dx, dz, w, y_) = (0,0,0,y[1]) - print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx, dz, w / d2r * 1000., y_)) - (cx, cz, w, fy) = self.inv_transform(dx, dz, w, y_) - print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx, cz, w / d2r * 1000., fy)) - - print(param) - + # check correctness of center: + w=2*np.pi*per/n*np.arange(n) + x_=bias+ampl*np.sin(w+phase) + #print(x_) + assert ((x[i]-x_).max()<1E-10) @staticmethod def meas_rot_ctr(y,per=1): - # find the amplitude bias and phase of an equidistant sampled sinus + # find the amplitude bias and phase of an equidistant sampled values + # the phase is amout the matching sine is shifted to the left (delayed) in radians # 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) + f = np.fft.rfft(y) idx=int(per) #bias=np.absolute(f[0]/n) assert(np.imag(f[0])==0.) bias=np.real(f[0]/n) #phase=np.angle(f[idx]) #f(w)=bias+ampl*cos(w+phase) - phase=np.angle(f[idx]*1j) #f(w)=bias+ampl*sin(w+phase) + phase=np.angle(f[idx]*1j) #f(w)=bias+ampl*sin(w+phase) ampl=np.absolute(f[idx])*2/n return (bias,ampl,phase) @@ -739,6 +752,7 @@ class HelicalScan(MotionBase): acq_per : acquire period: acquire data all acq_per servo loops (default=1) ''' comm=self.comm + if comm is None:return gt=self.gather gt.set_phasemode(False) #gt.set_address("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos") @@ -749,8 +763,6 @@ class HelicalScan(MotionBase): self.meta = {'timebase': ServoPeriod*acq_per} def setup_coord_trf(self,fnCrdTrf='/tmp/coordTrf.cfg'): - comm = self.comm - gpascii = comm.gpascii param=self.param prg = ''' // Set the motors as inverse kinematic axes in CS 1 @@ -768,13 +780,17 @@ a #3->I #1->I ''' + pbParam=param.copy() + pbParam[:,(0,2)]=-pbParam[:,(0,2)] #change sign of x_0,x_1,z_0,z_1, because axes have neg direction + sh=pbParam.shape s = ('z', 'y', 'x', 'r', 'phi') - a = np.ones(param.shape[1], dtype=np.uint8).reshape(1, -1) - b = np.arange(param.shape[0], dtype=np.uint8).reshape(1, -1) + a = np.ones(sh[1], dtype=np.uint8).reshape(1, -1) + b = np.arange(sh[0], dtype=np.uint8).reshape(1, -1) c = np.matmul(b.T, a).reshape(-1) - subsParam=dict(map(lambda k, i, v: (k + '_' + str(i), v), s * param.shape[0], c, param.reshape(-1))) + subsParam=dict(map(lambda k, i, v: (k + '_' + str(i), v), s * sh[0], c, pbParam.reshape(-1))) subsParam['d2r']=d2r/1000. subsParam['r2d']=1000./d2r + subsParam['cmt']='//' subs={'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1', @@ -787,7 +803,7 @@ a prg+=''' open forward - send 1"fwd_inp(%f) %f %f %f %f\\n",D0,{qCX},{qCZ},{qW},{qFY} + {cmt}send 1"fwd_inp(%f) %f %f %f %f\\n",D0,{qCX},{qCZ},{qW},{qFY} {W}={qW} {qW}={qW}*{d2r} //scale from 1000*deg to rad @@ -805,8 +821,7 @@ open forward {DX}={qCX}-{p0_x} {DZ}={qCZ}-{p0_z} {Y}={qFY} - //send 1"fwd_res %f %f %f %f\\n",{DX},{DZ},{W},{Y} - //P1001+=1 + {cmt}send 1"fwd_res %f %f %f %f\\n",{DX},{DZ},{W},{Y} D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 close '''.format(**subs) @@ -828,10 +843,10 @@ close prg+=''' open inverse - //if(D0>0) - // send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,{DX},{vDX},{DZ},{vDZ},{W},{vW},{Y},{vY} - //else - // send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y} + {cmt}if(D0>0) + {cmt} send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,{DX},{vDX},{DZ},{vDZ},{W},{vW},{Y},{vY} + {cmt}else + {cmt} send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y} {qW}={W} {W}={W}*{d2r} //scale from 1000*deg to rad @@ -851,20 +866,16 @@ open inverse {qFY}={Y} if(D0>0) {{ // calculate velocities for PVT motion - {vW}={vW}*{d2r} //scale from 1000*deg to rad {p_x}={sclY}*({p1_x}-{p0_x}) {p_z}={sclY}*({p1_z}-{p0_z}) {vqFY}={vY} - {vqCX}={vDX} + ({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY} //+ vqW*p_z - {vqCZ}={vDZ} + ({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY} //+ vqW*p_x - //vqW=vqW+(vqCX+(p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+(vqCZ+(p1_z-p0_z)/(p1_y-p0_y)*vY)*p_x - {vqW}={vW}//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x - {vqW}={vqW}*{r2d} //scale from rad to 1000*deg - // send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY} + {vqCX}={vDX}+({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY} + {vqCZ}={vDZ}+({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY} + {vqW}={vW} + {cmt}send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY} }} - //else - // send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY} - //P1002+=1 + {cmt}else + {cmt} send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY} close '''.format(**subs) @@ -873,19 +884,35 @@ close # (p1_x-p0_x)/(p1_y-p0_y)*vY velocity part in vqCX direction, when moving in vY # vqW*p_x velocity part of the rotation (vqW is in rad) - gpascii.send_block('disable plc 0') - time.sleep(.5) - gpascii.send_block(prg) +# {{ // calculate velocities for PVT motion +# {vW}={vW}*{d2r} //scale from 1000*deg to rad +# {p_x}={sclY}*({p1_x}-{p0_x}) +# {p_z}={sclY}*({p1_z}-{p0_z}) +# {vqFY}={vY} +# {vqCX}={vDX} + ({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY} //+ vqW*p_z +# {vqCZ}={vDZ} + ({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY} //+ vqW*p_x +# //vqW=vqW+(vqCX+(p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+(vqCZ+(p1_z-p0_z)/(p1_y-p0_y)*vY)*p_x +# {vqW}={vW}//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x +# {vqW}={vqW}*{r2d} //scale from rad to 1000*deg +# // send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY} + + comm = self.comm + if comm is not None: + gpascii = comm.gpascii + gpascii.send_block('disable plc 0') + time.sleep(.5) + gpascii.send_block(prg) if self.verbose & 4: print(prg) if fnCrdTrf is not None : fh=open(fnCrdTrf,'w') fh.write(prg) fh.close() - time.sleep(.5) - gpascii.send_block('enable plc 0') - time.sleep(.5) - gpascii.send_block('#1..7j/') + if comm is not None: + time.sleep(.5) + gpascii.send_block('enable plc 0') + time.sleep(.5) + gpascii.send_block('#1..7j/') def setup_motion(self,prgId=2,fnPrg='/tmp/prg.cfg',mode=0,**kwargs): ''' @@ -908,8 +935,6 @@ close wRng : starting and ending angle yRng : starting and ending height ''' - comm = self.comm - gpascii = comm.gpascii param=self.param prg=[] prg.append('open prog %d'%(prgId)) @@ -1047,7 +1072,10 @@ close #prg.append('&1\nb%dr\n'%prgId) prg='\n'.join(prg) + '\n' - gpascii.send_block(prg) + comm = self.comm + if comm is not None: + gpascii = comm.gpascii + gpascii.send_block(prg) if self.verbose & 4: print(prg) if fnPrg is not None : @@ -1071,127 +1099,78 @@ close if __name__=='__main__': def run_test(args): - #args.host=None + test=args.test + args.host=None + if args.host is None: comm=gather=None else: comm = PPComm(host=args.host) gather = Gather(comm) gpascii = comm.gpascii - #HelicalScanTests.calcParamSim() hs=HelicalScan(comm, gather, args.verbose) - #hs.test_find_rot_ctr() - #hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6) - fn='/tmp/helicalscan' - #hs.load_rec(fn+'.npz') - #hs.param[:4]+=np.pi/2.#add 90 deg + if test==1: + ### test find rotation center ### + HelicalScanTests.test_find_rot_ctr(n=32. ,per=1.,bias=2.31,ampl=4.12,phi=0*d2r) + HelicalScanTests.test_find_rot_ctr(n=32. ,per=1.,bias=2.31,ampl=4.12,phi=30.*d2r) + HelicalScanTests.test_find_rot_ctr(n=3.,per=1.,bias=-4.1,ampl=2.4,phi=37*d2r) + HelicalScanTests.test_find_rot_ctr(n=5. ,per=1.,bias=7.25,ampl=6.51,phi=-124.6*d2r) + return - #hsg=HelicalScanGui(hs);hsg.interactive_anim() - #hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=True) - #while True:hsg.update_anim(0) - #hs.param = np.ndarray((2,5)) - #hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) - #hs.param[1]=(15,4,0,2,np.pi/4)#(z_i, y_i, x_i, r_i,phi_i) + elif test==2: + ### test param calculation and display the gui### + HelicalScanTests.calcParamSim(hs) + hs.setup_motion(mode=1,cntHor=5,cntVert=15,hRng=(-50,50),wRng=(0,120000),smt=0,pt2pt_time=200) - #hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy() - #hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=True) - #while True: hsg.update_cx_cz_w_fy() #for debug purpose + for manip in (False,True): + hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=manip) + hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=manip) + #hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=manip) + return + elif test==3: + ### load recorded data, check param calculation not changed and display in the gui### + fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helicalscan' + hs.load_rec(fn+'.npz') + paramRec=hs.param + fh=open("/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helical.cmd") + s=fh.read();s=s.replace('calcParam','hs.calcParam') + eval(s) + assert (abs(paramRec-hs.param).max()<1E-10) - #hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y() - #hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=True) - #while True: hsg.update_dx_dz_w_y() #for debug purpose - #return - - #TODO: FE Digitizers PBPS117 timing not working! - - #hs.calcParam() + for manip in (False,True): + hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=manip) + hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=manip) + hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=manip) + return #gpasci: #1,4,5p // y,-x ,-z # 0deg 256.7 -762.5 -396.4 #120deg 258.5 731.7 -1896.9 - #240deg 256.2 -1282.8 -2496.5 - # 0deg 586.5 -1023.4 -696.8 - # 120deg 574.8 619.8 -1797.5 - # 240deg 580.0 -900.0 -2496.3 - - #hs.calcParam(x = ((-762.5, 731.7, -1282.8), (-1023.4, 619.8, -900.0)), - # y = (258.5,580.0), - # z = (( -396.4,-1896.9,-2496.5),(-696.8,-1797.5,-2496.3))) - + #... + #&1p #cpx X0 Z0 B0 Y258 #cpx X0 Z0 B120000 Y258 - #cpx X0 Z0 B240000 Y258 - #cpx X0 Z0 B0 Y580 - #cpx X0 Z0 B120000 Y580 - #cpx X0 Z0 B240000 Y580 - - - # 0deg 1405.7 -1154.4 -1309.6 - # 120deg 1401.7 216.3 -1010.9 - # 240deg 1407.9 -250.7 -2410.3 - # 0deg 1053.9 -1330.2 -1219.4 - # 120deg 1019.2 340.9 -918.8 - # 240deg 984.0 -230.4 -2510.4 #hs.calcParam(x = ((-1154.4, 216.3, -250.7), ( -1330.2, 340.9, -230.4)), # y = (1405.7,1019.2), # z = ((-1309.6, -1010.9, -2410.3),( -1219.4, -918.8, -2510.4))) - #&1p - #cpx X0 Z0 B0 Y1405 - #cpx X0 Z0 B120000 Y1405 - #cpx X0 Z0 B240000 Y1405 - #cpx X0 Z0 B0 Y1019 - #cpx X0 Z0 B120000 Y1019 - #cpx X0 Z0 B240000 Y1019 - - -#1600.5 -1484.0 -1264.2 -#1599.0 234.6 -663.3 -#1599.9 28.5 -2264.7 -# 468.3 629.9 -2663.3 -# 472.2 25.3 -164.4 -# 470.9 -1841.3 -1965.0 - - #hs.calcParam(x = ((-1484.0, 234.6, 28.5), ( 629.9, 25.3,-1841.3)), - # y = (1600.,470.), - # z = ((-1264.2, -663.3,-2264.7),(-2663.3, -164.4,-1965.0))) - - - #cpx X0 Z0 B0 Y1405 - #cpx X0 Z0 B120000 Y1405 - #cpx X0 Z0 B240000 Y1405 - #cpx X0 Z0 B0 Y1019 - #cpx X0 Z0 B120000 Y1019 - #cpx X0 Z0 B240000 Y1019 - fh=open("/sf/bernina/config/swissmx/exchange/helical.cmd") - s=fh.read();s=s.replace('calcParam','hs.calcParam') - eval(s) - - #hs.calcParamSim() - #hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) - #hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) - #hs.param[0]=(-100, 100,0,50,0)#(z_i, y_i, x_i, r_i,phi_i) - #hs.param[1]=(-100,-100,0,70,0)#(z_i, y_i, x_i, r_i,phi_i) - - #hs.test_coord_trf() - #hs.interactive_cx_cz_w_fy() - #hs.interactive_dx_dz_w_y() - - #mode bits: - #0:1 config simulated motors - #1:2 config real motors - #2:4 config coord trf - + ### use simulation motors ### #os.chdir(os.path.join(os.path.dirname(__file__),'../cfg')) #'sim_8_motors.cfg' #['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25']) # raw_input('press return when homed') - hs.setup_coord_trf() + ### hs.calcParam() ### + fh=open("/sf/bernina/config/swissmx/exchange/helical.cmd") + #fh=open("/sf/bernina/config/swissmx/exchange/helical_neg.cmd") + s=fh.read();s=s.replace('calcParam','hs.calcParam') + eval(s) + + hs.setup_coord_trf() hs.setup_sync(mode=2) # None: no sync at all mode=1: sync on timing UserFlag hs.setup_gather() @@ -1218,13 +1197,11 @@ if __name__=='__main__': #hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=10,cntHor=3,hRng=(-30,30),wRng=(0,36000),yRng=(-50,-100)) #hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-100,50),wRng=(000,10000),smt=0) - hs.run() print('wait until gather finished:') + fn='/tmp/helicalscan' hs.gather_upload(fn+'.npz') - hs.load_rec(fn+'.npz') - hsg=HelicalScanGui(hs) hsg.show_pos() hsg.show_vel() @@ -1259,6 +1236,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=255) parser.add_option('--host', help='hostname', default='SAR-CPPM-EXPMX1') + parser.add_option('-t','--test', type="int", default=None) (args, other)=parser.parse_args() args.other=other diff --git a/python/helicalscan_.py b/python/helicalscan_.py new file mode 100755 index 0000000..94f2d2d --- /dev/null +++ b/python/helicalscan_.py @@ -0,0 +1,1286 @@ +#!/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 + +Gather motor order +"Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos +motors CX CZ RY FY + 4 5 3 1 +Mot 1: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step +Mot 2: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step +Mot 3: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step +Mot 4: Stage X Stada Stepper 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) +Mot 5: Stage Z Stada Stepper 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) +Enc 6: Interferometer Y +Enc 7: Interferometer X + + + + + + + +verbose bits: + #1 basic info + #2 plot sorting steps + 4 list program + #4 upload progress + #8 plot gather path + + + +''' + +import os, sys, json,re +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 * +import telnetlib + +sys.path.insert(0,os.path.expanduser('~/Documents/prj/SwissFEL/PBTools/')) +#sys.path.insert(0,'/sf/bernina/config/swissmx/zamofing_t') +#sys.path.insert(0,'/sf/bernina/config/swissmx/zamofing_t/pbtools/misc/') +from pbtools.misc.pp_comm import PPComm +from pbtools.misc.gather import Gather +from MXMotion import MotionBase + +d2r=2*np.pi/360 +np.set_printoptions(formatter={'float':lambda x:"{:12.6g}".format(x)}) + +#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 HelicalScanGui(): + def __init__(self,helicalScan): + self.helScn=helicalScan + + def show_vel(self): + rec=self.helScn.rec + fig = plt.figure() + #y = np.diff(rec[:, 2]) + y = np.diff(rec,axis=0) + mx=y.max(0) + mn=y.min(0) + scl=np.maximum(mx,-mn) + scl=np.where(scl==0, 1, scl) # replace 0 by 1 + y/=scl + x = np.arange(0, y.shape[0]); + x= x.reshape(-1,1).dot(np.ones((1,y.shape[1]))) + lbl=('cx','cz','w','fy') + #plt.plot(x, y,label=lbl) + #for i in range(y.shape[1]): + for i in range(len(lbl)): + plt.plot(x[:,i], y[:,i],label=lbl[i]) + + plt.legend() + + def show_pos(self): + rec=self.helScn.rec + y = rec + #plt.ion() + fig = plt.figure(figsize=(20,6)) + if y.shape[1]==4: + c='bgrc';lbl=('cx','cz','w','fy') + else: + c='bgrcm';lbl=('cx','cz','w','fy','sync') + dx=.25/len(lbl) + for i in range(rec.shape[1]): + if i==0: + ax = fig.add_axes([.2, .05, .75, .90]) + axl =[ax,] + else: + ax = fig.add_axes(axl[0].get_position(), sharex=ax) + ax.spines['top'].set_visible(False) + ax.spines['bottom'].set_visible(False) + ax.xaxis.set_visible(False) + ax.patch.set_visible(False) + ax.spines['left'].set_position(('axes', -dx*i)) + axl.append(ax) + ax.set_ylabel(lbl[i], color=c[i]) + ax.tick_params(axis='y', colors=c[i]) + + x = np.arange(0, y.shape[0]); + h=[] + for i in range(rec.shape[1]): + h+=axl[i].plot(x, y[:,i],c[i],label=lbl[i]) + plt.legend(handles=h) + cid = fig.canvas.mpl_connect('motion_notify_event', self.onmove) + fig.obj=self + fig.data=y + + @staticmethod + def onmove(event): + #print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%( + # event.button, event.x, event.y, event.xdata, event.ydata) + obj = event.canvas.figure.obj + data = event.canvas.figure.data + if(event.xdata): + idx=int(round(event.xdata)) + msg='%d: cx:%.3f cz:%.3f w:%.1f fy:%.3f'%((idx,)+tuple(data[idx,:4])) + #print msg + event.canvas.toolbar.set_message(msg) + + def interactive_cx_cz_w_fy(self,manip=False): + '''interactive shows a cristal with sliders for cx,cz,w,fy + self.helScn.param is used a cristal parameters + THE AXES ARE RELABELED ! + The generated members are: + self.fig : handle to figure + self.hCrist : handle to cristal object + self.hOrig : handle to xyz-origin cross (xyz-length= 1/5 height of cristal + self.manip : True: moves the origin cross, False moves the Cristal + ''' + param=self.helScn.param + self.fig=fig=plt.figure();fig.canvas.set_window_title('interactive_cx_cz_w_fy'+(' manip' if manip else '')) + self.manip=manip + 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=10, azim=-170) + # param[i]=(z_i, y_i, x_i, r_i,phi_i) + l=max(2*param[:,3].max(),param[:,1].ptp()) #max of diameter and y peaktopeak + self.scale=l #scale is the length of a cube were the pltCrist object fits into + + 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 manip: + y0=param[:, 1].mean() + ly=sorted(param[:,1]) + x0=param[:, 2].mean() + lx=(-l/2+x0,l/2+x0) + z0=param[:, 0].mean() + lz=(-l/2+z0,l/2+z0) + self.axSetCenter((x0,y0,z0),l) + else: + self.axSetCenter((0, 0, 0), l) + ly=sorted(param[:,1]) + x0=param[:, 2].mean() + lx=(-l/2+x0,l/2+x0) + z0=param[:, 0].mean() + lz=(-l/2+z0,l/2+z0) + + 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) + + if manip: + #self.pltCrist(-x0, -z0, 0, -param[:,1].mean()) + self.pltCrist(0,0,0,0) + else: + self.pltOrig(Trf.trans(0, 0, 0)) + self.update_cx_cz_w_fy() + plt.show() + + def update_cx_cz_w_fy(self,val=None): + cx = self.sldCx.val + cz = self.sldCz.val + w = self.sldW.val + fy = self.sldFy.val + w=w*d2r + if self.manip: + param = self.helScn.param + # param[i]=(z_i, y_i, x_i, r_i,phi_i) + #rx=param[:,2].mean();rz=param[:,0].mean() + #precise caldulation of rotation center dependent of y + (z0, y0, x0, r0, phi0)=param[0] + (z1, y1, x1, r1, phi1)=param[1] + rx = x0+(x1-x0)/(y1-y0)*(fy-y0) + rz = z0+(z1-z0)/(y1-y0)*(fy-y0) + m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz))) + self.pltOrig(m) + else: + self.pltCrist(-cx, -cz, w, -fy) + #self.pltCrist(cx,cz,w*d2r,fy) + self.fig.canvas.draw_idle() + + def interactive_dx_dz_w_y(self,manip=False): + param=self.helScn.param + self.fig=fig=plt.figure();fig.canvas.set_window_title('interactive_dx_dz_w_y'+(' manip' if manip else '')) + self.manip=manip + 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=10, azim=-170) + + # param[i]=(z_i, y_i, x_i, r_i,phi_i) + l=max(2*param[:,3].max(),param[:,1].ptp()) #max of diameter and y peaktopeak + self.scale=l #scale is the length of a cube were the pltCrist object fits into + + 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]) + if manip: + y0=param[:, 1].mean() + ly=sorted(param[:,1]) + x0=param[:, 2].mean() + lx=(-l/2,l/2) + z0=param[:, 0].mean() + lz=(-l/2,l/2) + self.axSetCenter((x0,y0,z0),l) + else: + self.axSetCenter((0, 0, 0), l) + ly=sorted(param[:,1]) + x0=param[:, 2].mean() + lx=(-l/2,l/2) + z0=param[:, 0].mean() + lz=(-l/2,l/2) + + 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) + + if manip: + self.pltCrist(0,0,0,0) + else: + self.pltOrig(Trf.trans(0, 0, 0)) + self.update_dx_dz_w_y() + plt.show() + + def update_dx_dz_w_y(self,val=None): + helScn=self.helScn + dx = self.sldDx.val + dz = self.sldDz.val + w = self.sldW.val + y = self.sldY.val + w=w*d2r + (cx,cz,w,fy)=helScn.inv_transform(dx,dz,w,y) + if self.manip: + param = self.helScn.param + # param[i]=(z_i, y_i, x_i, r_i,phi_i) + #precise caldulation of rotation center dependent of y + (z0, y0, x0, r0, phi0)=param[0] + (z1, y1, x1, r1, phi1)=param[1] + rx = x0+(x1-x0)/(y1-y0)*(fy-y0) + rz = z0+(z1-z0)/(y1-y0)*(fy-y0) + m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz))) + self.pltOrig(m) + else: + self.pltCrist(-cx,-cz,w,-fy) + self.fig.canvas.draw_idle() + + def interactive_anim(self,manip=False): + param=self.helScn.param + rec=self.helScn.rec + fig = plt.figure();fig.canvas.set_window_title('interactive_anim'+(' manip' if manip else '')) + self.manip=manip + 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=10, azim=-170) + + # param[i]=(z_i, y_i, x_i, r_i,phi_i) + l=max(2*param[:,3].max(),param[:,1].ptp()) #max of diameter and y peaktopeak + self.scale=l #scale is the length of a cube were the pltCrist object fits into + + axFrm=plt.axes([0.1, 0.01, 0.8, 0.02]) + if manip: + y0=param[:, 1].mean() + x0=param[:, 2].mean() + z0=param[:, 0].mean() + self.axSetCenter((x0,y0,z0),l) + else: + self.axSetCenter((0, 0, 0), l) + + self.sldFrm=sFrm=Slider(axFrm, 'frm', 0, rec.shape[0]-1, valinit=0) + sFrm.on_changed(self.update_anim) + self.fig=fig + + if manip: + self.pltCrist(0,0,0,0) + else: + self.pltOrig(Trf.trans(0, 0, 0)) + self.update_anim(0) + plt.show() + + def update_anim(self,frm): + rec=self.helScn.rec + (cx, cz, w, fy)=rec[int(frm),:4] + #data/=. #scale from um to mm + w*=d2r/1000 # scale from deg to rad + if self.manip: + param = self.helScn.param + # param[i]=(z_i, y_i, x_i, r_i,phi_i) + #rx=param[:,2].mean();rz=param[:,0].mean() + #precise caldulation of rotation center dependent of y + (z0, y0, x0, r0, phi0)=param[0] + (z1, y1, x1, r1, phi1)=param[1] + rx = x0+(x1-x0)/(y1-y0)*(fy-y0) + rz = z0+(z1-z0)/(y1-y0)*(fy-y0) + m=Trf.trans(rx,fy,rz).dot(Trf.rotY(-w).dot(Trf.trans(cx-rx,0,cz-rz))) + self.pltOrig(m) + else: + self.pltCrist(-cx,-cz,w,-fy) + self.fig.canvas.draw_idle() + + def anim_gather_data(self,idx): + rec=self.helScn.rec + (cx, cz, w, fy)=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 pltOrig(self,m): + '''plots a xyz axes in rgb colors that shows the transformation matrix m + if h is not none, the handles are the existing object and is modified + m is a 4x4 matrix. the transformed matrix + ''' + ax=self.ax + idx=(2,0,1) + r=m[idx,0]*self.scale/3.#1st + g=m[idx,1]*self.scale/3.#2nd + b=m[idx,2]*self.scale/3.#3rd + o=m[idx,3] #origin !do not scale orogin + lines = np.ndarray((3, 2, 3)) # numlines, points, xyz + lines[:, 0, :] = o + lines[0, 1, :] = o + r + lines[1, 1, :] = o + g + lines[2, 1, :] = o + b + + try: + h=self.hOrig + except AttributeError: + lseg = tuple(lines) + col=('r','g','b') + h = plt3d.art3d.Line3DCollection(lseg, colors=col, linewidths=2) # , *args[argi:], **kwargs) + ax.add_collection(h) + self.hOrig=h + else: + h.set_segments(lines) + + def pltCrist(self,cx,cz,w,fy): + #h are the handles + ax = self.ax + helScn = self.helScn + param = helScn.param + pt = np.ndarray((4, 3)) + try: + h=self.hCrist + except AttributeError: + h=[] #handels + for i in range(2): + (z, y, x, r, phi) = param[i] + x+=cx;y+=fy;z+=cz;phi+=w + pt[i] = (z, x, y) + pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y) + obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('y' if i==0 else 'r', alpha=0.2)) + h1=ax.add_patch(obj) + h2=plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z") + #print h1._segment3d + h.append(obj) + #hs=ax.scatter(pt[:, 2], pt[:, 0], pt[:, 1]) + hs=ax.plot(pt[:, 0], pt[:, 1], pt[:, 2],'.') + hp=ax.plot(pt[2:, 0], pt[2:, 1], pt[2:, 2]) + h+=(hs[0],hp[0]) + if hasattr(helScn,'points'): + lines = self.get_meas_lines(pt,cx,cz,fy,w) + p=tuple(lines[:,0,:].T) + hl =plt3d.art3d.Line3D(*p,color='r',marker='.')#, *args[argi:], **kwargs) + ax.add_artist(hl);h.append(hl) + + lseg=tuple(lines) + col=(mpl.colors.colorConverter.to_rgba('k'),)*len(lseg) + hlc=plt3d.art3d.Line3DCollection(lseg,colors=col)#, *args[argi:], **kwargs) + ax.add_collection(hlc);h.append(hlc) + self.hCrist=h + else: + for i in range(2): + (z, y, x, r, phi) = param[i] + x+=cx;y+=fy;z+=cz;phi+=w + pt[i] = (z, x, y) + pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y) + h[i].remove() + obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('y' if i==0 else 'r', alpha=0.2)) + ax.add_patch(obj) + plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z") + h[i]=obj + h[2].set_data(pt[:, 0], pt[:, 1])#, pt[:, 1])) + h[2].set_3d_properties(pt[:, 2]) + + h[3].set_data(pt[2:, 0], pt[2:, 1])#, pt[:, 1])) + h[3].set_3d_properties(pt[2:, 2]) + if hasattr(helScn,'points'): + lines = self.get_meas_lines(pt,cx,cz,fy,w) + hl=h[4] + #hl.set_data(lines[:,0,0], lines[:,0,1]) # , pt[:, 1])) + #hl.set_3d_properties(lines[:,0,2]) + p=tuple(lines[:,0,:].T) + hl._verts3d=p;hl.stale=True + + hlc=h[5] + hlc.set_segments(lines) + + return (h,pt) + + def get_meas_lines(self,pt,cx,cz,fy,w,arwLen=None): + if arwLen is None: + arwLen=self.scale*.05 # default arrow length=0.01 of crystal size + param = self.helScn.param + pts = self.helScn.points + dx_ = pts[:, 0] # add 0.2 to test + dz_ = pts[:, 1] # add 0.2 to test + w_ = pts[:, 2] * (d2r / 1000.) + y_ = pts[:, 3] + + # self.inv_transform(self, dx, dz, w, y): + + f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1]) + lines = np.ndarray((pts.shape[0], 2, 3)) + ofx=dx_*-np.sin(w-w_)+dz_*np.cos(w-w_) # 0.2=dx + ofy=dx_*np.cos(w-w_)+dz_*np.sin(w-w_) + lines[:, 0, 0] = pt[2, 0] + (pt[3, 0] - pt[2, 0]) * f +ofx # z data + lines[:, 0, 1] = pt[2, 1] + (pt[3, 1] - pt[2, 1]) * f +ofy # x data + lines[:, 0, 2] = pts[:, 3] + fy # y data + lines[:, 1, 0] = lines[:, 0, 0] + np.cos(w-w_)*arwLen + lines[:, 1, 1] = lines[:, 0, 1] + np.sin(w-w_)*arwLen + lines[:, 1, 2] = lines[:, 0, 2] + return lines + + 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) + + +class HelicalScanTests(): + @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 + + @staticmethod + def test_coord_trf(helScn): + param = helScn.param + cx, cz, w, fy, = (0.2,0.3,0.1,0.4) + #cx, cz, w, fy, = (10.,20,3.,40) + print('input : cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)) + (dx,dz,w,y) = helScn.fwd_transform(cx,cz,w,fy) + print('fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)) + (cx,cz,w,fy) = helScn.inv_transform(dx,dz,w,y) + print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)) + + dx, dz, w, y, = (0.2,0.3,0.1,0.4) + #dx, dz, w, y, = (10.,20,3.,40) + print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)) + (cx,cz,w,fy) = helScn.inv_transform(dx,dz,w,y) + print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)) + (dx,dz,w,y) = helScn.fwd_transform(cx,cz,w,fy) + print('fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)) + + @staticmethod + def mpl_test(): + plt.ion() + fig = plt.figure() + ax = fig.add_axes([.2, .05, .75, .90]) + x=np.arange(0,2*np.pi,.1) + y=np.sin(x) + h=ax.plot(x,y) + # ax.set_position([.2,.05,.75,.90]) + #ax2 = ax.twinx() + ax2 = fig.add_axes([.2, .05, .75, .90],sharex=ax) + ax2.set_position([.1,.05,.75,.90]) + + ax2.spines['top'].set_visible(False) + ax2.spines['bottom'].set_visible(False) + + ax2.xaxis.set_visible(False) + ax2.patch.set_visible(False) + + y2=y+.1*np.random.random(y.shape) + h+=ax2.plot(x,y2,'g') + ax2.set_position(ax.get_position()) + ax2.set_ylabel('mylbl', color='r') + ax2.tick_params(axis='y', colors='b') + ax2.spines['left'].set_position(('axes',-.1)) + plt.legend(handels=h) + pass + + @staticmethod + def calcParamSim(): + #simulated test values + n = 3.; + per = 1.; + w = 2 * np.pi * per / n * np.arange(n) + #(z_i, y_i, x_i, r_i, phi_i)=param[i] + #paramSim = ((14.5, 2.3, .71, 4.12, 10.6 * d2r),(14.5, 6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi) + #paramSim = ((14.5, 2.3, -100., 10, 10. * d2r),(14.5, 6.2, 100., 10., -10. * d2r)) # (y, bias, ampl, phi) + paramSim = ((14.5, 2.3, -100., 10, 0. * d2r),(14.5, 6.2, 100., 10., 90. * d2r)) # (y, bias, ampl, phi) + paramSim=np.array(paramSim) + param = np.ndarray(paramSim.shape) + for i in range(param.shape[0]): + (z,y,bias,ampl,phi) = paramSim[i] + measX = ampl * np.sin(w + phi) + bias + measZ = ampl * np.cos(w + phi) + bias + print('xMeas_%d=' % i + str(measX) + ' zMeas_%d=' % i + str(measZ)) + # 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(measX) # (bias,ampl,phase) + (bias, ampl, phase) = param[i][2:] + print(param) + print(paramSim) + assert((paramSim-param).max()<1E-10) + return param + + +class HelicalScan(MotionBase): + def __init__(self,comm, gather, verbose): + MotionBase.__init__(self,comm, gather, verbose) + + def load_rec(self,fn_npz='/tmp/helicalscan.npz'): + try: + fh=np.load(fn_npz) + except IOError as e: + sys.stderr.write('Unable to open File: '+fn_npz+'\n') + else: + pass + s='content of numpy file: '+fn_npz+'\n' + for k,v in fh.items(): + s+=' '+k+': '+str(v.dtype)+' '+str(v.shape)+'\n' + setattr(self,k,v) + print(s) + + 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,x=((-241.,96.,-53.),(-162.,-293.,246.)), + y=(575.,175.), + z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))): +# #1,3,4,5p +# point 1 0,120,240 deg +# 575.5 0 -241.5 -1401.3 +# 575.5 120000 96.7 -1401.7 +# 575.5 240000 -53.8 -1802.4 +# +# point 2 0,120,240 deg +# 175.5 0 -162.3 -1802.5 +# 175.5 120000 -293.2 -1303.7 +# 175.5 240000 246.4 -1402.25 + + #real measured values: + #y : 2x1 array : y position were the measurements were taken + #x : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1] + #z : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1] + # the z value is only used to find a rought bias of z + + assert(len(y)==2) + n = float(len(x[0])) #number of angles + per = 1 #number of rotations + self.param = param = np.ndarray((2,5)) + for i in range(len(y)): + # param[i]=(z_i, y_i, x_i, r_i,phi_i) + param[i, 0] = HelicalScan.meas_rot_ctr(z[i])[0] + param[i, 1] = y[i] + param[i, 2:] = HelicalScan.meas_rot_ctr(x[i]) # (bias,ampl,phase) + (bias, ampl, phase) = param[i][2:] + + #check correctness of center: + w = 2 * np.pi * per / n * np.arange(n) + x_ = ampl * np.cos(w + phase) + bias + print(x_) + (dx,dz,w,y_) = (0,0,0,y[0]) + print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y_)) + (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y_) + print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)) + (dx, dz, w, y_) = (0,0,0,y[1]) + print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx, dz, w / d2r * 1000., y_)) + (cx, cz, w, fy) = self.inv_transform(dx, dz, w, y_) + print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx, cz, w / d2r * 1000., fy)) + + print(param) + + + @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) + assert(np.imag(f[0])==0.) + bias=np.real(f[0]/n) + #phase=np.angle(f[idx]) #f(w)=bias+ampl*cos(w+phase) + phase=np.angle(f[idx]*1j) #f(w)=bias+ampl*sin(w+phase) + ampl=np.absolute(f[idx])*2/n + return (bias,ampl,phase) + + def setup_gather(self,acq_per=1): + ''' + setup the channels to gather + kwargs: + acq_per : acquire period: acquire data all acq_per servo loops (default=1) + ''' + comm=self.comm + gt=self.gather + gt.set_phasemode(False) + #gt.set_address("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos") + gt.set_address("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos","Gate3[1].Chan[0].UserFlag") + gt.set_property(MaxSamples=1000000, Period=acq_per) + ServoPeriod= .2 #0.2ms #Sys.ServoPeriod is dependent of !common() macro + #ServoPeriod=comm.gpascii.servo_period + self.meta = {'timebase': ServoPeriod*acq_per} + + def setup_coord_trf(self,fnCrdTrf='/tmp/coordTrf.cfg'): + param=self.param + prg = ''' +// Set the motors as inverse kinematic axes in CS 1 +//motors CX CZ RY FY +// 4 5 3 1 +&1 +a +#4->0 +#5->0 +#3->0 +#1->0 + +#4->I +#5->I +#3->I +#1->I +''' + s = ('z', 'y', 'x', 'r', 'phi') + a = np.ones(param.shape[1], dtype=np.uint8).reshape(1, -1) + b = np.arange(param.shape[0], dtype=np.uint8).reshape(1, -1) + c = np.matmul(b.T, a).reshape(-1) + subsParam=dict(map(lambda k, i, v: (k + '_' + str(i), v), s * param.shape[0], c, param.reshape(-1))) + subsParam['d2r']=d2r/1000. + subsParam['r2d']=1000./d2r + + + subs={'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1', + 'DX':'C6', 'DZ':'C8', 'W':'C1', 'Y':'C7', + #coord X Z B Y + 'p0_x':'L10', 'p0_y':'L11', 'p0_z':'L12', + 'p1_x':'L13', 'p1_y':'L14', 'p1_z':'L15', + 'scale':'L16'} + subs.update(subsParam) + + prg+=''' +open forward + send 1"fwd_inp(%f) %f %f %f %f\\n",D0,{qCX},{qCZ},{qW},{qFY} + {W}={qW} + {qW}={qW}*{d2r} //scale from 1000*deg to rad + + {p0_x}={x_0}+{r_0}*sin({phi_0}+{qW}) + {p1_x}={x_1}+{r_1}*sin({phi_1}+{qW}) + {p0_y}={y_0} + {p1_y}={y_1} + {p0_z}={z_0}+{r_0}*cos({phi_0}+{qW}) + {p1_z}={z_1}+{r_1}*cos({phi_1}+{qW}) + + {scale}=({qFY}-({y_0}))/({y_1}-({y_0})) + {p0_x}={p0_x}+{scale}*({p1_x}-{p0_x}) + {p0_y}={p0_y}+{scale}*({p1_y}-{p0_y}) + {p0_z}={p0_z}+{scale}*({p1_z}-{p0_z}) + {DX}={qCX}-{p0_x} + {DZ}={qCZ}-{p0_z} + {Y}={qFY} + //send 1"fwd_res %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 +'''.format(**subs) + + #coord X Z B Y + subs={ 'DX':'C6' , 'DZ':'C8' , 'W':'C1', 'Y':'C7', + 'vDX':'C38', 'vDZ':'C40', 'vW':'C33', 'vY':'C39', + #motor q4 q5 q3 q1 + 'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1', + 'vqCX':'R4', 'vqCZ':'R5', 'vqW':'R3', 'vqFY':'R1', + + 'p0_x':'L10', 'p0_y':'L11', 'p0_z':'L12', + 'p1_x':'L13', 'p1_y':'L14', 'p1_z':'L15', + 'p_x':'L16', 'p_y':'L17', 'p_z':'L18', + 'sclY':'L19', + 'scl':'L20'} + + subs.update(subsParam) + + prg+=''' +open inverse + //if(D0>0) + // send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,{DX},{vDX},{DZ},{vDZ},{W},{vW},{Y},{vY} + //else + // send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y} + {qW}={W} + {W}={W}*{d2r} //scale from 1000*deg to rad + + {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_1}+{W}) + + {sclY}=({Y}-({y_0}))/({y_1}-({y_0})) + {p_x}={p0_x}+{sclY}*({p1_x}-{p0_x}) + {p_y}={p0_y}+{sclY}*({p1_y}-{p0_y}) + {p_z}={p0_z}+{sclY}*({p1_z}-{p0_z}) + {qCX}={DX}+{p_x} + {qCZ}={DZ}+{p_z} + {qFY}={Y} + if(D0>0) + {{ // calculate velocities for PVT motion + {vW}={vW}*{d2r} //scale from 1000*deg to rad + {p_x}={sclY}*({p1_x}-{p0_x}) + {p_z}={sclY}*({p1_z}-{p0_z}) + {vqFY}={vY} + {vqCX}={vDX} + ({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY} //+ vqW*p_z + {vqCZ}={vDZ} + ({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY} //+ vqW*p_x + //vqW=vqW+(vqCX+(p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+(vqCZ+(p1_z-p0_z)/(p1_y-p0_y)*vY)*p_x + {vqW}={vW}//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x + {vqW}={vqW}*{r2d} //scale from rad to 1000*deg + // send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY} + }} + //else + // send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY} + //P1002+=1 +close +'''.format(**subs) + +# vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY + vqW*p_x +# vDX is in same direction, so add as it is +# (p1_x-p0_x)/(p1_y-p0_y)*vY velocity part in vqCX direction, when moving in vY +# vqW*p_x velocity part of the rotation (vqW is in rad) + comm = self.comm + if comm is not None: + gpascii = comm.gpascii + gpascii.send_block('disable plc 0') + time.sleep(.5) + gpascii.send_block(prg) + if self.verbose & 4: + print(prg) + if fnCrdTrf is not None : + fh=open(fnCrdTrf,'w') + fh.write(prg) + fh.close() + if comm is not None: + time.sleep(.5) + gpascii.send_block('enable plc 0') + time.sleep(.5) + gpascii.send_block('#1..7j/') + + def setup_motion(self,prgId=2,fnPrg='/tmp/prg.cfg',mode=0,**kwargs): + ''' + kwargs: + acq_per : acquire period: acquire data all acq_per servo loops (default=1) + mode=-1: test motion + mode=0: #linear motion with 100 ms break at each measurement + cnt : move path multiple times + cntVert : number of vertical measurements + cntHor : number of horizontal measurements + hRng : min, max horizontal boundaries + wRng : starting and ending angle + yRng : starting and ending height + mode=1: #PVT motion + pt2pt_time : time to move from one point to the next point + cnt : move path multiple times + cntVert : number of vertical measurements + cntHor : number of horizontal measurements + hRng : min, max horizontal boundaries + wRng : starting and ending angle + yRng : starting and ending height + ''' + param=self.param + prg=[] + prg.append('open prog %d'%(prgId)) + prg.append(' P1000=0') + # this uses Coord[1].Tm and limits with MaxSpeed + + #******** mode -1 ******** + 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') + + # ******** mode 0 ******** + elif mode==0: #### linear motion + #y=2.3 6.2 dx=0, dz=0 w=0..3600000 # 10 rev + cnt= kwargs.get('cnt', 1) #move path multiple times + cntVert = kwargs.get('cntVert', 12) + cntHor = kwargs.get('cntHor', 4) + hRng = kwargs.get('hRng', (-.2,.2)) + wRng = kwargs.get('wRng', (0,360000)) + yRng = kwargs.get('yRng', self.param[:,1]) + + numPt=cntVert*cntHor + pt=np.zeros((numPt,4)) + if cntHor>1: + a=np.linspace(hRng[0],hRng[1],cntHor) + a=np.append(a,a[::-1]) + a=np.tile(a,(cntVert+1)//2) + pt[:,0]= a[0:pt.shape[0]] + #pt[:,1]= np.linspace(0,.2,numPt) #dz + pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w + pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y + self.points=pt + #prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics + prg.append(' nofrax') #needed to set all axis to use AltFeedRate + prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed + prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode + prg.append(' linear abs') + prg.append(' X%g Z%g B%g Y%g' % tuple(pt[0, :])) + prg.append(' dwell 100') + prg.append(' Gather.Enable=2') + prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed + prg.append(' Coord[1].SegMoveTime=0') #to calculate every 1 ms the inverse kinematics + prg.append(' P2000=0') + for i in range(pt.shape[0]): + prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :])) + #prg.append(' P2000=%d'%(i,)) + prg.append(' dwell 10') + prg.append(' Gather.Enable=0') + + # ******** mode 1 ******** + elif mode==1: #### pvt motion + #y=2.3 6.2 dx=0, dz=0 w=0..3600000 # 10 rev + cnt= kwargs.get('cnt', 1) #move path multiple times + cntVert = kwargs.get('cntVert', 12) + cntHor = kwargs.get('cntHor', 4) + hRng = kwargs.get('hRng', (-.2,.2)) + wRng = kwargs.get('wRng', (0,360000)) + yRng = kwargs.get('yRng', self.param[:,1]) + pt2pt_time = kwargs.get('pt2pt_time', 100) + smt = kwargs.get('smt', 1) # SegMoveTime, default = 1ms -> velocity calc not yet 100% correct (smt=0 not 100% working) + numPt=cntVert*cntHor + pt=np.zeros((numPt,4)) + if cntHor>1: + a=np.linspace(hRng[0],hRng[1],cntHor) + a=np.append(a,a[::-1]) + a=np.tile(a,(cntVert+1)//2) + pt[:,0]= a[0:pt.shape[0]] + #pt[:,1]= np.linspace(0,.2,numPt) #dz + pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w + pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y + self.points=pt + #pv is an array of dx,dz,w,y,vel_dx,vel_dz,vel_w,vel_y + pv=np.ndarray(shape=(pt.shape[0]+2,8),dtype=pt.dtype) + pv[:]=np.NaN + pv[ 0,(0,1,2,3)]=pt[0,:] + pv[ 1:-1,(0,1,2,3)]=pt + pv[ -1,(0,1,2,3)]=pt[-1,:] + pv[(0,0,0,0,-1,-1,-1,-1),(4,5,6,7,4,5,6,7)]=0 + dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)] + pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time) + + prg.append(' nofrax') #needed to set all axis to use AltFeedRate + prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed + prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode + prg.append(' linear abs') + prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)])) + prg.append(' dwell 10') + try: prg.extend(self.sync_prg.split('\n')) + except AttributeError: + # print('no sync code available') + pass + prg.append(' Gather.Enable=2') + if cnt>1: + prg.append(' P100=%d'%cnt) + prg.append(' N100:') + prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed + prg.append(' Coord[1].SegMoveTime=%d'%smt) #to calculate every 1 ms the inverse kinematics + prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position + for idx in range(1,pv.shape[0]): + #prg.append(' P2000=%d'%idx) + prg.append(' X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)])) + #prg.append('Y%g:%g' % tuple(pv[idx, (5, 7)])) + #prg.append('B%g:%g' %(idx*1000,0)) + 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 Z%g B%g Y%g' % tuple(pv[0, (0, 1, 2, 3)])) + prg.append(' dwell 100') + prg.append(' goto 100') + prg.append(' }') + else: + prg.append(' dwell 1000') + prg.append(' Gather.Enable=0') + + prg.append(' P1000=1') + prg.append('close') + #prg.append('&1\nb%dr\n'%prgId) + + prg='\n'.join(prg) + '\n' + comm = self.comm + if comm is not None: + gpascii = comm.gpascii + gpascii.send_block(prg) + if self.verbose & 4: + print(prg) + if fnPrg is not None : + fh=open(fnPrg,'w') + fh.write(prg) + fh.close() + + def gather_upload(self,fnRec='/tmp/helicalscan.npz'): + gpascii=self.comm.gpascii + gt=self.gather + gt.wait_stopped(verbose=True) + self.rec=rec=gt.upload() + channels = ["Motor[4].HomePos", "Motor[5].HomePos", "Motor[3].HomePos", "Motor[1].HomePos"] + ofs = np.ndarray(len(channels)) + for i, v in enumerate(channels): + ofs[i] = gpascii.get_variable(v,float) + rec[:,:4]-=ofs + if fnRec: + np.savez_compressed(fnRec, rec=rec, points=self.points, param=self.param, meta=self.meta) + + +if __name__=='__main__': + def run_test(args): + args.host=None + if args.host is None: + comm=gather=None + else: + comm = PPComm(host=args.host) + gather = Gather(comm) + gpascii = comm.gpascii + #HelicalScanTests.calcParamSim() + hs=HelicalScan(comm, gather, args.verbose) + + #hs.test_find_rot_ctr() + #hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6) + fn='/tmp/helicalscan' + fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helicalscan' + hs.load_rec(fn+'.npz') + #hs.param[:4]+=np.pi/2.#add 90 deg + + print hs.param + print hs.points + fh=open("/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helical.cmd") + s=fh.read();s=s.replace('calcParam','hs.calcParam') + eval(s) + hs.setup_coord_trf() + hs.setup_motion(mode=1,cntHor=5,cntVert=15,hRng=(-150,150),wRng=(0,120000),smt=0,pt2pt_time=200) + print hs.param + print hs.points + + + + hsg=HelicalScanGui(hs);hsg.interactive_anim() + #hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=True) + #while True:hsg.update_anim(0) + #hs.param = np.ndarray((2,5)) + #hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) + #hs.param[1]=(15,4,0,2,np.pi/4)#(z_i, y_i, x_i, r_i,phi_i) + + #hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy() + #hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=True) + #while True: hsg.update_cx_cz_w_fy() #for debug purpose + + + #hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y() + #hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=True) + #while True: hsg.update_dx_dz_w_y() #for debug purpose + #return + + #TODO: FE Digitizers PBPS117 timing not working! + + #hs.calcParam() + + #gpasci: #1,4,5p // y,-x ,-z + # 0deg 256.7 -762.5 -396.4 + #120deg 258.5 731.7 -1896.9 + #240deg 256.2 -1282.8 -2496.5 + # 0deg 586.5 -1023.4 -696.8 + # 120deg 574.8 619.8 -1797.5 + # 240deg 580.0 -900.0 -2496.3 + + #hs.calcParam(x = ((-762.5, 731.7, -1282.8), (-1023.4, 619.8, -900.0)), + # y = (258.5,580.0), + # z = (( -396.4,-1896.9,-2496.5),(-696.8,-1797.5,-2496.3))) + + #cpx X0 Z0 B0 Y258 + #cpx X0 Z0 B120000 Y258 + #cpx X0 Z0 B240000 Y258 + #cpx X0 Z0 B0 Y580 + #cpx X0 Z0 B120000 Y580 + #cpx X0 Z0 B240000 Y580 + + + # 0deg 1405.7 -1154.4 -1309.6 + # 120deg 1401.7 216.3 -1010.9 + # 240deg 1407.9 -250.7 -2410.3 + # 0deg 1053.9 -1330.2 -1219.4 + # 120deg 1019.2 340.9 -918.8 + # 240deg 984.0 -230.4 -2510.4 + + #hs.calcParam(x = ((-1154.4, 216.3, -250.7), ( -1330.2, 340.9, -230.4)), + # y = (1405.7,1019.2), + # z = ((-1309.6, -1010.9, -2410.3),( -1219.4, -918.8, -2510.4))) + + + #&1p + #cpx X0 Z0 B0 Y1405 + #cpx X0 Z0 B120000 Y1405 + #cpx X0 Z0 B240000 Y1405 + #cpx X0 Z0 B0 Y1019 + #cpx X0 Z0 B120000 Y1019 + #cpx X0 Z0 B240000 Y1019 + + +#1600.5 -1484.0 -1264.2 +#1599.0 234.6 -663.3 +#1599.9 28.5 -2264.7 +# 468.3 629.9 -2663.3 +# 472.2 25.3 -164.4 +# 470.9 -1841.3 -1965.0 + + #hs.calcParam(x = ((-1484.0, 234.6, 28.5), ( 629.9, 25.3,-1841.3)), + # y = (1600.,470.), + # z = ((-1264.2, -663.3,-2264.7),(-2663.3, -164.4,-1965.0))) + + + #cpx X0 Z0 B0 Y1405 + #cpx X0 Z0 B120000 Y1405 + #cpx X0 Z0 B240000 Y1405 + #cpx X0 Z0 B0 Y1019 + #cpx X0 Z0 B120000 Y1019 + #cpx X0 Z0 B240000 Y1019 + fh=open("/sf/bernina/config/swissmx/exchange/helical.cmd") + s=fh.read();s=s.replace('calcParam','hs.calcParam') + eval(s) + + #hs.calcParamSim() + #hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) + #hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) + #hs.param[0]=(-100, 100,0,50,0)#(z_i, y_i, x_i, r_i,phi_i) + #hs.param[1]=(-100,-100,0,70,0)#(z_i, y_i, x_i, r_i,phi_i) + + #hs.test_coord_trf() + #hs.interactive_cx_cz_w_fy() + #hs.interactive_dx_dz_w_y() + + #mode bits: + #0:1 config simulated motors + #1:2 config real motors + #2:4 config coord trf + + #os.chdir(os.path.join(os.path.dirname(__file__),'../cfg')) + #'sim_8_motors.cfg' + #['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25']) + # raw_input('press return when homed') + hs.setup_coord_trf() + + hs.setup_sync(mode=2) # None: no sync at all mode=1: sync on timing UserFlag + hs.setup_gather() + + #hs.gen_prog(mode=-1) + #hs.gen_prog(mode=0,cntHor=1,cntVert=3,wRng=(120000,120000)) + #hs.gen_prog(mode=0,cntHor=1) + #hs.gen_prog(mode=0) + #hs.gen_prog(mode=0,cntHor=1,cntVert=5,wRng=(120000,120000)) + #hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100) + #hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100,smt=0) + #hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(0,360000)) + #hs.gen_prog(mode=1,cntHor=1,cntVert=5,hRng=(-.3,.3),wRng=(0,360000),smt=1) + #hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-3,3),wRng=(120000,120000),smt=0) + #hs.gen_prog(mode=1,cntHor=3,cntVert=6,hRng=(-5,5),wRng=(00,120000),smt=0,pt2pt_time=10) + #hs.gen_prog(mode=0,cntHor=3,cntVert=10,hRng=(-5,5),wRng=(0,120000)) + #hs.gen_prog(mode=0,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000)) + #hs.gen_prog(mode=1,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000),smt=0,pt2pt_time=300) + hs.setup_motion(mode=1,cntHor=5,cntVert=15,hRng=(-150,150),wRng=(0,120000),smt=0,pt2pt_time=200) + #hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=40) + #hs.gen_prog(mode=1,cntHor=3,cntVert=20,hRng=(-5,5),wRng=(0,1200),smt=0,pt2pt_time=200) + #hs.gen_prog(mode=1, cntHor=2, cntVert=2, wRng=(0, 360000), smt=0) + #hs.gen_prog(mode=1) + #hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3)) + #hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=10,cntHor=3,hRng=(-30,30),wRng=(0,36000),yRng=(-50,-100)) + #hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-100,50),wRng=(000,10000),smt=0) + + + hs.run() + print('wait until gather finished:') + hs.gather_upload(fn+'.npz') + + hs.load_rec(fn+'.npz') + + hsg=HelicalScanGui(hs) + hsg.show_pos() + hsg.show_vel() + hsg.interactive_anim() + + + 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=255) + parser.add_option('--host', help='hostname', default='SAR-CPPM-EXPMX1') + + (args, other)=parser.parse_args() + args.other=other + run_test(args) + return + + #------------------ Main Code ---------------------------------- + ret=parse_args() + exit(ret)