diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m index 32ad10c..55dc568 100644 --- a/matlab/identifyFxFyStage.m +++ b/matlab/identifyFxFyStage.m @@ -22,6 +22,10 @@ function [mot1,mot2]=identifyFxFyStage() obj.phase=f.deg_phase*pi/180; %phase in rad response = obj.mag.*exp(1j*obj.phase); obj.meas= idfrd(response,obj.w,0); + + fMdl=load(strcat(path,sprintf('model%d.mat',motid))); + obj.mdl=fMdl; + end function tfc=currstep(obj) @@ -36,7 +40,11 @@ function [mot1,mot2]=identifyFxFyStage() [y,t]=step(tfc,t); f=figure(); subplot(1,2,1); - plot(t,y*1000,'r',t,obj.currstep.OutputData(11:210),'b'); + plot(t*1000,obj.currstep.OutputData(11:210),'b',t*1000,y*1000,'r'); + xlabel('ms') + ylabel('curr\_bits') + grid on + legend('real signal','model','Location','southeast') title(s); subplot(1,2,2); h=bodeplot(tfc,'r'); @@ -52,26 +60,6 @@ function [mot1,mot2]=identifyFxFyStage() s=sprintf('k:%g w0:%g damp:%g\n',k,w0,damp); end - function tf=fyModel() - num=[ 2.36527033e+11, 1.17108082e+13, 3.62387303e+17]; - den=[ 1.00000000e+00, 6.64495206e+03, 2.12777376e+07, ... - 1.23728427e+10, 3.07054470e+13, 1.72592127e+15, ... - 5.39888656e+17]; - - tf=idtf(num,den); - end - - function tf=fxModel() - num=[ 1.23284092e+11, 4.14791803e+13, 1.18702926e+18, ... - 2.96296718e+20, 2.67179357e+24, 4.04662786e+26, ... - 1.59131515e+30, 1.02778572e+32, 1.64551888e+35]; - den=[ 1.00000000e+00, 6.93892369e+03, 3.17041055e+07, ... - 7.66104262e+10, 2.36504992e+14, 2.23054854e+17, ... - 5.12578678e+20, 2.04416512e+23, 3.27771400e+26, ... - 4.77145416e+28, 3.85452959e+31, 1.28911178e+33, ... - 9.52157664e+34]; - tf=idtf(num,den); - end function mot=fyStage() mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1); @@ -84,12 +72,26 @@ function [mot1,mot2]=identifyFxFyStage() figure(); mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0)); - mot.tf_mdl= fyModel(); + mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den); + + g11=tf(mot.mdl.numc,mot.mdl.denc); % iqCmd->iqMeas + g12=tf([1 0],mot.mdl.denc*12); % iqCmd->iqVolts : iqVolts= i_meas*R+i_meas'*L + num=conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.numc); + den=conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.denc); + g13=tf(num,den); %iqCmd->ActPos + %sys=ss([g11;g12]) + sys=ss([g11;g12;g13]) + + %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g'); h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w); setoptions(h,'FreqUnits','Hz','Grid','on'); end + function y=myNorm(y) + %normalizes num and den by factor 1000 + %y.*10.^(3*(length(y):-1:1)) + end function mot=fxStage() mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',2); @@ -103,7 +105,65 @@ function [mot1,mot2]=identifyFxFyStage() figure(); mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0)); mot.tf13_9 = tfest(mot.meas, 13, 9, opt); - mot.tf_mdl = fxModel(); + mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den); + + %create ss from tf MIMO: + %https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system + %Gspm=[tf_iqCmd_actVolts;tf_iqCmd_iqMeas;tf_iqCmd_actPos]; + %sys=ss(Gspm); + + %normalize: [1E6 1E3 1].*mot.mdl.denc + numc=myNorm(mot.mdl.numc); + denc=myNorm(mot.mdl.denc); + num1=myNorm(mot.mdl.num1); + den1=myNorm(mot.mdl.den1); + num2=myNorm(mot.mdl.num2); + den2=myNorm(mot.mdl.den2); + num3=myNorm(mot.mdl.num3); + den3=myNorm(mot.mdl.den3); + num4=myNorm(mot.mdl.num4); + den4=myNorm(mot.mdl.den4); + num5=myNorm(mot.mdl.num5); + den5=myNorm(mot.mdl.den5); + num=myNorm(mot.mdl.num); + den=myNorm(mot.mdl.den); + + %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600 + %tf2ss MIMO + %https://ch.mathworks.com/help/control/ref/append.html + g1=tf(numc,denc); % iqCmd->iqMeas + s1=ss(g1); + s1.C=[s1.C; 1/s1.B(1) 0]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L + tf(s1) % display all transfer functions + + num=conv(conv(conv(conv(num1,num2),num3),num4),num5); + den=conv(conv(conv(conv(den1,den2),den3),den4),den5); + g2=tf(num,den); %iqMeas->ActPos + s2=ss(g2); + + s3=append(s1,s2); + tf(s3) + %connect iqMeas from s1 to iqMeas of s2 + s3.A(3,1)=1 %WHAT NUMBER ??? s3.B(3,2), s3.C2,:)? + %remove the direct iqMeas input + %s3.B(3,2)=0 + + t_=tf(s3) + t_(3,1) + figure; + bode(t_(3,1)) + %compare with tf iqCmd->ActPos + num=conv(conv(conv(conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.num3),mot.mdl.num4),mot.mdl.num5),mot.mdl.numc); + den=conv(conv(conv(conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.den3),mot.mdl.den4),mot.mdl.den5),mot.mdl.denc); + g4=tf(num,den); %iqCmd->ActPos + figure; + bode(g4) + + %sys=ss([g11;g12]) + sys=ss([g11;g12;g13]) + sys=ss(g13) + + %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g',mot.tf13_9,'m',mot.tf_py,'b'); h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w); setoptions(h,'FreqUnits','Hz','Grid','on'); @@ -115,106 +175,3 @@ function [mot1,mot2]=identifyFxFyStage() mot2=fxStage(); end - - -function f=SCRATCH() - [m1,m2]=identifyFxFyStage(); - controlSystemDesigner(1,m2.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited - - %identification toolbox - systemIdentification - - - - - - %opt=tfestOptions('Display','off'); - %opt=tfestOptions('Display','on','initializeMethod','svf'); - %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[]); - %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[1,5;20,570]); - %tf1 = tfest(mot1frq, 6, 4, opt); - % Model refinement - % Options = tf1.Report.OptionsUsed; - % Options.WeightingFilter = 'prediction'; - % tf1_1 = pem(mot1frq, tf1, Options) - - - - - - bodeplot(mot1frq,tf1) - mag,phase=bode(tf1,frq) - figure(1) - subplot(211) - - - bodeplot(tf1) - - Opt = n4sidOptions('N4Horizon',[15 15 15]); - n4s3 = n4sid(mot1frq, 3, Opt) - - - - - - %tf([1 2],[1 0 10]) - %specifies the transfer function (s+2)/(s^2+10) while - sys=tf([1],[1,0,0]) - bode(sys) - step(sys) - - sys=tf([1],[1,-1,2]) %instable - - sys=tf([1],[1,1,2]) %stable - - - %0dB at 12 Hz=12*2*pi rad/s =75.4=k^2 -> k=8.6833 - - sys=tf([10],[1,0,0]) - %1/s^2 -> 0dB at 1Hz -40dB/decade - %10=+20dB - - - sys=tf([1],[1,0,2]) %not damped constant sine after step - - - sys=zpk([],[1,0,0],100) %stable - - sys=zpk([],[-10,-10],100) - - - %parker stage 1 - %!encoder_sim(enc=1,tbl=9,mot=9,posSf=13000./2048) - %!encoder_inc(enc=1,tbl=1,mot=1,posSf=13000./650000) - %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000) - %!motor(mot=1,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index') - Ts=2E-4 % discrete sample time (servo period) - Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000 - Kp=25,Kvfb=0,Ki=0,Kvff=0,Kaff=0,MaxInt=0 - - - num=7.32 - den=[5.995e-04 4.897e-02 1.] - open('stage_closed_loop.slx') - %sim('stage_closed_loop.slx') - - sys=tf(num,den) - bode(sys) - G = tf(1.5,[1 14 40.02]); - controlSystemDesigner('bode',sys); - controlSystemDesigner - linearSystemAnalyzer - load ltiexamples - linearSystemAnalyzer(sys_dc) - - controlSystemDesigner('bode',sys); - controlSystemDesigner(1,sys); % <<<<<<<<< This opens a transferfiûnction that can be edited - - num=[8.32795069e-11, 1.04317228e-08, 6.68431323e-05, 3.31861324e-03, 7.32824533e+00]; - den=[5.26156641e-18, 1.12897840e-14, 7.67853031e-12, 1.03201301e-08, 2.05154780e-06, 1.34279894e-03, 7.19229912e-02, 1.00000000e+00]; - mot2=tf(num,den); - controlSystemDesigner('bode',mot2); - - - -end \ No newline at end of file diff --git a/matlab/libESB_MX.slx b/matlab/libESB_MX.slx new file mode 100644 index 0000000..3ea9eb4 Binary files /dev/null and b/matlab/libESB_MX.slx differ diff --git a/matlab/simFxFyStage.m b/matlab/simFxFyStage.m new file mode 100644 index 0000000..1af6fd5 --- /dev/null +++ b/matlab/simFxFyStage.m @@ -0,0 +1,39 @@ +function out=simFxFyStage() + global Kp Kvfb Ki Kvff Kaff MaxInt + global m1 m2 mot_num mot_den Ts MaxDac MaxPosErr + global A B C D + function ServoDeltaTau_z(motid) + Ts=2E-4; % 0.2ms=5kHz + MaxDac=2011.968; + MaxPosErr=10000; + if motid==1 + %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000) + %!motor(mot=1,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index') + Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000; + mot_num=m1.tf_mdl.Numerator; + mot_den=m1.tf_mdl.Denominator; + else + %!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000) + %!motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index') + Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000; + mot_num=m2.tf_mdl.Numerator; + mot_den=m2.tf_mdl.Denominator; + end + end + mdlName='stage_closed_loop'; + open(mdlName) + ServoDeltaTau_z(2) + [A,B,C,D]=tf2ss(mot_num,mot_den); + + %mdlWks=get_param(mdlName,'ModelWorkspace') + %whos global + %whos(mdlWks) + %assignin(mdlWks,'Ts',1234) + %getVariable(mdlWks,'Ts') + % in global space call: + %global Kp Kvfb Ki Kvff Kaff MaxInt + %global m1 m2 mot_num mot_den Ts MaxDac MaxPosErr + %[m1,m2]=identifyFxFyStage(); + + % +end diff --git a/matlab/stage_closed_loop.slx b/matlab/stage_closed_loop.slx new file mode 100644 index 0000000..e11bbe1 Binary files /dev/null and b/matlab/stage_closed_loop.slx differ diff --git a/python/MXTuning.py b/python/MXTuning.py index b06e3b7..27c368b 100755 --- a/python/MXTuning.py +++ b/python/MXTuning.py @@ -104,6 +104,13 @@ class MXTuning(Tuning): 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) @@ -171,6 +178,13 @@ class MXTuning(Tuning): 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) diff --git a/python/MXTuningDoc/fastStageTune.odt b/python/MXTuningDoc/fastStageTune.odt index 97c55fa..3ed3df5 100644 Binary files a/python/MXTuningDoc/fastStageTune.odt and b/python/MXTuningDoc/fastStageTune.odt differ