diff --git a/python/PBTuning.py b/python/PBTuning.py index 30736be..c603907 100755 --- a/python/PBTuning.py +++ b/python/PBTuning.py @@ -16,14 +16,14 @@ other available tuning progs on the powerbrick are: currentautotunecalc currentstep filtercalculation - openloopchirp - openloopsine + openloopchirp + + openloopsine + openlooptestmove othertrajectory parabolicmove randommove - sinesweep - sinusoidal + sinesweep + + sinusoidal + stepmove user_gantry_crosscoupled.h usertrajectory @@ -45,6 +45,7 @@ import matplotlib as mpl import matplotlib.pyplot as plt import subprocess as sprc import telnetlib +from scipy.signal.waveforms import chirp from utilities import * class PBTuning: @@ -181,33 +182,56 @@ class PBTuning: meta['file']=file else: mode=1 + #Sys.ServoPeriod=0.2ms = 5kHz -> therefore factor 2000 for the time second->servoCnt + #don't know why the frequency must be scaled... if openloop: - data=self.do_command('openloopchirp',motor,amp,minFrq,maxFrq,tSec*1000,mode,0) + data=self.do_command('openloopchirp',motor,amp,minFrq/2.,maxFrq/2.,tSec*2000,mode,0) else: - #sinesweep, chirpmove? - data=self.do_command('sinesweep', motor, amp, minFrq, maxFrq, tSec*1000, mode, 0) + data=self.do_command('sinesweep', motor, amp, minFrq/2., maxFrq/2., tSec*2000, mode, 0) data = data[:, (1, 2)] meta={'motor':motor,'date':time.asctime(),'minFrq':minFrq,'maxFrq':maxFrq,'tSec':tSec,'amp':amp} np.savez_compressed(file, data=data, meta=meta) meta['file'] = file - self.bode_chirp_plot(data, meta) + self.bode_chirp_plot(data, meta,openloop) - def bode_chirp_plot(self, data, meta): + def bode_chirp_plot(self, data, meta,openloop): tSec=meta['tSec'] minFrq=meta['minFrq'] maxFrq=meta['maxFrq'] - n=1000 - d=np.concatenate((np.ones(n-1)*data[0, 1],data[:, 1])) - d=np.convolve(d,np.ones(n),'valid')/n - data[:, 1]-=d + #n=25000 + #t=np.linspace(0, tSec, 2*n+1, endpoint=True) + #c=chirp(t, f0=minFrq, f1=maxFrq, t1=tSec, phi=-90, method='logarithmic') - #5..150 Hz - #15 sec - c = data[:,0] - o=data[:,1] - o-=o[0] + #plt.figure() + #plt.clf() + #plt.subplot(2, 1, 1) + #plt.plot(t, c) + #tstr="Logarithmic Chirp, f(0)=%g, f(%g)=%g"%(minFrq, tSec, maxFrq) + #plt.title(tstr) + + #plt.subplot(2, 1, 2) + #plt.plot(t, minFrq*(maxFrq/minFrq)**(t/tSec), 'r') + ## yscale('log') + #plt.grid(True) + #plt.ylabel('Frequency (Hz)') + #plt.xlabel('time (sec)') + + if openloop: + n=1000 + d=np.concatenate((np.ones(n-1)*data[0, 1],data[:, 1])) + d=np.convolve(d,np.ones(n),'valid')/n + data[:, 1]-=d + # 5..150 Hz + # 15 sec + c = data[:, 0] + o = data[:, 1] + o -= o[0] + else: + data=data-data[0,:] + c = data[:,0] + o=data[:,1] t = np.linspace(0, tSec, data.shape[0], endpoint=True) n=data.shape[0]/2 fig=plt.figure() @@ -222,30 +246,34 @@ class PBTuning: #ts=t1/(2*n)#samplingperiode =t[1]-t[0] #w_k=2*pi*k/T #w=2*np.pi*np.arange(n+1)/t1*(2*n) - i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j]) - w=np.arange(n+1)/tSec #Hz - w=w[i:j+1] + f=np.arange(n+1)/tSec #Hz #w*=2.*np.pi #rad/sec fig=plt.figure() fig.canvas.set_window_title(os.path.basename(meta['file']+' bode')) ftX=np.fft.rfft(c) ftY=np.fft.rfft(o) + + i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j]) + f=f[i:j+1] + ftX=ftX[i:j+1] + ftY=ftY[i:j+1] + ft=ftY/ftX phase=np.angle(ft) - phase=np.degrees(np.unwrap(phase[i:j+1])) + phase=np.degrees(np.unwrap(phase)) #magDb=10*np.log10(np.abs(ft)) #in decibel mag=np.abs(ftY)/np.abs(ftX) - magDb=20*np.log10(mag[i:j+1]) #in decibel (20=10*2: factor 2 because rfft only half) + magDb=20*np.log10(mag) #in decibel (20=10*2: factor 2 because rfft only half) #magDb=np.abs(ftY)/np.abs(ftX) ax=plt.subplot(2,1,1) - ax.semilogx(w,magDb,'b') # Bode magnitude plot + ax.semilogx(f,magDb,'b') # Bode magnitude plot #ax.plot(w,magDb) # Bode magnitude plot ax.axvline(minFrq,c='k');ax.axvline(maxFrq,c='k') ax.grid(True) ax=plt.subplot(2,1,2) - ax.semilogx(w,phase,'b') # Bode phase plot + ax.semilogx(f,phase,'b') # Bode phase plot ax.yaxis.set_label_text('Amplitude [dB]') #ax.set_ylim(phase[i],phase[j]) @@ -258,6 +286,64 @@ class PBTuning: #meta={'motor':motor,'date':time.asctime()} #np.savez_compressed(file, bode=bode, meta=meta) + def bode_full_plot(self, mot,base): + fn='sine_ol_mot%d_ext.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] + + 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) + f=np.load(file) + data=f['data'] + meta=f['meta'].item() + tSec=meta['tSec'] + minFrq=meta['minFrq'] + maxFrq=meta['maxFrq'] + amp=meta['amp'] + n=1000 + d=np.concatenate((np.ones(n-1)*data[0, 1],data[:, 1])) + d=np.convolve(d,np.ones(n),'valid')/n + data[:, 1]-=d + # 5..150 Hz + # 15 sec + c = data[:, 0] + o = data[:, 1] + o -= o[0] + n=data.shape[0]/2 + 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] + ftX=ftX[i:j+1] + ftY=ftY[i:j+1] + + ft=ftY/ftX + frq=np.concatenate((frq,f)) + phase=np.concatenate((phase,np.angle(ft))) + #if amp==50:amp*=np.sqrt(2) + mag=np.concatenate((mag,(np.abs(ftY)/np.abs(ftX)))) + + db_mag=20*np.log10(mag) + phase=np.degrees(np.unwrap(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.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') + ax.set_ylim(-360,0) + plt.grid(True) if __name__=='__main__': @@ -286,10 +372,44 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' #plt.ion() tune=PBTuning(args) + #data=self.do_command('openloopsine',motor,amp,frq,rep,0) + # #data=self.do_command('openloopchirp',motor,amp,minFrq,maxFrq,tSec*1000,mode,0) + + #THIS IS A TEST TO DO 5 Hz during 10 seconds + #frq=5 + #frq2=frq*1.1 + #ts=10 #seconds + + #5 kHz Servo + #ds=tune.do_command('openloopsine', 2, 20, frq, frq*ts, 0) + #dc=tune.do_command('openloopchirp', 2, 20, frq/2., frq2/2., ts*2000, 1, 0) #THIS IS NOT 10 Seconds!!! + + #dc=tune.do_command('openloopchirp', 2, 10, 5, 6, 10*1000, 1, 0) #THIS IS NOT 10 Seconds!!! + #ds=ds-ds[0,:] + #dc=dc-dc[0,:] + #plt.figure() + #plt.plot(ds[:,0],'b') + #plt.plot(ds[:,1],'g') + #plt.figure() + #plt.plot(dc[:,0],'b') + #plt.plot(dc[:,1],'g') + #plt.show() + #return base='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/PBTuning/' + #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') + #tune.bode_chirp(openloop=True, file=file, motor=2, minFrq=20,maxFrq=200,tSec=30.) + #plt.show() + #return if args.plot: #display bode plots + if args.plot[0]=='ext': + tune.bode_full_plot(mot=1,base=base) + tune.bode_full_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) @@ -312,7 +432,11 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' 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)) - tune.bode_chirp(openloop=True,file=file,motor=mot,maxFrq=500) + #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) elif mode==3: ol=True file=os.path.join(base,'sine_cl_mot%d_%s.npz'%(mot,tag)) @@ -321,10 +445,19 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' file=os.path.join(base,'chirp_cl_mot%d_%s.npz'%(mot,tag)) tune.bode_chirp(openloop=False,file=file,motor=mot) #tune.do_command('stepmove',1,100,500,0,0) - - plt.show() #------------------ Main Code ---------------------------------- #ssh_test() ret=parse_args() exit(ret) + +#enable plc1 +#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --tag ext +#enable plc1 +#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --tag ext +#enable plc1 +#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --tag ext +#enable plc1 +#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --tag ext +#enable plc1 +#./PBTuning.py --host SAR-CPPM-EXPMX1 --plot ext