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

37
Readme.md
View File

@@ -1014,4 +1014,41 @@ Gate3[1].Chan[0].UserFlag -> is set to 0 when event triggered (mapped to output
```
Testing Helical Coord Trf
-------------------------
```
B0.3504637004371034 X-13.68414496427224 Y-8.029999999998836 Z-1483.096457761122
&1p ->this will trigger:forward kinematic
cpx pmatch ->this will trigger:forward kinematic
cpx ;linear rel; X0Y0Z0B0
cpx ;linear abs; X-13.68 Y-8.03 Z-1483.1 B0.35
->this will trigger: inverse
&1;cpx abs linear;jog1=0;jog2=0;jog3=0;jog4=0;jog5=0
-1:fwd_inp(0) 0.45 -1103.7 0.350464 -7.98
-1:fwd_res -13.5538 -1481 0.350464 -7.98
//motors CX CZ RY FY
// 4 5 3 1
&1;cpx abs linear;jog1=0;jog2=0;jog3=0;jog4=0;jog5=0
def calcParam(self,x=((.468,-.627,-.523),(.357,-1.349,.351)),
y=(.557,-.008),
z=((1.73,.93,2.129),(2.13,.03,1.103))):
&1;cpx abs linear;jog1=557;jog3=0;jog4=468;jog5=1730
&1;cpx abs linear;jog1=-8;jog3=240000;jog4=2129;jog5=1.73
caQtDM -macro "NAME=ESB-MX-CAM,CAMNAME=ESB-MX-CAM" /sf/controls/config/qt/Camera/CameraMiniView

123
python/PBTuning.py
View File

@@ -34,7 +34,10 @@ Modes:
3: sine bode closed loop
4: chirp bode closed loop
5: full bode open loop record of both motors (including init_stage
10: bode of current step -> does not work because gathering phase data not implemented
-> check https://github.com/klauer/ppmac for fast data gathering server which supports
phase gathering -> not yet compiling: /home/zamofing_t/Documents/prj/SwissFEL/PowerBrickInspector/ppmac/fast_gather
BUT data acquired and stored in: /media/zamofing_t/DataUbuHD/VirtualBox/shared/data
TODO:
use openloopsine to create a bode diagram of the 'strecke'
'''
@@ -48,6 +51,8 @@ import subprocess as sprc
import telnetlib
from scipy.signal.waveforms import chirp
from scipy import signal
from scipy import interpolate
from scipy import stats
from utilities import *
class PBTuning:
@@ -71,8 +76,9 @@ class PBTuning:
fnLoc=self.fnLoc
except AttributeError:
fnLoc = '/tmp/gather.txt'
cmd=(PBGatherPlot,'-m24','-v0','--host',host,'--dat',fnLoc)
p = sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE)
cmd=(PBGatherPlot,'-m24','-v255','--host',host,'--dat',fnLoc)
#p = sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE)
p = sprc.Popen(cmd, shell=False)
retval = p.wait()
print(p.stderr.read())
#print(p.stdout.read())
@@ -312,11 +318,10 @@ class PBTuning:
f=np.load(file)
bode=f['bode']
meta=f['meta'].item()
frq=bode[:,0]
mag=bode[:,1]
phase=np.unwrap(bode[:,2])
frq=[bode[:,0],]
mag=[bode[:,1],]
phase=[np.unwrap(bode[:,2]),]
l=[0,len(frq)]
#for fn in ('chirp_ol_mot%da.npz','chirp_ol_mot%db.npz','chirp_ol_mot%dc.npz','chirp_ol_mot%dd.npz'):
# fn=fn%mot
# file=os.path.join(base,fn)
@@ -325,7 +330,7 @@ class PBTuning:
f=np.load(file)
data=f['data']
meta=f['meta'].item()
tSec=meta['tSec']
tSec=float(meta['tSec'])
minFrq=meta['minFrq']
maxFrq=meta['maxFrq']
amp=meta['amp']
@@ -342,42 +347,68 @@ class PBTuning:
ftX=np.fft.rfft(c)
ftY=np.fft.rfft(o)
i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
f=np.arange(n+1)/tSec #Hz
f=f[i:j+1]
frq_=np.arange(n+1)/tSec #Hz
frq_=frq_[i:j+1]
ftX=ftX[i:j+1]
ftY=ftY[i:j+1]
ft=ftY/ftX
frq=np.concatenate((frq,f))
frq.append(frq_)
phase_=np.unwrap(np.angle(ft))
if phase_[0]>0:
phase_-=2*np.pi
phase=np.concatenate((phase,phase_))
mag=np.concatenate((mag,(np.abs(ftY)/np.abs(ftX))))
l.append(len(frq))
phase.append(phase_)
mag.append(np.abs(ftY)/np.abs(ftX))
db_mag=20*np.log10(mag)
phase=np.degrees(phase)# numpy.unwrap(p, discont=3.141592653589793, axis=-1)
numFrq=1000
fFrq= np.logspace(np.log10(frq[0][0]), np.log10(frq[-1][-1]),numFrq)
fdb_mag = np.zeros(fFrq.shape)
fdeg_phase = np.zeros(fFrq.shape)
fig = plt.figure()
fig.canvas.set_window_title('full bode of motor %d'%mot)
ax = fig.add_subplot(2, 1, 1)
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2)
plt.title('bode of motor %d'%mot)
for i in range(len(l)-1):
ax.semilogx(frq[l[i]:l[i+1]], db_mag[l[i]:l[i+1]],'-') # Bode magnitude plot
ax.yaxis.set_label_text('dB ampl')
ax.set_xlim(1,2000)
plt.grid(True)
#ax.loglog(frqLst, bode[:,0],'.-') # Bode magnitude plot
ax = fig.add_subplot(2, 1, 2)
for i in range(len(l)-1):
ax.semilogx(frq[l[i]:l[i+1]], phase[l[i]:l[i+1]],'-')#,zorder=i) # Bode phase plot
ax.yaxis.set_label_text('phase')
ax.xaxis.set_label_text('frequency [Hz]')
ax.set_xlim(1,2000)
ax.set_ylim(-360,0)
plt.grid(True)
for i in range(len(frq)):
db_mag = 20 * np.log10(mag[i])
deg_phase = np.degrees(phase[i]) # numpy.unwrap(p, discont=3.141592653589793, axis=-1)
if deg_phase[0]>0:
deg_phase-=360
ax1.semilogx(frq[i], db_mag,'-') # Bode magnitude plot
ax2.semilogx(frq[i], deg_phase, '-') # ,zorder=i) # Bode phase plot
#fill the final magnitude and phase
if i==0:
f=interpolate.interp1d(frq[i], db_mag,bounds_error=False)
fdb_mag=f(fFrq)
f=interpolate.interp1d(frq[i], deg_phase,bounds_error=False)
fdeg_phase=f(fFrq)
else:
print((frq[i][0],frq[i][-1]))
s=stats.binned_statistic(frq[i], db_mag,'mean',fFrq)[0]
b=~np.isnan(s); fdb_mag[:-1][b]=s[b] #[:-2][b] because the statistics has one less entry than the count of bins
s=stats.binned_statistic(frq[i], deg_phase,'mean',fFrq)[0]
b=~np.isnan(s); fdeg_phase[:-1][b]=s[b]
pass
ax1.semilogx(fFrq, fdb_mag,'y')
ax2.semilogx(fFrq, fdeg_phase, 'y')
#export bode plot fot matlab analysis
fn = os.path.join(base,'full_bode_mot%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict={'db_mag':fdb_mag,'deg_phase':fdeg_phase})
#scipy.io.savemat('/home/zamofing_t/afs/ESB-MX/data/' + fn + '.mat', mdict=f)
ax1.yaxis.set_label_text('dB ampl')
ax1.set_xlim(1,2000)
ax1.grid(True)
ax2.yaxis.set_label_text('phase')
ax2.xaxis.set_label_text('frequency [Hz]')
ax2.set_xlim(1,2000)
ax2.set_ylim(-360,0)
ax2.grid(True)
pass
def bode_model_plot(self, mot,base):
self.bode_full_plot(mot,base)
@@ -481,7 +512,7 @@ class PBTuning:
den=den1*den2*den3*den4*den5*denc
mdl= signal.lti(num, den) #num denum
bode(mdl)
w=np.logspace(0,3,1000)*2*np.pi
w=np.logspace(0,np.log10(2000),1000)*2*np.pi
w,mag,phase = signal.bode(mdl,w)
f=w/(2*np.pi)
ax=fig.axes[0]
@@ -490,8 +521,24 @@ class PBTuning:
ax.semilogx(f, phase,'-k',lw=2) # Bode phase plot
# tp print see also: print(np.poly1d([1,2,3], variable='s')), print(np.poly1d([1,2,3], r=True, variable='s'))
def bode_current(self,openloop=True,motor=1,magMove=1000,magPhase=500,dwell=10,file='/tmp/bode.npz'):
#currentstep 2 1000 500 10
#magPhase: set this current to move the stage at a stable position: vslue in bits
#magMove: set this current to measure the current transition: value in bits
#dwell: measurement time in ms.the time the current is set
# Amplifier specs (Power Brick LV User Manual.pdf p.19)
# 5A_rms continous current
# 15A_rms peak current
# 14 bit ADC resolution
# 2us PWM deadBand
# 33.85A Maximum ADC Current (corresponds to a DAC Value 32737 ==2^15)
data = self.do_command('currentstep', motor, magMove, magPhase, dwell)
def bode(mdl):
w,mag,phase = signal.bode(mdl)
w,mag,phase = signal.bode(mdl,1000)
f=w/(2*np.pi)
fig = plt.figure()
ax = fig.add_subplot(2, 1, 1)
@@ -626,6 +673,10 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
tune.bode_chirp(openloop=True,file=file[:-4]+'c.npz',motor=mot,amp=50,minFrq=300,maxFrq=1500,tSec=10)
tune.init_stage()
tune.bode_chirp(openloop=True,file=file[:-4]+'d.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10)
elif mode==10:
#for mot in (1,2):
tune.bode_current(motor=mot, magMove=1000, magPhase=500, dwell=10, file='/tmp/curr_step%d.npz'%mot)
print 'done'
plt.show()
#------------------ Main Code ----------------------------------
#ssh_test()

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)