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

fix display. Behaviour for motion remains compatible.

Browse Source
This commit is contained in:
zamofing_t
2018-10-26 15:57:31 +02:00
parent 00c84235a0
commit 9c8e2a417e
4 changed files with 1524 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
SAR-EXPMX1.subs
View File

@@ -16,12 +16,12 @@
file asyn.template {{P="$(P_M)1", PORT=$(PORT_M)}} file asyn.template {{P="$(P_M)1", PORT=$(PORT_M)}}
file PPMACMotor.template { file PPMACMotor.template {
pattern{ DESC , P , M , PORT , ADDR, DIR, VELO, HVEL, ACCL, JAR, MRES , PREC, EGU , DHLM, DLLM} 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 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 , 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 , 1 , 50 , 50 , 0.1 , 20 , -0.001, 3 , "deg", 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 , 0 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 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 , 0 , 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 , 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 , 1 , 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 }
} }

6
python/MXMotion.py
View File

@@ -44,7 +44,6 @@ class MotionBase:
sync_wait can be put in the program to force a timing sync sync_wait can be put in the program to force a timing sync
sync_run are the commands to run the whole program sync_run are the commands to run the whole program
''' '''
gpascii = self.comm.gpascii
if mode == 0: if mode == 0:
try: try:
del self.sync_prg del self.sync_prg
@@ -83,7 +82,10 @@ while(1)
disable plc {plcId} disable plc {plcId}
close close
'''.format(plcId=plcId, crdId=crdId, flag=flag) '''.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_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) self.sync_run = 'enable plc {plcId};&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId)

474
python/helicalscan.py
View File

