#!/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]) #reiner integrator: 30Hz=0dB -> k=30*2*pi=180 #num1=np.poly1d([120*120]) #den1 = np.poly1d([1,0,0]) #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 custom_chirp(self): motor = 1 amp, minFrq, maxFrq, tSec = (10, 10, 300, 30) file='/tmp/gather.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) prog = ''' &0 //cout works only in coord 0 open prog 999 L11=0 L10=0 L12=0 L13=0 L14=0 Gather.Enable=2 while(L10<300005) { L12=10*sin(31.415926535897931*(pow(1.058324104020218,(L10*0.000199996614513))-1)/log(1.058324104020218)) cout%d:(L12) L10=L10+1 } Gather.Enable=0 close b999r '''%motor gpascii = self.comm.gpascii gt = self.gather print(gpascii.servo_period) gt.set_phasemode(False) address=("Motor[1].IqCmd", "Motor[1].ActPos",) gt.set_address(*address) #Gather.Enable=1 gt.set_property(MaxSamples=300000, Period=1) # gt.enable(2) gpascii.send_line(prog) gpascii.sync() gt.wait_stopped() self.data=data=gt.upload() meta={'motor':motor,'date':time.asctime(),'minFrq':minFrq,'maxFrq':maxFrq,'tSec':tSec,'amp':amp,'address':address} np.savez_compressed(file, data=data, meta=meta) meta['file'] = file self.bode_chirp_plot(data, meta,True) pass def bode_sine(self,openloop=True,motor=1,minFrq=1,maxFrq=20,numFrq=15,amp=10,file='/tmp/gather.npz'): '''calculates phase and amplitude at different frequencies and saves:#loads and plots the bode diagram''' if False:# os.path.isfile(file): f=np.load(file) bode=f['bode'] meta=f['meta'].item() meta['file']=file else: gpascii=self.comm.gpascii #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) 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)] gpascii.send_line('#1j=0') time.sleep(1) 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.show(block=False);plt.pause(.05) 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(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 import logging logger = logging.getLogger(__name__) logger = logging.getLogger('pbtools.misc.pp_comm') logger.setLevel(logging.DEBUG) logging.basicConfig(format=('%(asctime)s %(name)-12s ' '%(levelname)-8s %(message)s'), datefmt='%m-%d %H:%M', ) 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') elif mode==7: #further tests tune.init_stage(); plt.close('all') tune.bode_sine() tune.custom_chirp() 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