#!/usr/bin/env python # *-----------------------------------------------------------------------* # | | # | Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch) | # | | # | Author Thierry Zamofing (thierry.zamofing@psi.ch) | # *-----------------------------------------------------------------------* ''' tuning functions for ESB-MX Modes: 1 full recording current step 2 full recording open loop 3 full recording closed loop 4 plot the full bode recording 5 plot the full bode recording with an approximation model 6 plot all raw acquired data files -> 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 ''' import os, sys, json, time import numpy as np import matplotlib as mpl #mpl.use('GTKAgg') import matplotlib.pyplot as plt from scipy import signal sys.path.insert(0,os.path.expanduser('~/Documents/prj/SwissFEL/PBTools/')) #import pbtools.misc.pp_comm as pp_comm -> pp_comm.PPComm from pbtools.misc.pp_comm import PPComm from pbtools.misc.gather import Gather from pbtools.misc.tuning import Tuning class MXTuning(Tuning): tuneDir='/opt/ppmac/tune/' def __init__(self,comm,gather): Tuning.__init__(self,comm,gather) def init_stage(self): comm=self.comm gpascii=comm.gpascii sys.stdout.write('homing stage');sys.stdout.flush() gpascii.send_line('enable plc1') time.sleep(.2) while True: act=gpascii.get_variable('Plc[1].Active',type_=int) if act==0: sys.stdout.write('\n') break sys.stdout.write('.');sys.stdout.flush() time.sleep(.2) #Coord[%d].ProgActive if a running proc def bode_model_plot(self, mot,base): self.bode_full_plot(mot,base) fig=plt.gcf() _N=1.#E-3 # normalization factor: -> moves 3 decades to right but has factors around 1 # s -> ms # Hz -> kHz # rad/s -> rad/ms if mot==1: #identify matlab: k:0.671226 w0:134.705 damp:0.191004 mag1=0.671226 #10**(db_mag1/20) db_mag1=20*np.log10(mag1)#dB w1=134.705*_N #rad/sec f1=w1/2/np.pi; # ca. 6.5Hz T1=1/w1 d1=0.19 # 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([197,199]) d2=np.array([.02,.02])#daempfung w2=f2*2*np.pi*_N #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 #identification with matlab: k=1, w0=8725, d=0.75 dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 wc=8725.*_N # rad/sec #...but phase lag seems to have earlier effect -> reduce wc wc*=.5 # rad/sec fc=wc/2/np.pi # ca 1388Hz Tc=1/wc numc = np.poly1d([1.]) denc = np.poly1d([Tc**2,2*Tc*dc,1]) num=num1*num2*numc#*num3 den=den1*den2*denc#*den3 mdl= signal.lti(num, den) #num denum print num print den print mdl d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'numc':numc.coeffs, 'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'denc':denc.coeffs} fn=os.path.join(base,'model%d.mat'%mot) import scipy.io scipy.io.savemat(fn, mdict=d) print('save to matlab file:'+fn) elif mot==2: #identify matlab: k:1.7282 w0:51.069 damp:0.327613 mag1=1.7282 #10**(db_mag1/20) db_mag1=20*np.log10(mag1)#dB w1=51.069*_N #rad/sec f1=w1/2/np.pi; # ca. 6.5Hz T1=1/w1 d1=0.32 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 num1=np.poly1d([mag1]) den1 = np.poly1d([T1**2,2*T1*d1,1]) #resonance frequency f2=np.array([57.8,61.8]) d2=np.array([.08,.095])#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) #resonance frequency f3=np.array([136,148]) d3=np.array([.05,.05])#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) #resonance frequency f4=np.array([410,417]) d4=np.array([.015,.015])#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) #resonance frequency f5=np.array([230,233]) d5=np.array([.04,.04])#daempfung w5=f5*2*np.pi #rad/sec T5=1/w5 num5 = np.poly1d([T5[0]**2,2*T5[0]*d5[0],1]) den5 = np.poly1d([T5[1]**2,2*T5[1]*d5[1],1]) #current loop 2nd order approx #identification with matlab: k=1, w0=8725, d=0.75 dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 wc=8725.*_N # rad/sec #...but phase lag seems to have earlier effect -> reduce wc wc*=.5 # rad/sec fc=wc/2/np.pi # ca 1388Hz Tc=1/wc numc = np.poly1d([1.]) denc = np.poly1d([Tc**2,2*Tc*dc,1]) num=num1*num2*num3*num4*num5*numc den=den1*den2*den3*den4*den5*denc mdl= signal.lti(num, den) #num denum print num print den print mdl d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,'num5':num5.coeffs,'numc':numc.coeffs, 'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs,'den5':den5.coeffs,'denc':denc.coeffs} fn=os.path.join(base,'model%d.mat'%mot) import scipy.io scipy.io.savemat(fn, mdict=d) print('save to matlab file:'+fn) bode(mdl) 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] ax.semilogx(f, mag,'-k',lw=1) # Bode magnitude plot ax=fig.axes[1] ax.semilogx(f, phase,'-k',lw=1) # 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,1000) 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 def parse_args(): 'main command line interpreter function' #usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg (h, t)=os.path.split(sys.argv[0]);cmd='\n '+(t if len(h)>3 else sys.argv[0])+' ' exampleCmd=('-n', '-v15' ) epilog=__doc__+''' Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' parser=ArgumentParser(epilog=epilog,formatter_class=RawDescriptionHelpFormatter) parser.add_argument('--host', help='hostname', metavar='HOST', default='SAR-CPPM-EXPMX1') parser.add_argument('--mode', '-m', type=int, help='modes (see below)', default=1) parser.add_argument('--dir', help='dir', default=None) args=parser.parse_args() #plt.ion() #comm = PPComm(host=args.host) #gt = Gather(comm) #tune=MXTuning(comm,gt) tune = MXTuning(None,None) base='MXTuning' if args.dir is not None: base=args.dir assert(os.path.exists(base)) #base=os.path.join(base,args.dir) #if not os.path.exists(base): # os.mkdir(base) mode=args.mode #plt.ion() if mode==1: # full recording current step homed=False for mot in (1, 2): fn=os.path.join(base, 'curr_step%d.npz' % mot) if not os.path.isfile(fn) and not homed: tune.init_stage();homed=True fig=plt.figure(mot) tune.bode_current(motor=mot, magMove=1000, magPhase=500, dwell=10, file=fn,fig=fig) plt.show(block=False) f=np.load(fn) fn=fn[:-3]+'mat' import scipy.io scipy.io.savemat(fn, mdict=f) print('save to matlab file:'+fn) elif mode==2: # full recording open loop for mot in (1, 2): fsin=os.path.join(base, 'sine_ol_mot%d.npz' % (mot)) fcrp=os.path.join(base, 'chirp_ol_mot%d' % (mot)) if not os.path.isfile(fsin): tune.init_stage();plt.close('all') tune.bode_sine(openloop=True, file=fsin, motor=mot) plt.show(block=False) for ext,amp,minFrq,maxFrq,tSec in (('a', 10, 10, 300,30), ('b', 50,100, 500,30), ('c', 50,300,1500,10), ('d',100,300,2000,10)): f=fcrp+ext+'.npz' if not os.path.isfile(f): tune.init_stage();plt.close('all') tune.bode_chirp(openloop=True, file=f, motor=mot, amp=amp, minFrq=minFrq, maxFrq=maxFrq, tSec=tSec) plt.show(block=False) elif mode==3: # full recording closed loop for mot in (1,2): #fsin=os.path.join(base, 'sine_cl_mot%d.npz' % (mot)) fcrp=os.path.join(base, 'chirp_cl_mot%d' % (mot)) #if not os.path.isfile(fsin): tune.init_stage();plt.close('all') #tune.bode_sine(openloop=False, file=fsin, motor=mot) #the generated program is prog 999 (only during acquisition) # Gather.MaxLines=174762 -> ca 15 sec. is max. #with higher frequency and amplitudes often is in DacLimit ! for ext,amp,minFrq,maxFrq,tSec in (('a',100, 1, 200,15), ('b', 20,10, 300,5), ('c', 5,10, 300,5), ('d', 1,10,1000,5), ): f=fcrp+ext+'.npz' if not os.path.isfile(f): tune.init_stage();plt.close('all') tune.bode_chirp(openloop=False, file=f, motor=mot, amp=amp, minFrq=minFrq, maxFrq=maxFrq, tSec=tSec) plt.show(block=False) elif mode==4: #plot the full bode recording tune.bode_full_plot(mot=1,base=base) tune.bode_full_plot(mot=2,base=base) elif mode==5: #plot the full bode recording with an approximation model tune.bode_model_plot(mot=1,base=base) tune.bode_model_plot(mot=2,base=base) elif mode == 6: # plot all raw acquired data files # 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) print 'done' plt.show() #------------------ Main Code ---------------------------------- #ssh_test() ret=parse_args() exit(ret) #enable plc1 #./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --dir tmp #./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --dir tmp #-> at low frequencied the speed is too high and encoder looses steps #enable plc1 #AFTER each chirp measurement do enable plc1 again! #./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --dir tmp #./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --dir tmp #./PBTuning.py --host SAR-CPPM-EXPMX1 --plot tmp