#!/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 modes: 0: test scratch mode 1: test find rotation center 2: test param calculation and display the gui 3: load recorded data, check param calculation not changed display in the gui 4: load recorded data, display in the interactive_anim gui (default) ''' #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 from __future__ import print_function #from __future__ import absolute_import,division,generators,nested_scopes,print_function,unicode_literals,with_statement try: raw_input;input=raw_input except NameError: pass 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/')) 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)}) 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];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(rx-cx,0,rz-cz))) self.pltOrig(m) else: self.pltCrist(cx, cz, w, 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]) lx=(-l/2,l/2) 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];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(rx-cx,0,rz-cz))) 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];cx=-cx;cz=-cz #change sign because axes have neg direction 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) (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(rx-cx,0,rz-cz))) 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),:];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),:] #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];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)) 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,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];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() 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,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,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 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] 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] = 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] 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*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 (in rad) # bias bias value # ampl amplitude t = np.arange(n) w=2*np.pi*per/n*t y=bias+ampl*np.sin(w+phi) plt.figure('test_find_rot_ctr') 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') 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=bias+ampl*np.sin(t2+phi_) plt.plot(t2,y2,'g-') plt.stem(w,y,'b-') plt.show() @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(hs): #simulated test values n = 3.; per = 1.; w = 2 * np.pi * per / n * np.arange(n) # 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, 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) 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(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): def __init__(self, comm, gather, verbose, **kwargs): if comm==None: return MotionBase.__init__(self, comm, gather, verbose, **kwargs) 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];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(w+phi_i) # 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];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(w+phi_i) 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 calcParam1Pt(self,x=(-241.,-162.), y=(575.,175.), z=(-1401.,-1802.), w=1.2): ''' the rotation center of the stage does not change for a new cristal. after the function calcParam was called once, this 1 point coordinate transformation can be used. it needs 1 point at start and 1 point at end of the crystal ''' #x: x position at y0 and y1 : (x_y0, x_y1) #y: lower and upper cristal point (y0, y1) #z: x position at y0 and y1 : (z_y0, z_y1) #w: stage angle in radians # param[i]=(z_i, y_i, x_i, r_i,phi_i) # z_i not changed # y_i trivial # x_i not changed # r_i calculate # phi_i calculate try: param=self.param except AttributeError as e: raise AttributeError('calcParam must be called first') for i in range(len(y)): r_i =np.sqrt(x[i]**2+z[i]**2) phi_i=np.arctan2(z[i],x[i]) param[i, 1]=y[i] param[i, 3:]=(r_i,phi_i-w) pass def calcParam2Pt(self,x=((-241.,96.),(-162.,-293.)), y=(575.,175.), w=(1.2,1.4)): ''' the rotation center of the stage does not change for a new cristal. after the function calcParam was called once, this 2 point coordinate transformation can be used. it needs 2 point at start and 2 point at end of the crystal ''' #x: ((x_w0y0, x_w1y0),(x_w0y1, x_w1y1) #y: lower and upper cristal point (y0, y1) #w: start and end stage angle in radians (w0,w1) # param[i]=(z_i, y_i, x_i, r_i,phi_i) # z_i not changed # y_i trivial # x_i not changed # r_i calculate # phi_i calculate try: param=self.param except AttributeError as e: raise AttributeError('calcParam must be called first') for i in range(len(y)): # param[i]=(z_i, y_i, x_i, r_i,phi_i) r_i=np.sqrt(x[i]**2+z[i]**2) x0=x[i][0]-param[i,2] x1=x[i][1]-param[i,2] ww=w[i] if x0>x1: phi_i=np.arctan2(np.cos(ww)-x1/x0,np.sin(ww)) r_i =x0/np.cos(phi_i) else: phi_i=np.arctan2(np.cos(ww)-x0/x1,np.sin(ww)) r_i =x1/np.cos(phi_i) param[i, 1]=y[i] param[i, 3:]=(r_i,phi_i) pass def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)), y=(575.,175.), z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))): ''' calculates coordinate parameters out of measurements at aequidistant angles (typically 0,120,240 deg) if a needle tip is used to calibrate (only one y value) use: x=(x_meas,x_meas), (x_meas is a nx1 array) z=(x_meas,x_meas), (z_meas is a nx1 array) y=(y_meas,x_meas+ofs), (y_meas is a value, ofs is any value>0 recommended 100.) ''' # param[i]=(z_i, y_i, x_i, r_i,phi_i) #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_=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 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.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) 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 if comm is None:return gt=self.gather gt.set_phasemode(False) #up to now the trigger is not saved #if self.meta['sync_flag']&2: # address=("Motor[4].ActPos", "Motor[5].ActPos", "Motor[3].ActPos", "Motor[1].ActPos","Coord[1].Q[11]") #else: # address=("Motor[4].ActPos", "Motor[5].ActPos", "Motor[3].ActPos", "Motor[1].ActPos","Gate3[1].Chan[1].UserFlag") address=("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos") gt.set_address(*address) 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.update({'acq_per':acq_per,'address':address}) 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 #1->0 #2->0 #3->0 #4->0 #5->0 #4->I #5->I #3->I #1->I Motor[4].MaxSpeed=8 Motor[5].MaxSpeed=8 ''' pbParam=param.copy() sh=pbParam.shape s = ('z', 'y', 'x', 'r', 'phi') 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 * 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', '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 {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 {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} {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) #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 {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 {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 {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} {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} }} {cmt}else {cmt} send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY} 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) # {{ // 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() 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 sync_frq=kwargs.get('sync_frq',10) # synchronization mark all n points 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) dwell = kwargs.get('dwell', 100) # wait time at end of motion 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(' 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 %d' % dwell) prg.append(' goto 100') prg.append(' }') else: prg.append('dwell %d' % dwell) prg.append(' Gather.Enable=0') 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=self.meta['address'] 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): mode=args.mode if mode in(3,4): comm=gather=None else: import socket s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # create an INET, STREAMing socket s.settimeout(.1) try: port=22 #ssh port s.connect((args.host, port)) # try to connect to port s.close() except (socket.error, socket.gaierror) as e: comm=gather=None else: comm = PPComm(host=args.host) gather = Gather(comm) # direct start #hs=HelicalScan(comm, gather, args.verbose,sync_mode=0,pt2pt_time=20) #simulated start and frame trigger no sync #hs=HelicalScan(comm, gather, args.verbose,sync_mode=1,sync_flag=3) #simulated start and frame trigger with sync #hs=HelicalScan(comm, gather, args.verbose,sync_mode=2,sync_flag=3) #simulated start real frame trigger no sync #hs=HelicalScan(comm, gather, args.verbose,sync_mode=1,sync_flag=1) #simulated start real frame trigger with sync hs=HelicalScan(comm, gather, args.verbose,sync_mode=2,sync_flag=1) if mode==0: # gpasci: #1,4,5p // y,-x ,-z # 0deg 256.7 -762.5 -396.4 # 120deg 258.5 731.7 -1896.9 # ... # &1p # cpx X0 Z0 B0 Y258 # cpx X0 Z0 B120000 Y258 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))) #hs.calcParam(x=((-70.25682272433501, 287.48437780001825, -477.99870973215764), # (-41.466601738723284, 274.71565975031103, -504.9508517714285)), # y=(1207.6326052816642, 1704.2138281362475), z=( # (-1196.2847548574225, -1686.284754857423, -1686.284754857423), # (-1196.2847548574225, -1686.284754857423, -1686.284754857423))) ### 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.calcParam() ### # fh=open("/sf/bernina/config/swissmx/exchange/helical.cmd") # fh=open("/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helical.cmd") #fh = open("/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/lyso001_0063_helical_debug.cmd") #fh = open("/sf/bernina/data/p17592/res/20181130/helicaltest/lyso005/lyso005_0072_helical_debug.cmd") # find /sf/bernina/data/p17592/res/ -name '*.cmd' #s = fh.read(); #s = s.replace('calcParam', 'hs.calcParam') #eval(s) # &1p # cpx X0 Z0 B0 Y258 # cpx X0 Z0 B120000 Y258 hs.setup_gather(acq_per=1) hs.setup_sync(verbose=args.verbose & 32) hs.setup_coord_trf() # reset to shape path system # 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.setup_motion(mode=1, cntHor=10, cntVert=20, hRng=(-300, 300), wRng=(0, 100000), smt=0, pt2pt_time=40) # 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() hs.wait_armed() # wait until motors are at first position hs.trigger(0.5) # send a start trigger (if needed) ater given time 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() hsg.interactive_anim() if mode==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 elif mode==2: ### test param calculation and display the gui### HelicalScanTests.calcParamSim(hs) hs.setup_gather() # setup_sync(self, crdId=1, prgId=2, plcId=2, mode=0, **kwargs): # sp.setup_sync() #no sync at all # sp.setup_sync(mode=1) #sync with timing system (PROG) hs.setup_sync(mode=2) # sync with timing system and PLC to sync speed (PROG) hs.setup_coord_trf() hs.setup_motion(mode=1,cntHor=5,cntVert=15,hRng=(-50,50),wRng=(0,120000),smt=0,pt2pt_time=200) hs.run() print('wait until gather finished:') fn = '/tmp/helicalscan' hs.gather_upload(fn + '.npz') hs.load_rec(fn + '.npz') 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 mode==3: ### load recorded data, #check param calculation not changed display in the gui### fn=args.other[0] hs.load_rec(fn) paramRec=hs.param #fh=open("/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helical.cmd") fh = open(fn[-4]+'_helical_debug.cmd') s=fh.read();s=s.replace('calcParam','hs.calcParam') eval(s) assert (abs(paramRec-hs.param).max()<1E-10) 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 elif mode==4: ### load recorded data, display in the interactive_anim gui### fn=args.other[0] hs.load_rec(fn) hsg = HelicalScanGui(hs) hsg.show_pos() hsg.interactive_anim(manip=False) return 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=('-m0', '/sf/bernina/data/p17592/res/20181130/helicaltest/lyso001/lyso001_0062.npz ' ) 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') parser.add_option('-m','--mode', type="int", default=4) (args, other)=parser.parse_args() args.other=other run_test(args) return #------------------ Main Code ---------------------------------- ret=parse_args() exit(ret)