sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

full openloop bode plot

Browse Source
This commit is contained in:
zamofing_t
2018-08-23 12:36:12 +02:00
parent a47064de0a
commit 9f980a8f2d
1 changed files with 161 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
python/PBTuning.py
View File

@@ -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