From 0e306f41f220542d9dc3eabee831d23b9aa81f31 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Mon, 17 Sep 2018 16:14:50 +0200 Subject: [PATCH] towards stage model --- python/PBTuning.py | 197 +++++++++++++++++++++++++++++++++++++++------ 1 file changed, 174 insertions(+), 23 deletions(-) diff --git a/python/PBTuning.py b/python/PBTuning.py index c603907..3e58a85 100755 --- a/python/PBTuning.py +++ b/python/PBTuning.py @@ -46,6 +46,7 @@ import matplotlib.pyplot as plt import subprocess as sprc import telnetlib from scipy.signal.waveforms import chirp +from scipy import signal from utilities import * class PBTuning: @@ -287,18 +288,22 @@ class PBTuning: #np.savez_compressed(file, bode=bode, meta=meta) def bode_full_plot(self, mot,base): - fn='sine_ol_mot%d_ext.npz'%mot + import glob + fn='sine_ol_mot%d.npz'%mot file=os.path.join(base,fn) f=np.load(file) bode=f['bode'] meta=f['meta'].item() frq=bode[:,0] mag=bode[:,1] - phase=bode[:,2] + phase=np.unwrap(bode[:,2]) - for fn in ('chirp_ol_mot%d_exta.npz','chirp_ol_mot%d_extb.npz','chirp_ol_mot%d_extc.npz'): - fn=fn%mot - file=os.path.join(base,fn) + 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) + for file in sorted(glob.glob(os.path.join(base,'chirp_ol_mot%d*'%mot))): + print(os.path.basename(file)) f=np.load(file) data=f['data'] meta=f['meta'].item() @@ -326,25 +331,156 @@ class PBTuning: ft=ftY/ftX frq=np.concatenate((frq,f)) - phase=np.concatenate((phase,np.angle(ft))) - #if amp==50:amp*=np.sqrt(2) + 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)) + db_mag=20*np.log10(mag) - phase=np.degrees(np.unwrap(phase))# numpy.unwrap(p, discont=3.141592653589793, axis=-1) + phase=np.degrees(phase)# numpy.unwrap(p, discont=3.141592653589793, axis=-1) + fig = plt.figure() fig.canvas.set_window_title('full bode of motor %d'%mot) ax = fig.add_subplot(2, 1, 1) - ax.semilogx(frq, db_mag,'-') # Bode magnitude plot - ax.xaxis.set_label_text('dB Mag.') + 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) - ax.semilogx(frq, phase,'-') # Bode phase plot - ax.xaxis.set_label_text('phase') + 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) + def bode_model_plot(self, mot,base): + self.bode_full_plot(mot,base) + fig=plt.gcf() + if mot==1: + db_mag1=17.3 #dB + mag1=10**(db_mag1/20) + f1=6.5 #Hz + w1=f1*2*np.pi #rad/sec + T1=1/w1 + d1=.7 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 + num1=np.poly1d([mag1]) + den1 = np.poly1d([T1**2,2*T1*d1,1]) + + #first resonance frequency + f2=np.array([191,198]) + d2=np.array([.05,.05])#daempfung + w2=f2*2*np.pi #rad/sec + T2=1/w2 + num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1]) + den2 = np.poly1d([T2[1]**2,2*T2[1]*d2[1],1]) + mdl= signal.lti(num2, den2) #num denum + #bode(mdl) + + + #current loop 2nd order approx + f4=900. + d4=1 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 + w4=f4*2*np.pi #rad/sec + T4=1/w4 + num4 = np.poly1d([1.]) + den4 = np.poly1d([T4**2,2*T4*d4,1]) + #mdl= signal.lti(num4, den4) #num denum + #bode(mdl) + + num=num1*num2*num4#*num3 + den=den1*den2*den4#*den3 + mdl= signal.lti(num, den) #num denum + print num,den + print mdl + elif mot==2: + # basic 1/s^2 system with damping an d resonance + db_mag1=17.3 #dB + mag1=10**(db_mag1/20) + f1=4.5 #Hz + w1=f1*2*np.pi #rad/sec + d1=.3 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 + T1=1/w1 + num1 = np.poly1d([mag1]) + den1 = np.poly1d([T1**2,2*T1*d1,1]) + + #first resonance frequency + f2=np.array([57.8,63.8]) + d2=np.array([.06,.1])#daempfung + w2=f2*2*np.pi #rad/sec + T2=1/w2 + num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1]) + den2 = np.poly1d([T2[1]**2,2*T2[1]*d2[1],1]) + mdl= signal.lti(num2, den2) #num denum + #bode(mdl) + + #second resonance frequency + f3=np.array([130,141]) + d3=np.array([.05,.07])#daempfung + w3=f3*2*np.pi #rad/sec + T3=1/w3 + num3 = np.poly1d([T3[0]**2,2*T3[0]*d3[0],1]) + den3 = np.poly1d([T3[1]**2,2*T3[1]*d3[1],1]) + #mdl= signal.lti(num3, den3) #num denum + #bode(mdl) + + #second resonance frequency + f4=np.array([405,415]) + d4=np.array([.02,.02])#daempfung + w4=f4*2*np.pi #rad/sec + T4=1/w4 + num4 = np.poly1d([T4[0]**2,2*T4[0]*d4[0],1]) + den4 = np.poly1d([T4[1]**2,2*T4[1]*d4[1],1]) + #mdl= signal.lti(num3, den3) #num denum + #bode(mdl) + + + + #current loop 2nd order approx + fc=900. + dc=1 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 + wc=fc*2*np.pi #rad/sec + Tc=1/wc + numc = np.poly1d([1.]) + denc = np.poly1d([Tc**2,2*Tc*dc,1]) + #mdl= signal.lti(num4, den4) #num denum + #bode(mdl) + + num=num1*num2*num3*num4*numc + den=den1*den2*den3*den4*denc + mdl= signal.lti(num, den) #num denum + bode(mdl) + w=np.logspace(0,3,1000)*2*np.pi + w,mag,phase = signal.bode(mdl,w) + f=w/(2*np.pi) + ax=fig.axes[0] + ax.semilogx(f, mag,'-k',lw=2) # Bode magnitude plot + ax=fig.axes[1] + 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(mdl): + w,mag,phase = signal.bode(mdl) + f=w/(2*np.pi) + fig = plt.figure() + ax = fig.add_subplot(2, 1, 1) + ax.semilogx(f,mag,'-') # Bode magnitude plot + ax.yaxis.set_label_text('dB ampl') + plt.grid(True) + ax = fig.add_subplot(2, 1, 2) + ax.semilogx(f,phase,'-') # Bode magnitude plot + ax.yaxis.set_label_text('phase') + ax.xaxis.set_label_text('frequency [Hz]') + plt.grid(True) + #plt.show() + if __name__=='__main__': from argparse import ArgumentParser,RawDescriptionHelpFormatter @@ -366,7 +502,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' parser.add_argument('--dryrun', '-n', action='store_true', help='dryrun to stdout') parser.add_argument('--mode', '-m', type=int, help='modes (see below)', default=1) parser.add_argument('--mot', type=int, help='motor', default=1) - parser.add_argument('--tag', help='tag', default='') + parser.add_argument('--dir', help='dir', default='') args=parser.parse_args() @@ -396,6 +532,11 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' #plt.show() #return base='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/PBTuning/' + if args.dir is not None: + base=os.path.join(base,args.dir) + if not os.path.exists(base): + os.mkdir(base) + #file=os.path.join(base, 'sine_ol_mot_tst.npz') #tune.bode_sine(openloop=True,file=file,motor=2, minFrq=20,maxFrq=200,numFrq=30) #file=os.path.join(base, 'chirp_ol_mot_tst.npz') @@ -405,11 +546,16 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' if args.plot: #display bode plots - if args.plot[0]=='ext': + if args.plot[0]=='full': tune.bode_full_plot(mot=1,base=base) tune.bode_full_plot(mot=2,base=base) plt.show() return + if args.plot[0]=='model': + tune.bode_model_plot(mot=1,base=base) + tune.bode_model_plot(mot=2,base=base) + plt.show() + return for fn in args.plot: if os.path.basename(fn).startswith('sine_ol_mot'): tune.bode_sine(openloop=True,file=fn) @@ -423,27 +569,32 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' return mode=args.mode - tag=args.tag mot=args.mot if mode==1: ol=True - file=os.path.join(base,'sine_ol_mot%d_%s.npz'%(mot,tag)) + file=os.path.join(base,'sine_ol_mot%d.npz'%(mot)) #def bode_sine(self, openloop=True, motor=1, minFrq=1, maxFrq=300, numFrq=150, amp=10, file='/tmp/bode.npz'): tune.bode_sine(openloop=True,file=file,motor=mot) elif mode==2: - file=os.path.join(base,'chirp_ol_mot%d_%s.npz'%(mot,tag)) + file=os.path.join(base,'chirp_ol_mot%d.npz'%(mot)) #def bode_chirp(self,openloop=True,motor=1,minFrq=10,maxFrq=150,tSec=30.,amp=10,mode=1,file='/tmp/gather.npz'): #tune.bode_chirp(openloop=True,file=file,motor=mot, minFrq=10, maxFrq=300, tSec=30) tune.bode_chirp(openloop=True,file=file[:-4]+'a.npz',motor=mot, minFrq=10, maxFrq=300, tSec=30) tune.bode_chirp(openloop=True,file=file[:-4]+'b.npz',motor=mot,amp=50,minFrq=100,maxFrq=500, tSec=30) tune.bode_chirp(openloop=True,file=file[:-4]+'c.npz',motor=mot,amp=50,minFrq=300,maxFrq=1500,tSec=10) + tune.bode_chirp(openloop=True,file=file[:-4]+'d.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10) elif mode==3: ol=True - file=os.path.join(base,'sine_cl_mot%d_%s.npz'%(mot,tag)) + file=os.path.join(base,'sine_cl_mot%d.npz'%(mot)) tune.bode_sine(openloop=False,file=file,motor=mot) elif mode==4: - file=os.path.join(base,'chirp_cl_mot%d_%s.npz'%(mot,tag)) + file=os.path.join(base,'chirp_cl_mot%d.npz'%(mot)) tune.bode_chirp(openloop=False,file=file,motor=mot) + elif mode==100: #testing + file=os.path.join(base,'chirp_ol_mot%d.npz'%(mot)) + #def bode_chirp(self,openloop=True,motor=1,minFrq=10,maxFrq=150,tSec=30.,amp=10,mode=1,file='/tmp/gather.npz'): + #tune.bode_chirp(openloop=True,file=file,motor=mot, minFrq=10, maxFrq=300, tSec=30) + tune.bode_chirp(openloop=True,file=file[:-4]+'TEST.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10) #tune.do_command('stepmove',1,100,500,0,0) plt.show() #------------------ Main Code ---------------------------------- @@ -452,12 +603,12 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' exit(ret) #enable plc1 -#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --tag ext +#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --dir tmp #enable plc1 -#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --tag ext +#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --tag tmp #enable plc1 -#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --tag ext +#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --tag tmp #enable plc1 -#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --tag ext +#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --tag tmp #enable plc1 #./PBTuning.py --host SAR-CPPM-EXPMX1 --plot ext