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

major renaming of transfer functions

Browse Source
This commit is contained in:
zamofing_t
2019-02-15 10:46:29 +01:00
parent f3c5fafe5f
commit e792d0b028
11 changed files with 592 additions and 580 deletions
Download Patch File Download Diff File Expand all files Collapse all files

225
python/MXTuning.py
View File

@@ -95,31 +95,82 @@ class MXTuning(Tuning):
# Hz -> kHz
# rad/s -> rad/ms
if mot==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])
#simplified current loop 1st order approx
wd=2*np.pi*400. #45deg at 360Hz
Td=1/wd
numd = np.poly1d([1.])
dend = np.poly1d([Td,1])
#force(idMeas)->position
#identify matlab: k:0.671226 w0:134.705 damp:0.191004
mag1=6 #10**(db_mag1/20)
db_mag1=20*np.log10(mag1)#dB
w1=50.*_N #rad/sec
f1=w1/2/np.pi; # ca. 6.5Hz
T1=1/w1
d1=0.6 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
num1=np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1])
magp=6 #10**(db_mag1/20)
db_magp=20*np.log10(magp)#dB
wp=50.*_N #rad/sec
fp=wp/2/np.pi; # ca. 6.5Hz
Tp=1/wp
dp=0.6 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
nump=np.poly1d([magp])
denp = np.poly1d([Tp**2,2*Tp*dp,1])
#force->position (simplified)
#reiner integrator: 19.8Hz=0dB -> k=frq*2*pi=180
numq=np.poly1d([(19.8*2*np.pi)**2])
denq=np.poly1d([1,0,0])
#reiner integrator: 19.8Hz=0dB -> k=30*2*pi=180
num0=np.poly1d([(19.8*2*np.pi)**2])
den0=np.poly1d([1,0,0])
#force->velocity (simplified)
#reiner integrator: 19.8Hz=0dB -> k=frq*2*pi
numv=np.poly1d([19.8*2*np.pi])
denv=np.poly1d([1,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])
f1=np.array([197,199])
d1=np.array([.02,.02])#daempfung
w1=f1*2*np.pi*_N #rad/sec
T1=1/w1
num1 = np.poly1d([T1[0]**2,2*T1[0]*d1[0],1])
den1 = np.poly1d([T1[1]**2,2*T1[1]*d1[1],1])
num=numc*nump*num1#*num3
den=denc*denp*den1#*den3
mdl= signal.lti(num, den) #num denum
#print(num);print(den);print(mdl)
w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
mdlcp= signal.lti(numc*nump, denc*denp)
mdldp= signal.lti(numd*nump, dend*denp)
mdlp= signal.lti(nump, denp)
mdlq= signal.lti(numq, denq)
bode(((mdl,'plant'),
(mdlcp,'mdl_cp'),
(mdldp,'mdl_dp'),
(mdlp,'mdl_p'),
(mdlq,'mdl_q'),
),w)
d={'num':num.coeffs,'numc':numc.coeffs,'numd':numd.coeffs,'nump':nump.coeffs,'numq':numq.coeffs,'numv':numv.coeffs,'num1':num1.coeffs,
'den':den.coeffs,'denc':denc.coeffs,'dend':dend.coeffs,'denp':denp.coeffs,'denq':denq.coeffs,'denv':denv.coeffs,'den1':den1.coeffs}
fn=os.path.join(self.baseDir,'model%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict=d)
print('save to matlab file:'+fn)
elif mot==2:
#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
@@ -139,52 +190,39 @@ class MXTuning(Tuning):
dend = np.poly1d([Td,1])
num=num1*num2*numc#*num3
den=den1*den2*denc#*den3
mdl= signal.lti(num, den) #num denum
#print(num);print(den);print(mdl)
w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
mdl1c= signal.lti(num1*numc, den1*denc)
mdl1d= signal.lti(num1*numd, den1*dend)
mdl1= signal.lti(num1, den1)
mdl0= signal.lti(num0, den0)
bode(((mdl,'plant'),
(mdl1c,'mdl1c'),
(mdl1d,'mdl1d'),
(mdl1,'mdl1'),
(mdl0,'mdl0'),
),w)
d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'numc':numc.coeffs,'num0':num0.coeffs,'numd':numd.coeffs,
'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'denc':denc.coeffs,'den0':den0.coeffs,'dend':dend.coeffs}
fn=os.path.join(self.baseDir,'model%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict=d)
print('save to matlab file:'+fn)
elif mot==2:
#force(idMeas)->position
#identify matlab: k:1.7282 w0:51.069 damp:0.327613
mag1=12. #10**(db_mag1/20)
db_mag1=20*np.log10(mag1)#dB
w1=21.*_N #rad/sec
f1=w1/2/np.pi; # ca. 6.5Hz
T1=1/w1
d1=0.4 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
num1=np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1])
magp=12. #10**(db_mag1/20)
db_magp=20*np.log10(magp)#dB
wp=21.*_N #rad/sec
fp=wp/2/np.pi; # ca. 6.5Hz
Tp=1/wp
dp=0.4 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
nump=np.poly1d([magp])
denp = np.poly1d([Tp**2,2*Tp*dp,1])
#reiner integrator: 11.84Hz=0dB -> k=30*2*pi=180
num0=np.poly1d([(11.84*2*np.pi)**2])
den0=np.poly1d([1,0,0])
numq=np.poly1d([(11.84*2*np.pi)**2])
denq=np.poly1d([1,0,0])
#force->velocity (simplified)
#reiner integrator: 19.8Hz=0dB -> k=frq*2*pi
numv=np.poly1d([19.8*2*np.pi])
denv=np.poly1d([1,0])
#resonance frequency
f2=np.array([55.,61.])
d2=np.array([.2,.2])#daempfung
f1=np.array([55.,61.])
d1=np.array([.2,.2])#daempfung
w1=f1*2*np.pi #rad/sec
T1=1/w1
num1 = np.poly1d([T1[0]**2,2*T1[0]*d1[0],1])
den1 = np.poly1d([T1[1]**2,2*T1[1]*d1[1],1])
#resonance frequency
f2=np.array([128,137])
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])
@@ -192,8 +230,8 @@ class MXTuning(Tuning):
#resonance frequency
f3=np.array([128,137])
d3=np.array([.05,.05])#daempfung
f3=np.array([410,417])
d3=np.array([.015,.015])#daempfung
w3=f3*2*np.pi #rad/sec
T3=1/w3
num3 = np.poly1d([T3[0]**2,2*T3[0]*d3[0],1])
@@ -201,64 +239,36 @@ class MXTuning(Tuning):
#resonance frequency
f4=np.array([410,417])
d4=np.array([.015,.015])#daempfung
f4=np.array([230,233])
d4=np.array([.04,.04])#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])
#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])
#simplified current loop 1st order approx
wd=2*np.pi*400. #45deg at 360Hz
Td=1/wd
numd = np.poly1d([1.])
dend = np.poly1d([Td,1])
num=num1*num2*num3*num4*num5*numc
den=den1*den2*den3*den4*den5*denc
num=numc*nump*num1*num2*num3*num4
den=denc*denp*den1*den2*den3*den4
mdl= signal.lti(num, den) #num denum
#print(num);print(den);print(mdl)
w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
mdl1c= signal.lti(num1*numc, den1*denc)
mdl1d= signal.lti(num1*numd, den1*dend)
mdl1= signal.lti(num1, den1)
mdl0= signal.lti(num0, den0)
mdlcp= signal.lti(numc*nump, denc*denp)
mdldp= signal.lti(numd*nump, dend*denp)
mdlp= signal.lti(nump, denp)
mdlq= signal.lti(numq, denq)
bode(((mdl,'plant'),
(mdl1c,'mdl1c'),
(mdl1d,'mdl1d'),
(mdl1,'mdl1'),
(mdl0,'mdl0'),
(mdlcp,'mdlcp'),
(mdldp,'mdldp'),
(mdlp,'mdlp'),
(mdlq,'mdlq'),
),w)
d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,'num5':num5.coeffs,'numc':numc.coeffs,'num0':num0.coeffs,'numd':numd.coeffs,
'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs,'den5':den5.coeffs,'denc':denc.coeffs,'den0':den0.coeffs,'dend':dend.coeffs}
d={'num':num.coeffs,'numc':numc.coeffs,'numd':numd.coeffs,'nump':nump.coeffs,'numq':numq.coeffs,'numv':numv.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,
'den':den.coeffs,'denc':denc.coeffs,'dend':dend.coeffs,'denp':denp.coeffs,'denq':denq.coeffs,'denv':denv.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs}
fn=os.path.join(self.baseDir,'model%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict=d)
@@ -357,7 +367,16 @@ class MXTuning(Tuning):
prog+=''' //set output
iqCmd={u}*{V}-{y};
//return iqCmd;
pshm->P[200{motid}]=iqCmd;
pshm->P[20{motid}0]=DesPos;
pshm->P[20{motid}1]=IqMeas;
pshm->P[20{motid}2]=ActVel;
pshm->P[20{motid}3]=ActPos;
pshm->P[20{motid}4]=_x[0];
pshm->P[20{motid}5]=_x[1];
pshm->P[20{motid}6]=_x[2];
pshm->P[20{motid}7]=_x[3];
pshm->P[20{motid}8]=iqCmd;
return pshm->ServoCtrl(Mptr);
}}
else
@@ -723,7 +742,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
bm+=32 #amplitude linear (not dB)
self.bode_plot(data, mode=bm, kwargs=meta)
plt.show(block=False)
save_figs(fn)
#save_figs(fn)
#wait_key()
raw_input('press return')

2
python/usr_code/Makefile
View File

@@ -112,7 +112,7 @@ tags:
Debug: all
../../matlab/ssc%.mat: ../../matlab/StateSpaceControlDesign.m ../../matlab/identifyFxFyStage.m
../../matlab/ssc%.mat: ../../matlab/StateSpaceControlDesign.m ../../matlab/identifyFxFyStage.m Makefile
cd ../../matlab; /afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab -nodisplay -nojvm -nosplash -nodesktop -r \
"clear;clear global;close all;\
mot=identifyFxFyStage(7);\