#!/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)