@@ -20,23 +20,42 @@ Mot 5: Stage Z Stada Stepper 670mA 200 poles 1 rev = 100*2
Enc 6: Interferometer Y Enc 6: Interferometer Y
Enc 7: Interferometer X Enc 7: Interferometer X
verbose bits: verbose bits:
#1 basic info #1 basic info
#2 plot sorting steps #2 plot sorting steps
4 list program 4 list program
#4 upload progress #4 upload progress
#8 plot gather path #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 os, sys, json,re
import numpy as np import numpy as np
import matplotlib as mpl import matplotlib as mpl
@@ -56,10 +75,7 @@ from pbtools.misc.gather import Gather
from MXMotion import MotionBase from MXMotion import MotionBase
d2r=2*np.pi/360 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: class Trf:
#https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector #https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector
@@ -184,17 +200,17 @@ class HelicalScanGui():
if manip: if manip:
y0=param[:, 1].mean() y0=param[:, 1].mean()
ly=sorted(param[:,1]) ly=sorted(param[:,1])
x0=param[:, 2].mean() x0=-param[:, 2].mean()
lx=(-l/2+x0,l/2+x0) lx=(-l/2+x0,l/2+x0)
z0=param[:, 0].mean() z0=-param[:, 0].mean()
lz=(-l/2+z0,l/2+z0) lz=(-l/2+z0,l/2+z0)
self.axSetCenter((x0,y0,z0),l) self.axSetCenter((-x0,-y0,-z0),l)
else: else:
self.axSetCenter((0, 0, 0), l) self.axSetCenter((0, 0, 0), l)
ly=sorted(param[:,1]) ly=sorted(param[:,1])
x0=param[:, 2].mean() x0=-param[:, 2].mean()
lx=(-l/2+x0,l/2+x0) lx=(-l/2+x0,l/2+x0)
z0=param[:, 0].mean() z0=-param[:, 0].mean()
lz=(-l/2+z0,l/2+z0) lz=(-l/2+z0,l/2+z0)
self.sldCx=sCx=Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.) 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) # param[i]=(z_i, y_i, x_i, r_i,phi_i)
#rx=param[:,2].mean();rz=param[:,0].mean() #rx=param[:,2].mean();rz=param[:,0].mean()
#precise caldulation of rotation center dependent of y #precise caldulation of rotation center dependent of y
(z0, y0, x0, r0, phi0)=param[0] (z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0
(z1, y1, x1, r1, phi1)=param[1] (z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1
rx = x0+(x1-x0)/(y1-y0)*(fy-y0) rx = x0+(x1-x0)/(y1-y0)*(fy-y0)
rz = z0+(z1-z0)/(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) self.pltOrig(m)
else: else:
self.pltCrist(-cx, -cz, w, -fy) self.pltCrist(cx, cz, w, fy)
#self.pltCrist(cx,cz,w*d2r,fy)
self.fig.canvas.draw_idle() self.fig.canvas.draw_idle()
def interactive_dx_dz_w_y(self,manip=False): def interactive_dx_dz_w_y(self,manip=False):
@@ -255,17 +270,15 @@ class HelicalScanGui():
if manip: if manip:
y0=param[:, 1].mean() y0=param[:, 1].mean()
ly=sorted(param[:,1]) ly=sorted(param[:,1])
x0=param[:, 2].mean() x0=-param[:, 2].mean()
lx=(-l/2,l/2) lx=(-l/2,l/2)
z0=param[:, 0].mean() z0=-param[:, 0].mean()
lz=(-l/2,l/2) lz=(-l/2,l/2)
self.axSetCenter((x0,y0,z0),l) self.axSetCenter((-x0,-y0,-z0),l)
else: else:
self.axSetCenter((0, 0, 0), l) self.axSetCenter((0, 0, 0), l)
ly=sorted(param[:,1]) ly=sorted(param[:,1])
x0=param[:, 2].mean()
lx=(-l/2,l/2) lx=(-l/2,l/2)
z0=param[:, 0].mean()
lz=(-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.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 = self.helScn.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i) # param[i]=(z_i, y_i, x_i, r_i,phi_i)
#precise caldulation of rotation center dependent of y #precise caldulation of rotation center dependent of y
(z0, y0, x0, r0, phi0)=param[0] (z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0
(z1, y1, x1, r1, phi1)=param[1] (z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1
rx = x0+(x1-x0)/(y1-y0)*(fy-y0) rx = x0+(x1-x0)/(y1-y0)*(fy-y0)
rz = z0+(z1-z0)/(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) self.pltOrig(m)
else: else:
self.pltCrist(-cx,-cz,w,-fy) self.pltCrist(cx,cz,w,fy)
self.fig.canvas.draw_idle() self.fig.canvas.draw_idle()
def interactive_anim(self,manip=False): def interactive_anim(self,manip=False):
@@ -321,10 +334,10 @@ class HelicalScanGui():
axFrm=plt.axes([0.1, 0.01, 0.8, 0.02]) axFrm=plt.axes([0.1, 0.01, 0.8, 0.02])
if manip: if manip:
y0=param[:, 1].mean() y0= param[:, 1].mean()
x0=param[:, 2].mean() x0=-param[:, 2].mean()
z0=param[:, 0].mean() z0=-param[:, 0].mean()
self.axSetCenter((x0,y0,z0),l) self.axSetCenter((-x0,-y0,-z0),l)
else: else:
self.axSetCenter((0, 0, 0), l) self.axSetCenter((0, 0, 0), l)
@@ -341,27 +354,24 @@ class HelicalScanGui():
def update_anim(self,frm): def update_anim(self,frm):
rec=self.helScn.rec rec=self.helScn.rec
(cx, cz, w, fy)=rec[int(frm),:4] (cx, cz, w, fy)=rec[int(frm),:4];cx=-cx;cz=-cz
#data/=. #scale from um to mm
w*=d2r/1000 # scale from deg to rad w*=d2r/1000 # scale from deg to rad
if self.manip: if self.manip:
param = self.helScn.param param = self.helScn.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i) # param[i]=(z_i, y_i, x_i, r_i,phi_i)
#rx=param[:,2].mean();rz=param[:,0].mean() (z0, y0, x0, r0, phi0)=param[0];x0=-x0;z0=-z0
#precise caldulation of rotation center dependent of y (z1, y1, x1, r1, phi1)=param[1];x1=-x1;z1=-z1
(z0, y0, x0, r0, phi0)=param[0]
(z1, y1, x1, r1, phi1)=param[1]
rx = x0+(x1-x0)/(y1-y0)*(fy-y0) rx = x0+(x1-x0)/(y1-y0)*(fy-y0)
rz = z0+(z1-z0)/(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) self.pltOrig(m)
else: else:
self.pltCrist(-cx,-cz,w,-fy) self.pltCrist(cx,cz,w,fy)
self.fig.canvas.draw_idle() self.fig.canvas.draw_idle()
def anim_gather_data(self,idx): def anim_gather_data(self,idx):
rec=self.helScn.rec 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 w*=d2r/1000 # scale from deg to rad
self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist) self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
#data=self.rec[int(idx*self.step),:] #data=self.rec[int(idx*self.step),:]
@@ -406,8 +416,8 @@ class HelicalScanGui():
except AttributeError: except AttributeError:
h=[] #handels h=[] #handels
for i in range(2): for i in range(2):
(z, y, x, r, phi) = param[i] (z, y, x, r, phi) = param[i];z=-z;x=-x
x+=cx;y+=fy;z+=cz;phi+=w x=cx-x;y=fy-y;z=cz-z;phi=phi+w
pt[i] = (z, x, y) pt[i] = (z, x, y)
pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), 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)) 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]) hp=ax.plot(pt[2:, 0], pt[2:, 1], pt[2:, 2])
h+=(hs[0],hp[0]) h+=(hs[0],hp[0])
if hasattr(helScn,'points'): 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) p=tuple(lines[:,0,:].T)
hl =plt3d.art3d.Line3D(*p,color='r',marker='.')#, *args[argi:], **kwargs) hl =plt3d.art3d.Line3D(*p,color='r',marker='.')#, *args[argi:], **kwargs)
ax.add_artist(hl);h.append(hl) ax.add_artist(hl);h.append(hl)
@@ -432,8 +442,8 @@ class HelicalScanGui():
self.hCrist=h self.hCrist=h
else: else:
for i in range(2): for i in range(2):
(z, y, x, r, phi) = param[i] (z, y, x, r, phi) = param[i];z=-z;x=-x
x+=cx;y+=fy;z+=cz;phi+=w x=cx-x;y=fy-y;z=cz-z;phi=phi+w
pt[i] = (z, x, y) pt[i] = (z, x, y)
pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y) pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y)
h[i].remove() h[i].remove()
@@ -447,7 +457,7 @@ class HelicalScanGui():
h[3].set_data(pt[2:, 0], pt[2:, 1])#, pt[:, 1])) h[3].set_data(pt[2:, 0], pt[2:, 1])#, pt[:, 1]))
h[3].set_3d_properties(pt[2:, 2]) h[3].set_3d_properties(pt[2:, 2])
if hasattr(helScn,'points'): 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=h[4]
#hl.set_data(lines[:,0,0], lines[:,0,1]) # , pt[:, 1])) #hl.set_data(lines[:,0,0], lines[:,0,1]) # , pt[:, 1]))
#hl.set_3d_properties(lines[:,0,2]) #hl.set_3d_properties(lines[:,0,2])
@@ -459,7 +469,11 @@ class HelicalScanGui():
return (h,pt) 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: if arwLen is None:
arwLen=self.scale*.05 # default arrow length=0.01 of crystal size arwLen=self.scale*.05 # default arrow length=0.01 of crystal size
param = self.helScn.param param = self.helScn.param
@@ -468,16 +482,13 @@ class HelicalScanGui():
dz_ = pts[:, 1] # add 0.2 to test dz_ = pts[:, 1] # add 0.2 to test
w_ = pts[:, 2] * (d2r / 1000.) w_ = pts[:, 2] * (d2r / 1000.)
y_ = pts[:, 3] y_ = pts[:, 3]
f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1]) #part of y move: 0..1
# 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)) lines = np.ndarray((pts.shape[0], 2, 3))
ofx=dx_*-np.sin(w-w_)+dz_*np.cos(w-w_) # 0.2=dx 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_) 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, 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, 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, 0] = lines[:, 0, 0] + np.cos(w-w_)*arwLen
lines[:, 1, 1] = lines[:, 0, 1] + np.sin(w-w_)*arwLen lines[:, 1, 1] = lines[:, 0, 1] + np.sin(w-w_)*arwLen
lines[:, 1, 2] = lines[:, 0, 2] lines[:, 1, 2] = lines[:, 0, 2]
@@ -494,18 +505,17 @@ class HelicalScanGui():
class HelicalScanTests(): class HelicalScanTests():
@staticmethod @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 # find the rotation center, amplitude out of n (niminum 3) measurements
# n number of equidistant measurements # n number of equidistant measurements
# per number of periods (full rotation of all measurements nut be a interger value for precise 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 # bias bias value
# ampl amplitude # ampl amplitude
t = np.arange(n) t = np.arange(n)
w=2*np.pi*per/n*t w=2*np.pi*per/n*t
y=ampl*np.cos(w+phi*d2r)+bias y=bias+ampl*np.sin(w+phi)
plt.figure(1) plt.figure('test_find_rot_ctr')
plt.subplot(311) plt.subplot(311)
plt.plot(t,y,'b.-') plt.plot(t,y,'b.-')
@@ -513,19 +523,20 @@ class HelicalScanTests():
f = np.fft.fft(y) f = np.fft.fft(y)
plt.step(t, f.real,'b.-', t, f.imag,'r.-', where='mid') plt.step(t, f.real,'b.-', t, f.imag,'r.-', where='mid')
(bias,ampl,phase)=HelicalScan.meas_rot_ctr(y, per) inp=np.array((bias,ampl,phi))
print('bias: '+str(bias)) (bias_,ampl_,phi_)=out=HelicalScan.meas_rot_ctr(y, per)
print('amplitude: '+str(ampl)) print('input bias:{} amplitude:{} phase:{: .4f}'.format(bias,ampl,phi/d2r))
print('phase: '+str(phase*360./2/np.pi)) print('output bias:{} amplitude:{} phase:{: .4f}'.format(bias_,ampl_,phi_/d2r))
assert(np.abs((inp-out)).max()<1E-10)
plt.subplot(313) plt.subplot(313)
t2 = np.linspace(0,2*np.pi,64) 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.plot(t2,y2,'g-')
plt.stem(w,y,'b-') plt.stem(w,y,'b-')
plt.show() plt.show()
pass
@staticmethod @staticmethod
def test_coord_trf(helScn): def test_coord_trf(helScn):
@@ -575,31 +586,54 @@ class HelicalScanTests():
pass pass
@staticmethod @staticmethod
def calcParamSim(): def calcParamSim(hs):
#simulated test values #simulated test values
n = 3.; n = 3.;
per = 1.; per = 1.;
w = 2 * np.pi * per / n * np.arange(n) 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, .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, 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) paramSim=np.array(paramSim)
param = np.ndarray(paramSim.shape) measX=np.ndarray((paramSim.shape[0],w.size))
for i in range(param.shape[0]): measZ=np.ndarray((paramSim.shape[0],w.size))
(z,y,bias,ampl,phi) = paramSim[i] for i in range(paramSim.shape[0]):
measX = ampl * np.sin(w + phi) + bias (z_i,y_i,x_i,r_i,phi_i) = paramSim[i]
measZ = ampl * np.cos(w + phi) + bias measX[i,:] = x_i+r_i*np.sin(w+phi_i)
print('xMeas_%d=' % i + str(measX) + ' zMeas_%d=' % i + str(measZ)) measZ[i,:] = z_i+r_i*np.cos(w+phi_i)
# param[i]=(z_i, y_i, x_i, r_i,phi_i) if i==0:
param[i, 0] = z print('measX='+str(measX[i,:])+' measZ='+str(measZ[i,:]))
param[i, 1] = y else:
param[i, 2:] = HelicalScan.meas_rot_ctr(measX) # (bias,ampl,phase) print(' '+str(measX[i,:])+' '+str(measZ[i,:]))
(bias, ampl, phase) = param[i][2:] measY=paramSim[:, 1]
print(param) print('measY='+str(measY))
hs.calcParam(measX,measY,measZ)
print(hs.param)
print(paramSim) print(paramSim)
assert((paramSim-param).max()<1E-10) assert(abs(paramSim-hs.param).max()<1E-10)
return param
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): class HelicalScan(MotionBase):
@@ -628,10 +662,10 @@ class HelicalScan(MotionBase):
param=self.param param=self.param
p=np.ndarray((param.shape[0], 3)) p=np.ndarray((param.shape[0], 3))
for i in range(2): for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i] (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(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx 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,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] v=p[1]-p[0]
#for y = 0..1: #for y = 0..1:
#v=v*y #v=v*y
@@ -652,10 +686,10 @@ class HelicalScan(MotionBase):
param=self.param param=self.param
p=np.ndarray((param.shape[0], 3)) p=np.ndarray((param.shape[0], 3))
for i in range(2): for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i] (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(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx p[i,0]=x_i-r_i*np.sin(w+phi_i)
p[i,1]=y_i # y= y_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] v=p[1]-p[0]
#for y = 0..1: #for y = 0..1:
#v=v*y #v=v*y
@@ -672,23 +706,11 @@ class HelicalScan(MotionBase):
def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)), def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)),
y=(575.,175.), y=(575.,175.),
z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))): 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: #real measured values:
#y : 2x1 array : y position were the measurements were taken #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] #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] #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 # the z value is only used to find a rought bias of z
assert(len(y)==2) assert(len(y)==2)
n = float(len(x[0])) #number of angles n = float(len(x[0])) #number of angles
per = 1 #number of rotations per = 1 #number of rotations
@@ -700,35 +722,26 @@ class HelicalScan(MotionBase):
param[i, 2:] = HelicalScan.meas_rot_ctr(x[i]) # (bias,ampl,phase) param[i, 2:] = HelicalScan.meas_rot_ctr(x[i]) # (bias,ampl,phase)
(bias, ampl, phase) = param[i][2:] (bias, ampl, phase) = param[i][2:]
#check correctness of center: # check correctness of center:
w = 2 * np.pi * per / n * np.arange(n) w=2*np.pi*per/n*np.arange(n)
x_ = ampl * np.cos(w + phase) + bias x_=bias+ampl*np.sin(w+phase)
print(x_) #print(x_)
(dx,dz,w,y_) = (0,0,0,y[0]) assert ((x[i]-x_).max()<1E-10)
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 @staticmethod
def meas_rot_ctr(y,per=1): 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 # 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 # per is the number of persiods, default is 1 period =360 deg
n=len(y) n=len(y)
f = np.fft.fft(y) f = np.fft.rfft(y)
idx=int(per) idx=int(per)
#bias=np.absolute(f[0]/n) #bias=np.absolute(f[0]/n)
assert(np.imag(f[0])==0.) assert(np.imag(f[0])==0.)
bias=np.real(f[0]/n) bias=np.real(f[0]/n)
#phase=np.angle(f[idx]) #f(w)=bias+ampl*cos(w+phase) #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 ampl=np.absolute(f[idx])*2/n
return (bias,ampl,phase) return (bias,ampl,phase)
@@ -739,6 +752,7 @@ class HelicalScan(MotionBase):
acq_per : acquire period: acquire data all acq_per servo loops (default=1) acq_per : acquire period: acquire data all acq_per servo loops (default=1)
''' '''
comm=self.comm comm=self.comm
if comm is None:return
gt=self.gather gt=self.gather
gt.set_phasemode(False) 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")
@@ -749,8 +763,6 @@ class HelicalScan(MotionBase):
self.meta = {'timebase': ServoPeriod*acq_per} self.meta = {'timebase': ServoPeriod*acq_per}
def setup_coord_trf(self,fnCrdTrf='/tmp/coordTrf.cfg'): def setup_coord_trf(self,fnCrdTrf='/tmp/coordTrf.cfg'):
comm = self.comm
gpascii = comm.gpascii
param=self.param param=self.param
prg = ''' prg = '''
// Set the motors as inverse kinematic axes in CS 1 // Set the motors as inverse kinematic axes in CS 1
@@ -768,13 +780,17 @@ a
#3->I #3->I
#1->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') s = ('z', 'y', 'x', 'r', 'phi')
a = np.ones(param.shape[1], dtype=np.uint8).reshape(1, -1) a = np.ones(sh[1], dtype=np.uint8).reshape(1, -1)
b = np.arange(param.shape[0], 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) 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['d2r']=d2r/1000.
subsParam['r2d']=1000./d2r subsParam['r2d']=1000./d2r
subsParam['cmt']='//'
subs={'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1', subs={'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1',
@@ -787,7 +803,7 @@ a
prg+=''' prg+='''
open forward 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} {W}={qW}
{qW}={qW}*{d2r} //scale from 1000*deg to rad {qW}={qW}*{d2r} //scale from 1000*deg to rad
@@ -805,8 +821,7 @@ open forward
{DX}={qCX}-{p0_x} {DX}={qCX}-{p0_x}
{DZ}={qCZ}-{p0_z} {DZ}={qCZ}-{p0_z}
{Y}={qFY} {Y}={qFY}
//send 1"fwd_res %f %f %f %f\\n",{DX},{DZ},{W},{Y} {cmt}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 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close close
'''.format(**subs) '''.format(**subs)
@@ -828,10 +843,10 @@ close
prg+=''' prg+='''
open inverse open inverse
//if(D0>0) {cmt}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} {cmt} send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,{DX},{vDX},{DZ},{vDZ},{W},{vW},{Y},{vY}
//else {cmt}else
// send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y} {cmt} send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y}
{qW}={W} {qW}={W}
{W}={W}*{d2r} //scale from 1000*deg to rad {W}={W}*{d2r} //scale from 1000*deg to rad
@@ -851,20 +866,16 @@ open inverse
{qFY}={Y} {qFY}={Y}
if(D0>0) if(D0>0)
{{ // calculate velocities for PVT motion {{ // calculate velocities for PVT motion
{vW}={vW}*{d2r} //scale from 1000*deg to rad
{p_x}={sclY}*({p1_x}-{p0_x}) {p_x}={sclY}*({p1_x}-{p0_x})
{p_z}={sclY}*({p1_z}-{p0_z}) {p_z}={sclY}*({p1_z}-{p0_z})
{vqFY}={vY} {vqFY}={vY}
{vqCX}={vDX} + ({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY} //+ vqW*p_z {vqCX}={vDX}+({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY}
{vqCZ}={vDZ} + ({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY} //+ vqW*p_x {vqCZ}={vDZ}+({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY}
//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}
{vqW}={vW}//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x {cmt}send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY}
{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 {cmt}else
// send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY} {cmt} send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY}
//P1002+=1
close close
'''.format(**subs) '''.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 # (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) # vqW*p_x velocity part of the rotation (vqW is in rad)
gpascii.send_block('disable plc 0') # {{ // calculate velocities for PVT motion
time.sleep(.5) # {vW}={vW}*{d2r} //scale from 1000*deg to rad
gpascii.send_block(prg) # {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: if self.verbose & 4:
print(prg) print(prg)
if fnCrdTrf is not None : if fnCrdTrf is not None :
fh=open(fnCrdTrf,'w') fh=open(fnCrdTrf,'w')
fh.write(prg) fh.write(prg)
fh.close() fh.close()
time.sleep(.5) if comm is not None:
gpascii.send_block('enable plc 0') time.sleep(.5)
time.sleep(.5) gpascii.send_block('enable plc 0')
gpascii.send_block('#1..7j/') time.sleep(.5)
gpascii.send_block('#1..7j/')
def setup_motion(self,prgId=2,fnPrg='/tmp/prg.cfg',mode=0,**kwargs): def setup_motion(self,prgId=2,fnPrg='/tmp/prg.cfg',mode=0,**kwargs):
''' '''
@@ -908,8 +935,6 @@ close
wRng : starting and ending angle wRng : starting and ending angle
yRng : starting and ending height yRng : starting and ending height
''' '''
comm = self.comm
gpascii = comm.gpascii
param=self.param param=self.param
prg=[] prg=[]
prg.append('open prog %d'%(prgId)) prg.append('open prog %d'%(prgId))
@@ -1047,7 +1072,10 @@ close
#prg.append('&1\nb%dr\n'%prgId) #prg.append('&1\nb%dr\n'%prgId)
prg='\n'.join(prg) + '\n' 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: if self.verbose & 4:
print(prg) print(prg)
if fnPrg is not None : if fnPrg is not None :
@@ -1071,127 +1099,78 @@ close
if __name__=='__main__': if __name__=='__main__':
def run_test(args): def run_test(args):
#args.host=None test=args.test
args.host=None
if args.host is None: if args.host is None:
comm=gather=None comm=gather=None
else: else:
comm = PPComm(host=args.host) comm = PPComm(host=args.host)
gather = Gather(comm) gather = Gather(comm)
gpascii = comm.gpascii gpascii = comm.gpascii
#HelicalScanTests.calcParamSim()
hs=HelicalScan(comm, gather, args.verbose) hs=HelicalScan(comm, gather, args.verbose)
#hs.test_find_rot_ctr() if test==1:
#hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6) ### test find rotation center ###
fn='/tmp/helicalscan' HelicalScanTests.test_find_rot_ctr(n=32. ,per=1.,bias=2.31,ampl=4.12,phi=0*d2r)
#hs.load_rec(fn+'.npz') HelicalScanTests.test_find_rot_ctr(n=32. ,per=1.,bias=2.31,ampl=4.12,phi=30.*d2r)
#hs.param[:4]+=np.pi/2.#add 90 deg 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() elif test==2:
#hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=True) ### test param calculation and display the gui###
#while True:hsg.update_anim(0) HelicalScanTests.calcParamSim(hs)
#hs.param = np.ndarray((2,5)) hs.setup_motion(mode=1,cntHor=5,cntVert=15,hRng=(-50,50),wRng=(0,120000),smt=0,pt2pt_time=200)
#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() for manip in (False,True):
#hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=True) hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=manip)
#while True: hsg.update_cx_cz_w_fy() #for debug purpose 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() for manip in (False,True):
#hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=True) hsg=HelicalScanGui(hs);hsg.interactive_anim(manip=manip)
#while True: hsg.update_dx_dz_w_y() #for debug purpose hsg=HelicalScanGui(hs);hsg.interactive_cx_cz_w_fy(manip=manip)
#return hsg=HelicalScanGui(hs);hsg.interactive_dx_dz_w_y(manip=manip)
return
#TODO: FE Digitizers PBPS117 timing not working!
#hs.calcParam()
#gpasci: #1,4,5p // y,-x ,-z #gpasci: #1,4,5p // y,-x ,-z
# 0deg 256.7 -762.5 -396.4 # 0deg 256.7 -762.5 -396.4
#120deg 258.5 731.7 -1896.9 #120deg 258.5 731.7 -1896.9
#240deg 256.2 -1282.8 -2496.5 #...
# 0deg 586.5 -1023.4 -696.8 #&1p
# 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 B0 Y258
#cpx X0 Z0 B120000 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)), #hs.calcParam(x = ((-1154.4, 216.3, -250.7), ( -1330.2, 340.9, -230.4)),
# y = (1405.7,1019.2), # y = (1405.7,1019.2),
# z = ((-1309.6, -1010.9, -2410.3),( -1219.4, -918.8, -2510.4))) # z = ((-1309.6, -1010.9, -2410.3),( -1219.4, -918.8, -2510.4)))
#&1p ### use simulation motors ###
#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')) #os.chdir(os.path.join(os.path.dirname(__file__),'../cfg'))
#'sim_8_motors.cfg' #'sim_8_motors.cfg'
#['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25']) #['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25'])
# raw_input('press return when homed') # raw_input('press return when homed')
hs.setup_coord_trf()
### hs.calcParam(<data from Zac>) ###
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_sync(mode=2) # None: no sync at all mode=1: sync on timing UserFlag
hs.setup_gather() 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,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.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-100,50),wRng=(000,10000),smt=0)
hs.run() hs.run()
print('wait until gather finished:') print('wait until gather finished:')
fn='/tmp/helicalscan'
hs.gather_upload(fn+'.npz') hs.gather_upload(fn+'.npz')
hs.load_rec(fn+'.npz') hs.load_rec(fn+'.npz')
hsg=HelicalScanGui(hs) hsg=HelicalScanGui(hs)
hsg.show_pos() hsg.show_pos()
hsg.show_vel() 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('-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('--host', help='hostname', default='SAR-CPPM-EXPMX1')
parser.add_option('-t','--test', type="int", default=None)
(args, other)=parser.parse_args() (args, other)=parser.parse_args()
args.other=other args.other=other

1286
python/helicalscan_.py Executable file
View File

File diff suppressed because it is too large Load Diff