From a47064de0a27cd6276a93b4f0051a61babf66c7a Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Tue, 21 Aug 2018 14:28:13 +0200 Subject: [PATCH] wip --- python/PBTuning.py | 276 ++++++++++++++++++++++++++++++--------------- 1 file changed, 186 insertions(+), 90 deletions(-) diff --git a/python/PBTuning.py b/python/PBTuning.py index f4649d7..30736be 100755 --- a/python/PBTuning.py +++ b/python/PBTuning.py @@ -28,6 +28,12 @@ other available tuning progs on the powerbrick are: user_gantry_crosscoupled.h usertrajectory +Modes: + 1: sine bode open loop + 2: chirp bode open loop + 3: sine bode closed loop + 4: chirp bode closed loop + TODO: use openloopsine to create a bode diagram of the 'strecke' ''' @@ -51,6 +57,7 @@ class PBTuning: args=self.args host=args.host cmd=('ssh','root@'+host, self.tuneDir+cmd)+tuple(map(str,param)) + print(' '.join(cmd)) p=sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE) res=p.wait() print(res) @@ -86,7 +93,10 @@ class PBTuning: col=('b-','g-') for i in (0,1): data=self.data[:,i] - data=(data-data[0])*w + avg=data.mean(0) + #data=data-data[0] + data=data-avg + data*=w ax1.plot(data, col[i]) f = np.fft.fft(data) ax2.plot(np.absolute(f) , col[i]) @@ -99,105 +109,155 @@ class PBTuning: plt.pause(.05) return (res[1,0]/res[0,0],res[1,1]-res[0,1]) - def bode_gather(self,motor=1,minFrq=1,maxFrq=300,numFrq= 150,amp=10,file='/tmp/bode.npz'): - #amp= percentage of maximum amplitude + def bode_sine(self,openloop=True,motor=1,minFrq=1,maxFrq=300,numFrq=150,amp=10,file='/tmp/bode.npz'): + if os.path.isfile(file): + f=np.load(file) + bode=f['bode'] + meta=f['meta'].item() + meta['file']=file + else: + #motor 1 maximum: 13750 + #amp= percentage of maximum amplitude + fig = plt.figure() + ax = fig.add_subplot(1, 1, 1) + frqLst=np.logspace(np.log10(minFrq),np.log10(maxFrq),numFrq) + n=len(frqLst) + #frqLst=(10,15,20,25,30) + bode=np.ndarray((n,3)) + bode[:, 0]=frqLst + #for i in range(n): + for i in range(n-1,-1,-1): + frq=frqLst[i] + t=1 + rep=max(1,frq*t) + self.fnLoc='/tmp/gather%d.txt'%i + if openloop: + data=self.do_command('openloopsine',motor,amp,frq,rep,0) + else: + data=self.do_command('sinusoidal',motor,amp,frq,rep,0) + data=data[:,(1,2)] + ax.clear() + avg=data.mean(0) + print(avg) + ax.plot(data[:, 0]-avg[0] , 'b-', label='input') + ax.plot(data[:, 1]-avg[1], 'g-', label='output') + plt.pause(.05) + bode[i,1:]=self.phase_amp(frq, rep) + print('frq %g ampl %g phase %g'%tuple(bode[i,:])) + plt.draw_all() + + meta={'motor':motor,'date':time.asctime()} + np.savez_compressed(file, bode=bode, meta=meta) + meta['file']=file + self.bode_sine_plot(bode, meta) + + def bode_sine_plot(self,bode,meta): + frq=bode[:,0] + db_mag=20*np.log10(bode[:,1]) + phase=np.degrees(np.unwrap(bode[:,2]))# numpy.unwrap(p, discont=3.141592653589793, axis=-1) + if phase[0]>0: phase-=360 fig = plt.figure() - ax = fig.add_subplot(1, 1, 1) - frqLst=np.logspace(np.log10(minFrq),np.log10(maxFrq),numFrq) - n=len(frqLst) - #frqLst=(10,15,20,25,30) - bode=np.ndarray((n,3)) - bode[:, 0]=frqLst - for i in range(n): - frq=frqLst[i] - t=1 - rep=max(1,frq*t) - self.fnLoc='/tmp/gather%d.txt'%i - data=self.do_command('openloopsine',motor,amp,frq,rep,0) - ax.clear() - ax.plot(data[:, 0]-data[0, 0] , 'b-', label='input') - ax.plot(data[:, 1]-data[0, 1], 'g-', label='output') - plt.pause(.05) - bode[i,1:]=self.phase_amp(frq, rep) - print('frq %g ampl %g phase %g'%tuple(bode[i,:])) - plt.draw_all() + fig.canvas.set_window_title(os.path.basename(meta['file'])) + 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') + plt.grid(True) - meta={'motor':motor,'date':time.asctime()} - np.savez_compressed(file, bode=bode, meta=meta) + def bode_chirp(self,openloop=True,motor=1,minFrq=10,maxFrq=150,tSec=30.,amp=10,mode=1,file='/tmp/gather.npz'): + #amp= percentage of maximum amplitud + if os.path.isfile(file): + f=np.load(file) + data=f['data'] + #try: + meta=f['meta'].item() + #except KeyError: + # meta={'motor': motor, 'date': time.asctime(), 'minFrq': 10, 'maxFrq': 150, 'tSec': 30, 'amp': amp} + # np.savez_compressed(file, data=data, meta=meta) + meta['file']=file + else: + mode=1 + if openloop: + data=self.do_command('openloopchirp',motor,amp,minFrq,maxFrq,tSec*1000,mode,0) + else: + #sinesweep, chirpmove? + data=self.do_command('sinesweep', motor, amp, minFrq, maxFrq, tSec*1000, mode, 0) + data = data[:, (1, 2)] - def bode_gather2(self,motor=1,minFrq=5,maxFrq=150,tSec=15,amp=30,mode=1,file='/tmp/bode.npz'): - #amp= percentage of maximum amplitude - self.fnLoc='/tmp/gather.txt' - #self.fnLoc='/tmp/gather70.txt' - mode=1 - data=self.do_command('openloopchirp',motor,amp,minFrq,maxFrq,tSec*1000,mode,0) - self.data = data= np.genfromtxt(self.fnLoc, delimiter=' ') - n = self.data.shape[0] - w=np.hamming(n) + 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) + + def bode_chirp_plot(self, data, meta): + tSec=meta['tSec'] + minFrq=meta['minFrq'] + maxFrq=meta['maxFrq'] - #data[:,0]*=w - #data[:,1]=(data[:, 1]-data[0, 1])*w - #data[:, 1]-=np.convolve(data[:,1],np.ones(n),'same')/n 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 - fig = plt.figure() - ax = fig.add_subplot(1, 1, 1) - ax.plot(data[:, 0]) + #5..150 Hz + #15 sec + c = data[:,0] + o=data[:,1] + o-=o[0] + t = np.linspace(0, tSec, data.shape[0], endpoint=True) + n=data.shape[0]/2 + fig=plt.figure() + fig.canvas.set_window_title(os.path.basename(meta['file']+' raw')) + plt.plot(t, c) + plt.plot(t, o) + #bode_plot(o) + #plt.show() - ax.plot(data[:, 1]) - #d=np.concatenate((np.ones(n)*data[0, 1],data[:, 1],np.ones(n)*data[-1, 1])) + #n samples + #t1 seconds + #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] + #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) + ft=ftY/ftX + phase=np.angle(ft) + phase=np.degrees(np.unwrap(phase[i:j+1])) - fig = plt.figure() - iFt = np.fft.fft(data[:, 0]) - oFt= np.fft.fft(data[:, 1]) - iPhase = np.angle(iFt) - iAmpl = np.absolute(iFt) - oPhase = np.angle(oFt) - oAmpl = np.absolute(oFt) - ax = fig.add_subplot(2, 1, 1) - ax.plot(iAmpl) - ax.plot(oAmpl) - ax = fig.add_subplot(2, 1, 2) - ax.plot(iPhase) - ax.plot(oPhase) + #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=np.abs(ftY)/np.abs(ftX) + ax=plt.subplot(2,1,1) + ax.semilogx(w,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) - frq=np.arange(0,iAmpl.shape[0]) - #bode=np.ndarray((n,3)) - db_mag=20*np.log10(oAmpl-iAmpl) - phase=np.unwrap(oPhase-iPhase)# numpy.unwrap(p, discont=3.141592653589793, axis=-1) - fig = plt.figure() - ax = fig.add_subplot(2, 1, 1) - ax.semilogx(frq, db_mag,'.-') # Bode magnitude plot - ax.xaxis.set_label_text('dB Mag.') - #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') - plt.show() + ax.semilogx(w,phase,'b') # Bode phase plot + ax.yaxis.set_label_text('Amplitude [dB]') + #ax.set_ylim(phase[i],phase[j]) + ax.grid(True) + ax.xaxis.set_label_text('Frequency [Hz]') + ax.yaxis.set_label_text('Phase [degree]') + #plt.axvline(x=0.22058956) + #plt.show() #meta={'motor':motor,'date':time.asctime()} #np.savez_compressed(file, bode=bode, meta=meta) - def bode_plot(self,file): - f=np.load(file) - bode=f['bode'] - meta=f['meta'] - frq=bode[:,0] - db_mag=20*np.log10(bode[:,1]) - phase=np.unwrap(bode[:,2])# numpy.unwrap(p, discont=3.141592653589793, axis=-1) - fig = plt.figure() - ax = fig.add_subplot(2, 1, 1) - ax.semilogx(frq, db_mag,'.-') # Bode magnitude plot - ax.xaxis.set_label_text('dB Mag.') - #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') - plt.show() if __name__=='__main__': @@ -214,20 +274,56 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' parser=ArgumentParser(epilog=epilog,formatter_class=RawDescriptionHelpFormatter) - parser.add_argument('--host', help='hostname', metavar='HOST', required=True) + parser.add_argument('--plot', nargs='*') + parser.add_argument('--host', help='hostname', metavar='HOST') parser.add_argument('--verbose', '-v', type=int, help='verbosity bits (see below)', default=0) 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='') + args=parser.parse_args() #plt.ion() tune=PBTuning(args) - base='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/' - for i in (1,):#(1,2): - file=os.path.join(base,'bode%d.npz'%i) - #tune.bode_gather(file=file,motor=i) - tune.bode_gather2(file=file,motor=i) - tune.bode_plot(file) + base='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/PBTuning/' + + if args.plot: + #display bode plots + for fn in args.plot: + if os.path.basename(fn).startswith('sine_ol_mot'): + tune.bode_sine(openloop=True,file=fn) + if os.path.basename(fn).startswith('chirp_ol_mot'): + tune.bode_chirp(openloop=True,file=fn) + if os.path.basename(fn).startswith('sine_cl_mot'): + tune.bode_sine(openloop=False, file=fn) + if os.path.basename(fn).startswith('chirp_cl_mot'): + tune.bode_chirp(openloop=False, file=fn) + plt.show() + 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)) + #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)) + tune.bode_chirp(openloop=True,file=file,motor=mot,maxFrq=500) + elif mode==3: + ol=True + file=os.path.join(base,'sine_cl_mot%d_%s.npz'%(mot,tag)) + 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)) + 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()