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

towards stage model

Browse Source
This commit is contained in:
zamofing_t
2018-09-17 16:14:50 +02:00
parent 9f980a8f2d
commit 0e306f41f2
1 changed files with 174 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

197
python/PBTuning.py
View File

@@ -46,6 +46,7 @@ import matplotlib.pyplot as plt
import subprocess as sprc
import telnetlib
from scipy.signal.waveforms import chirp
from scipy import signal
from utilities import *
class PBTuning:
@@ -287,18 +288,22 @@ class PBTuning:
#np.savez_compressed(file, bode=bode, meta=meta)
def bode_full_plot(self, mot,base):
fn='sine_ol_mot%d_ext.npz'%mot
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=bode[:,2]
phase=np.unwrap(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)
l=[0,len(frq)]
#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()
@@ -326,25 +331,156 @@ class PBTuning:
ft=ftY/ftX
frq=np.concatenate((frq,f))
phase=np.concatenate((phase,np.angle(ft)))
#if amp==50:amp*=np.sqrt(2)
phase_=np.unwrap(np.angle(ft))
if phase_[0]>0:
phase_-=2*np.pi
phase=np.concatenate((phase,phase_))
mag=np.concatenate((mag,(np.abs(ftY)/np.abs(ftX))))
l.append(len(frq))
db_mag=20*np.log10(mag)
phase=np.degrees(np.unwrap(phase))# numpy.unwrap(p, discont=3.141592653589793, axis=-1)
phase=np.degrees(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.title('bode of motor %d'%mot)
for i in range(len(l)-1):
ax.semilogx(frq[l[i]:l[i+1]], db_mag[l[i]:l[i+1]],'-') # Bode magnitude plot
ax.yaxis.set_label_text('dB ampl')
ax.set_xlim(1,2000)
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')
for i in range(len(l)-1):
ax.semilogx(frq[l[i]:l[i+1]], phase[l[i]:l[i+1]],'-')#,zorder=i) # Bode phase plot
ax.yaxis.set_label_text('phase')
ax.xaxis.set_label_text('frequency [Hz]')
ax.set_xlim(1,2000)
ax.set_ylim(-360,0)
plt.grid(True)
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([191,198])
d2=np.array([.05,.05])#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,63.8])
d2=np.array([.06,.1])#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([130,141])
d3=np.array([.05,.07])#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([405,415])
d4=np.array([.02,.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)
#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*numc
den=den1*den2*den3*den4*denc
mdl= signal.lti(num, den) #num denum
bode(mdl)
w=np.logspace(0,3,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)
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
@@ -366,7 +502,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
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='')
parser.add_argument('--dir', help='dir', default='')
args=parser.parse_args()
@@ -396,6 +532,11 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#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')
@@ -405,11 +546,16 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
if args.plot:
#display bode plots
if args.plot[0]=='ext':
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)
@@ -423,27 +569,32 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
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))
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_%s.npz'%(mot,tag))
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:
ol=True
file=os.path.join(base,'sine_cl_mot%d_%s.npz'%(mot,tag))
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_%s.npz'%(mot,tag))
file=os.path.join(base,'chirp_cl_mot%d.npz'%(mot))
tune.bode_chirp(openloop=False,file=file,motor=mot)
elif mode==100: #testing
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]+'TEST.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10)
#tune.do_command('stepmove',1,100,500,0,0)
plt.show()
#------------------ Main Code ----------------------------------
@@ -452,12 +603,12 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
exit(ret)
#enable plc1
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --tag ext
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --dir tmp
#enable plc1
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --tag ext
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --tag tmp
#enable plc1
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --tag ext
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --tag tmp
#enable plc1
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --tag ext
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --tag tmp
#enable plc1
#./PBTuning.py --host SAR-CPPM-EXPMX1 --plot ext