From d589cf8aed18578422e2bca53521dac334a98e22 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Wed, 26 Sep 2018 16:43:34 +0200 Subject: [PATCH] helicalscan commissioned at ESB_MX! --- Readme.md | 37 +++++++++++++ python/PBTuning.py | 123 +++++++++++++++++++++++++++++------------- python/helicalscan.py | 112 ++++++++++++++++++++++++++++---------- 3 files changed, 207 insertions(+), 65 deletions(-) diff --git a/Readme.md b/Readme.md index 260d889..c205b0f 100644 --- a/Readme.md +++ b/Readme.md @@ -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 diff --git a/python/PBTuning.py b/python/PBTuning.py index 7bc7dc2..1ad7bf2 100755 --- a/python/PBTuning.py +++ b/python/PBTuning.py @@ -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() diff --git a/python/helicalscan.py b/python/helicalscan.py index ed803f3..4d34d4e 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -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)