diff --git a/Readme.md b/Readme.md index c205b0f..739f6c9 100644 --- a/Readme.md +++ b/Readme.md @@ -1046,9 +1046,45 @@ cpx ;linear abs; X-13.68 Y-8.03 Z-1483.1 B0.35 z=((1.73,.93,2.129),(2.13,.03,1.103))): -&1;cpx abs linear;jog1=557;jog3=0;jog4=468;jog5=1730 +&1;cpx abs linear;jog1=557;jog3=0;jog4=-468;jog5=-1730 -&1;cpx abs linear;jog1=-8;jog3=240000;jog4=2129;jog5=1.73 +&1;cpx abs linear;jog1=-8;jog3=240000;jog4=-351;jog5=-1103 caQtDM -macro "NAME=ESB-MX-CAM,CAMNAME=ESB-MX-CAM" /sf/controls/config/qt/Camera/CameraMiniView + +#1,3,4,5p +point 1 0,120,240 deg +575.5 0 -241.5 -1401.3 +575.5 120000 96.7 -1401.7 +575.5 240000 -53.8 -1802.4 + +point 2 0,120,240 deg +175.5 0 -162.3 -1802.5 +175.5 120000 -293.2 -1303.7 +175.5 240000 246.4 -1402.25 + + +#1j=575.5; #3j=0 ; #4j=-241.5; #5j=-1401.3 +#1j=575.5; #3j=120000; #4j= 96.7; #5j=-1401.7 +#1j=575.5; #3j=240000; #4j= -53.8; #5j=-1802.4 +#1j=175.5; #3j=0 ; #4j=-162.3; #5j=-1802.5 +#1j=175.5; #3j=120000; #4j=-293.2; #5j=-1303.7 +#1j=175.5; #3j=240000; #4j= 246.4; #5j=-1402.25 + + +pixel center 1110 1090 + +#1j=575.5; #3j=0 ; #4j=-241.5; #5j=-1401.3 +cpx pmatch + + +-1:fwd_inp(0) -241.5 -1401.3 0 575.5 +-1:fwd_res -88.9829 308.514 0 575.5 + + +cpx ;linear abs; X0 Z0 Y575 B0 +cpx ;linear abs; X0 Z0 Y575 B100000 + + +TODO: CLEANUP CODE OF HELICALSCAN, REMOVE LOGGING (comment out) diff --git a/python/MXTuning.py b/python/MXTuning.py new file mode 100755 index 0000000..9797b0a --- /dev/null +++ b/python/MXTuning.py @@ -0,0 +1,303 @@ +#!/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 +from utilities import * + +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() + 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([197,199]) + d2=np.array([.02,.02])#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,61.8]) + d2=np.array([.05,.055])#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([138,151]) + d3=np.array([.04,.03])#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([410,417]) + d4=np.array([.015,.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) + + f5=np.array([228,230]) + d5=np.array([.03,.03])#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 + 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*num5*numc + den=den1*den2*den3*den4*den5*denc + mdl= signal.lti(num, den) #num denum + 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=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,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) + 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) + 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, 600,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 diff --git a/python/PBTuning.py b/python/PBTuning.py deleted file mode 100755 index 1ad7bf2..0000000 --- a/python/PBTuning.py +++ /dev/null @@ -1,697 +0,0 @@ -#!/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 deltatau: -For now it does 10 step move and uploads the data -other available tuning progs on the powerbrick are: - analyzerautotunemove - autotunecalc - autotunemove - chirpmove - currentautotunecalc - currentstep - filtercalculation - openloopchirp + - openloopsine + - openlooptestmove - othertrajectory - parabolicmove - randommove - sinesweep + - sinusoidal + - stepmove - 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 - 5: full bode open loop record of both motors (including init_stage - 10: bode of current step -> does not work because gathering phase data not implemented - -> 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 -TODO: -use openloopsine to create a bode diagram of the 'strecke' -''' - -import os, sys, json, time -import numpy as np -import matplotlib as mpl -#mpl.use('GTKAgg') -import matplotlib.pyplot as plt -import subprocess as sprc -import telnetlib -from scipy.signal.waveforms import chirp -from scipy import signal -from scipy import interpolate -from scipy import stats -from utilities import * - -class PBTuning: - tuneDir='/opt/ppmac/tune/' - - def __init__(self,args): - self.args=args - - def do_command(self,cmd,*param): - 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) - print(p.stderr.read()) - print(p.stdout.read()) - PBGatherPlot='/home/zamofing_t/scripts/PBGatherPlot' - try: - fnLoc=self.fnLoc - except AttributeError: - fnLoc = '/tmp/gather.txt' - cmd=(PBGatherPlot,'-m24','-v255','--host',host,'--dat',fnLoc) - #p = sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE) - p = sprc.Popen(cmd, shell=False) - retval = p.wait() - print(p.stderr.read()) - #print(p.stdout.read()) - - #self.meta = {'timebase': ServoPeriod*gather['Period']} - self.data = np.genfromtxt(fnLoc, delimiter=' ') - return self.data - - def init_stage(self): - args=self.args - host=args.host - gpasciiCommander='/home/zamofing_t/scripts/gpasciiCommander' - cmd=(gpasciiCommander ,'--host',host,'--cmd','enable plc1') - p = sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE) - retval = p.wait() - print(p.stderr.read());print(p.stdout.read()) - time.sleep(10) - #cmd=(gpasciiCommander ,'--host',host,'--cmd','#1..2j=10000') - #p = sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE) - #retval = p.wait() - #print(p.stderr.read());print(p.stdout.read()) - #time.sleep(5) - - - - def phase_amp(self,frq,rep): - try: - ax1=self.ax1 - ax2=self.ax2 - ax1.clear() - ax2.clear() - except AttributeError: - fig = plt.figure(); self.ax1=ax1 = fig.add_subplot(1, 1, 1) - fig = plt.figure(); self.ax2=ax2 = fig.add_subplot(1, 1, 1) - n = self.data.shape[0] - w=np.hamming(n) - res=np.ndarray((2,2)) - col=('b-','g-') - for i in (0,1): - data=self.data[:,i] - 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]) - idx = int(rep) - bias = np.absolute(f[0] / n) - phase = np.angle(f[idx]) - #ampl = np.absolute(f[idx]) * 2 / n - ampl = np.absolute(f[idx]) * 2 / w.sum() - res[i,:]=(ampl,phase) - plt.pause(.05) - return (res[1,0]/res[0,0],res[1,1]-res[0,1]) - - 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() - 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) - - 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 - #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/2.,maxFrq/2.,tSec*2000,mode,0) - else: - 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,openloop) - - def bode_chirp_plot(self, data, meta,openloop): - tSec=meta['tSec'] - minFrq=meta['minFrq'] - maxFrq=meta['maxFrq'] - - #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') - - #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() - fig.canvas.set_window_title(os.path.basename(meta['file']+' raw')) - plt.plot(t, c) - plt.plot(t, o) - #bode_plot(o) - #plt.show() - - #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) - 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)) - - #magDb=10*np.log10(np.abs(ft)) #in decibel - mag=np.abs(ftY)/np.abs(ftX) - 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(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(f,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_full_plot(self, mot,base): - 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=[np.unwrap(bode[:,2]),] - - #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() - tSec=float(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]) - frq_=np.arange(n+1)/tSec #Hz - frq_=frq_[i:j+1] - ftX=ftX[i:j+1] - ftY=ftY[i:j+1] - - ft=ftY/ftX - frq.append(frq_) - phase_=np.unwrap(np.angle(ft)) - phase.append(phase_) - mag.append(np.abs(ftY)/np.abs(ftX)) - - numFrq=1000 - fFrq= np.logspace(np.log10(frq[0][0]), np.log10(frq[-1][-1]),numFrq) - fdb_mag = np.zeros(fFrq.shape) - fdeg_phase = np.zeros(fFrq.shape) - - fig = plt.figure() - fig.canvas.set_window_title('full bode of motor %d'%mot) - ax1 = fig.add_subplot(2, 1, 1) - ax2 = fig.add_subplot(2, 1, 2) - plt.title('bode of motor %d'%mot) - for i in range(len(frq)): - db_mag = 20 * np.log10(mag[i]) - deg_phase = np.degrees(phase[i]) # numpy.unwrap(p, discont=3.141592653589793, axis=-1) - if deg_phase[0]>0: - deg_phase-=360 - - - ax1.semilogx(frq[i], db_mag,'-') # Bode magnitude plot - ax2.semilogx(frq[i], deg_phase, '-') # ,zorder=i) # Bode phase plot - - #fill the final magnitude and phase - if i==0: - f=interpolate.interp1d(frq[i], db_mag,bounds_error=False) - fdb_mag=f(fFrq) - f=interpolate.interp1d(frq[i], deg_phase,bounds_error=False) - fdeg_phase=f(fFrq) - else: - print((frq[i][0],frq[i][-1])) - s=stats.binned_statistic(frq[i], db_mag,'mean',fFrq)[0] - b=~np.isnan(s); fdb_mag[:-1][b]=s[b] #[:-2][b] because the statistics has one less entry than the count of bins - s=stats.binned_statistic(frq[i], deg_phase,'mean',fFrq)[0] - b=~np.isnan(s); fdeg_phase[:-1][b]=s[b] - pass - - ax1.semilogx(fFrq, fdb_mag,'y') - ax2.semilogx(fFrq, fdeg_phase, 'y') - #export bode plot fot matlab analysis - fn = os.path.join(base,'full_bode_mot%d.mat'%mot) - import scipy.io - scipy.io.savemat(fn, mdict={'db_mag':fdb_mag,'deg_phase':fdeg_phase}) - #scipy.io.savemat('/home/zamofing_t/afs/ESB-MX/data/' + fn + '.mat', mdict=f) - - ax1.yaxis.set_label_text('dB ampl') - ax1.set_xlim(1,2000) - ax1.grid(True) - ax2.yaxis.set_label_text('phase') - ax2.xaxis.set_label_text('frequency [Hz]') - ax2.set_xlim(1,2000) - ax2.set_ylim(-360,0) - ax2.grid(True) - pass - - 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([197,199]) - d2=np.array([.02,.02])#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,61.8]) - d2=np.array([.05,.055])#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([138,151]) - d3=np.array([.04,.03])#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([410,417]) - d4=np.array([.015,.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) - - f5=np.array([228,230]) - d5=np.array([.03,.03])#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 - 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*num5*numc - den=den1*den2*den3*den4*den5*denc - mdl= signal.lti(num, den) #num denum - 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=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_current(self,openloop=True,motor=1,magMove=1000,magPhase=500,dwell=10,file='/tmp/bode.npz'): - #currentstep 2 1000 500 10 - #magPhase: set this current to move the stage at a stable position: vslue in bits - #magMove: set this current to measure the current transition: value in bits - #dwell: measurement time in ms.the time the current is set - - # Amplifier specs (Power Brick LV User Manual.pdf p.19) - # 5A_rms continous current - # 15A_rms peak current - # 14 bit ADC resolution - # 2us PWM deadBand - # 33.85A Maximum ADC Current (corresponds to a DAC Value 32737 ==2^15) - - data = self.do_command('currentstep', motor, magMove, magPhase, dwell) - - -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('--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('--dir', help='dir', default='') - - args=parser.parse_args() - - #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/' - 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') - #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]=='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) - 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 - mot=args.mot - if mode==1: - 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.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: - 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.npz'%(mot)) - tune.bode_chirp(openloop=False,file=file,motor=mot) - elif mode==5: #full recording open loop - for mot in (1,2): - tune.init_stage() - file=os.path.join(base,'sine_ol_mot%d.npz'%(mot)) - tune.bode_sine(openloop=True,file=file,motor=mot) - file=os.path.join(base,'chirp_ol_mot%d.npz'%(mot)) - tune.init_stage() - tune.bode_chirp(openloop=True,file=file[:-4]+'a.npz',motor=mot, minFrq=10, maxFrq=300, tSec=30) - tune.init_stage() - tune.bode_chirp(openloop=True,file=file[:-4]+'b.npz',motor=mot,amp=50,minFrq=100,maxFrq=500, tSec=30) - tune.init_stage() - tune.bode_chirp(openloop=True,file=file[:-4]+'c.npz',motor=mot,amp=50,minFrq=300,maxFrq=1500,tSec=10) - tune.init_stage() - tune.bode_chirp(openloop=True,file=file[:-4]+'d.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10) - elif mode==10: - #for mot in (1,2): - tune.bode_current(motor=mot, magMove=1000, magPhase=500, dwell=10, file='/tmp/curr_step%d.npz'%mot) - 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 diff --git a/python/helicalscan.py b/python/helicalscan.py index 4d34d4e..3e681e3 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -762,7 +762,7 @@ open forward DX=qCX-p0_x DZ=qCZ-p0_z Y=qFY - send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y + //send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y //P1001+=1 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 close @@ -783,10 +783,10 @@ open inverse define(p_x='L16', p_y='L17', p_z='L18') define(sclY='L19') define(scl='L20') - if(D0>0) - send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY - else - send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''') + //if(D0>0) + // send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY + //else + // send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''') for i in range(2): # https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i] @@ -821,10 +821,10 @@ open inverse vqW=vW//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x ''') prg.append(" vqW=vqW*%g"%(1000./d2r)) #scale from rad to 1000*deg - prg.append(''' send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY + prg.append('''// send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY } - else - send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY + //else + // send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY //P1002+=1 close ''') @@ -1193,8 +1193,8 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' #hs.gen_prog(mode=0,cntHor=3,cntVert=10,hRng=(-5,5),wRng=(0,120000)) #hs.gen_prog(mode=0,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000)) #hs.gen_prog(mode=1,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000),smt=0,pt2pt_time=300) - #hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=300) - hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=40) + hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=300) + #hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=40) #hs.gen_prog(mode=1,cntHor=3,cntVert=20,hRng=(-5,5),wRng=(0,1200),smt=0,pt2pt_time=200) #hs.gen_prog(mode=1, cntHor=2, cntVert=2, wRng=(0, 360000), smt=0) #hs.gen_prog(mode=1)