452 lines
16 KiB
Python
Executable File
452 lines
16 KiB
Python
Executable File
#!/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
|