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

helicalscan commissioned at ESB_MX!

Browse Source
This commit is contained in:
zamofing_t
2018-09-26 16:43:34 +02:00
parent 6f5278ec5c
commit d589cf8aed
3 changed files with 207 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
python/helicalscan.py
View File

@@ -470,9 +470,9 @@ class HelicalScan:
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,0]=x_i+r_i*np.cos(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)
p[i,2]=z_i-r_i*np.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
v=p[1]-p[0]
#for y = 0..1:
#v=v*y
@@ -495,9 +495,9 @@ class HelicalScan:
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,0]=x_i+r_i*np.cos(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)
p[i,2]=z_i-r_i*np.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
v=p[1]-p[0]
#for y = 0..1:
#v=v*y
@@ -512,11 +512,13 @@ class HelicalScan:
return res
def calcParam(self):
def calcParamSim(self):
#simulated test values
n = 3.;
per = 1.;
w = 2 * np.pi * per / n * np.arange(n)
p = ((2.3, .71, 4.12, 10.6 * d2r),(6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi)
#p = ((2.3, .71, 4.12, 10.6 * d2r),(6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi)
p = ((2.3, -100., 10, 10. * d2r),(6.2, 100., 10., -10. * d2r)) # (y, bias, ampl, phi)
self.param = param = np.ndarray((len(p), 5))
z = 14.5 # fix z position
for i in range(2):
@@ -531,6 +533,53 @@ class HelicalScan:
print param
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
def pltOrig(self,m,h=None):
ax=self.ax
# m is a 4x4 matrix. the transformed matrix
@@ -646,7 +695,9 @@ class HelicalScan:
n=len(y)
f = np.fft.fft(y)
idx=int(per)
bias=np.absolute(f[0]/n)
#bias=np.absolute(f[0]/n)
assert(np.imag(f[0])==0.)
bias=np.real(f[0]/n)
phase=np.angle(f[idx])
ampl=np.absolute(f[idx])*2/n
return (bias,ampl,phase)
@@ -689,7 +740,7 @@ open forward
define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16')
//send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY''')
send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY''')
for i in range(2):
#https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
@@ -697,12 +748,12 @@ open forward
prg.append(" W=qW")
prg.append(" qW=qW*%g"%(d2r/1000.)) #scale from 1000*deg to rad
prg.append('''
p0_x=x_0+r_0*sin(phi_0+qW)
p1_x=x_1+r_1*sin(phi_1+qW)
p0_x=x_0+r_0*cos(phi_0+qW)
p1_x=x_1+r_1*cos(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)
p0_z=z_0-r_0*sin(phi_0+qW)
p1_z=z_1-r_1*sin(phi_1+qW)
scale=(qFY-(y_0))/(y_1-(y_0))
p0_x=p0_x+scale*(p1_x-p0_x)
@@ -711,7 +762,7 @@ open forward
DX=qCX-p0_x
DZ=qCZ-p0_z
Y=qFY
//send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y
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
@@ -732,10 +783,10 @@ open inverse
define(p_x='L16', p_y='L17', p_z='L18')
define(sclY='L19')
define(scl='L20')
//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''')
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''')
for i in range(2):
# https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
@@ -744,12 +795,12 @@ open inverse
prg.append(" W=W*%g"%(d2r/1000.)) #scale from 1000*deg to rad
prg.append('''
p0_x=x_0+r_0*sin(phi_0+W)
p1_x=x_1+r_1*sin(phi_1+W)
p0_x=x_0+r_0*cos(phi_0+W)
p1_x=x_1+r_1*cos(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)
p0_z=z_0-r_0*sin(phi_0+W)
p1_z=z_1-r_1*sin(phi_1+W)
sclY=(Y-(y_0))/(y_1-(y_0))
p_x=p0_x+sclY*(p1_x-p0_x)
@@ -770,10 +821,10 @@ open inverse
vqW=vW//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x
''')
prg.append(" vqW=vqW*%g"%(1000./d2r)) #scale from rad to 1000*deg
prg.append('''// send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY
prg.append(''' 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
else
send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY
//P1002+=1
close
''')
@@ -1096,12 +1147,13 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
hs.calcParam()
#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.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.test_coord_trf()
#hs.interactive_cx_cz_w_fy()
#hs.interactive_dx_dz_w_y()
@@ -1109,7 +1161,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#0:1 config simulated motors
#1:2 config real motors
#2:4 config coord trf
mode=6#5#4#0
mode=4#5#4#0
os.chdir(os.path.join(os.path.dirname(__file__),'../cfg'))
if mode&1:
hs.download(file='sim_8_motors.cfg')
@@ -1140,7 +1192,9 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#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.gen_prog(mode=1,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000),smt=0,pt2pt_time=300)
#hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=300)
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)