diff --git a/MXfastStageDoc/MXfastStage.tex b/MXfastStageDoc/MXfastStage.tex index baf513f..d3e9872 100644 --- a/MXfastStageDoc/MXfastStage.tex +++ b/MXfastStageDoc/MXfastStage.tex @@ -448,8 +448,7 @@ It has to be checked if the model matches the real stage. Therefore simulations To compare the measurements with the model following lines were executed in MATLAB \begin{verbatim} clear;clear global;close all; -mot=cell(2,1); -[mot{1},mot{2}]=identifyFxFyStage(); +mot=identifyFxFyStage(7); for k =1:2 [pb]=simFxFyStage(mot{k});sim('stage_closed_loop'); f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g'); @@ -537,10 +536,11 @@ As first approach the tf function is just converted to the ss space and the ss m The matlab models are:\\ \begin{tabular}{ll} -\texttt{ssPlt:} & best approach of the plant with mechanics, resonance, current loop etc.\\ -\texttt{ssMdl\_c1:} & model without resonance (only current and main mechanical)\\ -\texttt{ssMdl\_12:} & model without current loop, with one resonance (main mechanical + first resonance)\\ -\texttt{ssMdl\_1:} & model without current loop, no resonance (only main mechanical)\\ +\texttt{ss\_plt:} & best approach of the plant with mechanics, resonance, current loop etc.\\ +\texttt{ss\_c1:} & model without resonance (only current and main mechanical)\\ +\texttt{ss\_d1:} & model without resonance (simplified current and main mechanical)\\ +\texttt{ss\_1:} & model without current loop, no resonance (only main mechanical)\\ +\texttt{ss\_0:} & simplified mechanical, no current loop, no resonance\\ \end{tabular}\\ \vspace{1pc} @@ -549,9 +549,9 @@ Following code calculates parameters for a observer controller, does a simulatio \begin{verbatim} clear;clear global;close all; mot=cell(2,1); -[mot{1},mot{2}]=identifyFxFyStage(); +[mot{1},mot{2}]=identifyFxFyStage(7); for k =1:2 - [ssc]=StateSpaceControlDesign(mot1{k});sim('observer'); + [ssc]=StateSpaceControlDesign(mot{k});sim('observer'); f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g'); set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]); print(f,sprintf('figures/sim_cl_observer_%d',mot{k}.id),'-depsc'); diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m index b967bfd..275de65 100644 --- a/matlab/StateSpaceControlDesign.m +++ b/matlab/StateSpaceControlDesign.m @@ -16,18 +16,12 @@ function [ssc]=StateSpaceControlDesign(mot) % https://www.youtube.com/watch?v=Lax3etc837U %mPlt: mode to select plant - %0 real plant (model of real plant) - %1 current, mechanic, no resonance - %2 no current current, mechanic, no resonance - %3 no current current, mechanic, first resonance - - - %mMdl: mode to select model for observer - %0 real plant (NOT RECOMANDED, because not observab,econtrolable) - %1 current, mechanic, no resonance - %2 no current current, mechanic, no resonance - %3 no current current, mechanic, first resonance + %0 ss_plt :real plant (model of real plant) + %1 ss_c1 :current, mechanic, no resonance + %2 ss_d1 :simpl. current, mechanic, no resonance + %3 ss_1 :no current, mechanic, no resonance + %4 ss_0 :no current, simpl. mechanic, no resonance %mPrefilt:prefilter mode %0 no filter @@ -55,34 +49,38 @@ function [ssc]=StateSpaceControlDesign(mot) switch mPlt case 0 - ssPlt=mot.ssPlt;%real plant (model of real plant) + ss_plt=mot.ss_plt; case 1 - ssPlt=mot.ssMdl_c1;%current, mechanic, no resonance + ss_plt=mot.ss_c1; case 2 - ssPlt=mot.ssMdl_1;%no current current, mechanic, no resonance + ss_plt=mot.ss_d1; case 3 - ssPlt=mot.ssMdl_12;%no current current, mechanic, first resonance + ss_plt=mot.ss_1; + case 4 + ss_plt=mot.ss_0; end - ssPlt.Name='open loop plant'; + ss_plt.Name='open loop plant'; switch mMdl case 0 - ssMdl=mot.ssPlt;%real plant (model of real plant) + ss_mdl=mot.ss_plt; case 1 - ssMdl=mot.ssMdl_c1;%current, mechanic, no resonance + ss_mdl=mot.ss_c1; case 2 - ssMdl=mot.ssMdl_1;%no current current, mechanic, no resonance + ss_mdl=mot.ss_d1; case 3 - ssMdl=mot.ssMdl_12;%no current current, mechanic, first resonance + ss_mdl=mot.ss_1; + case 4 + ss_mdl=mot.ss_0; end - ssMdl.Name='open loop model'; %model for observer + ss_mdl.Name='open loop model'; %model for observer - [Ap,Bp,Cp,Dp]=ssdata(ssPlt); - [Am,Bm,Cm,Dm]=ssdata(ssMdl); + [Ap,Bp,Cp,Dp]=ssdata(ss_plt); + [Am,Bm,Cm,Dm]=ssdata(ss_mdl); if bitand(mShow,1) - figure();h=bodeplot(ssPlt,ssMdl); + figure();h=bodeplot(ss_plt,ss_mdl); setoptions(h,'IOGrouping','all') end @@ -93,8 +91,8 @@ function [ssc]=StateSpaceControlDesign(mot) % step answer on open loop: t = 0:1E-4:.5; u = ones(size(t)); - [yp,t,x] = lsim(ssPlt,u,t,xp0); - [ym,t,x] = lsim(ssMdl,u,t,xm0); + [yp,t,x] = lsim(ss_plt,u,t,xp0); + [ym,t,x] = lsim(ss_mdl,u,t,xm0); figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)'); legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos') end @@ -108,9 +106,9 @@ function [ssc]=StateSpaceControlDesign(mot) % %place poles for the controller feedback if use_lqr %use the lqr controller - Q=eye(length(ssMdl.A)); + Q=eye(length(ss_mdl.A)); R=1; - [K,P,E]=lqr(ssMdl,Q,R,0); + [K,P,E]=lqr(ss_mdl,Q,R,0); else if mot.id==1 %2500rad/s = 397Hz -> locate poles here @@ -155,12 +153,11 @@ function [ssc]=StateSpaceControlDesign(mot) end %if lqr V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) - %Nbar(2)=1; %the voltage stuff is crap for now if length(V)>1 V=V(3); % only the position scaling needed end - ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ssMdl.InputName,'OutputName',ssMdl.OutputName); + ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ss_mdl.InputName,'OutputName',ss_mdl.OutputName); if bitand(mShow,4) % step answer on closed loop with space state controller: t = 0:1E-4:.5; @@ -182,7 +179,7 @@ function [ssc]=StateSpaceControlDesign(mot) Ct = [ Cm zeros(size(Cm)) ]; Dt=0; - ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ssMdl.OutputName); + ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ss_mdl.OutputName); if bitand(mShow,8) % step answer on closed loop with observer controller: figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer'); @@ -233,8 +230,8 @@ function [ssc]=StateSpaceControlDesign(mot) end %discrete plant - ssPltz = c2d(ssPlt,Ts); - [Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz); + %ss_pltz = c2d(ss_plt,Ts); + %[Apz,Bpz,Cpz,Dpz]=ssdata(ss_pltz); %discrete observer controller ss_oz = c2d(ss_o,Ts); @@ -261,7 +258,9 @@ function [ssc]=StateSpaceControlDesign(mot) %state space controller ssc=struct(); - for k=["Ts","At","Bt","Ct","Dt","Atz","Btz","Ctz","Dtz","Ap","Bp","Cp","Dp","Am","Bm","Cm","Dm","Ao","Bo","Co","Do","Apz","Bpz","Cpz","Dpz","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"] + %for k=["Ts","At","Bt","Ct","Dt","Atz","Btz","Ctz","Dtz","Ap","Bp","Cp","Dp","Am","Bm","Cm","Dm","Ao","Bo","Co","Do","Apz","Bpz","Cpz","Dpz","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"] + %for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"] + for k=["Ts","ss_plt","ss_o","ss_oz","prefilt","prefiltz","V"] ssc=setfield(ssc,k,eval(k)); end save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc'); @@ -302,93 +301,3 @@ function pf=Prefilt(mot,mode) end -%code snipplets from an example on youtube (see reference at top) -function SCRATCH() -%import numpy as np -%fh=np.load('mode1.npz') -%import scipy.io -%scipy.io.savemat('mode1.mat',fh,do_compression=True) - -%matlab: - load('mode1.mat'); - plot(pts(:,1),pts(:,2),'.');hold on; - plot(rec(:,5),rec(:,6),'-');%despos - plot(rec(:,2),rec(:,3),'-');%actPos - - %sig.time = [0 1 1 5 5 8 8 10]; - %sig.signals.values = [0 0 2 2 2 3 3 3]'; - %sig.signals.dimensions = 1; - sig.time=0:2E-4:(length(rec)-1)*2E-4; - sig.signals.values=rec(:,5); - sig.signals.dimensions = 1; - - sum(desPos_actPos.Data(:,1)-desPos_actPos.Data(:,2)) - - A = [ 0 1 0 - 980 0 -2.8 - 0 0 -100 ]; - - B = [ 0 - 0 - 100 ]; - - C = [ 1 0 0 ]; - - poles = eig(A) - - t = 0:0.01:2; - u = zeros(size(t)); - x0 = [0.01 0 0]; - - sys = ss(A,B,C,0); - - [y,t,x] = lsim(sys,u,t,x0); - plot(t,y) - title('Open-Loop Response to Non-Zero Initial Condition') - xlabel('Time (sec)') - ylabel('Ball Position (m)') - - p1 = -10 + 10i; - p2 = -10 - 10i; - p3 = -50; - - K = place(A,B,[p1 p2 p3]); - sys_cl = ss(A-B*K,B,C,0); - - lsim(sys_cl,u,t,x0); - xlabel('Time (sec)') - ylabel('Ball Position (m)') - - p1 = -20 + 20i; - p2 = -20 - 20i; - p3 = -100; - - K = place(A,B,[p1 p2 p3]); - sys_cl = ss(A-B*K,B,C,0); - - lsim(sys_cl,u,t,x0); - xlabel('Time (sec)') - ylabel('Ball Position (m)') - - - t = 0:0.01:2; - u = 0.001*ones(size(t)); - - sys_cl = ss(A-B*K,B,C,0); - - lsim(sys_cl,u,t); - xlabel('Time (sec)') - ylabel('Ball Position (m)') - axis([0 2 -4E-6 0]) - - Nbar = rscale(sys,K) - - lsim(sys_cl,Nbar*u,t) - title('Linear Simulation Results (with Nbar)') - xlabel('Time (sec)') - ylabel('Ball Position (m)') - axis([0 2 0 1.2*10^-3]) -end - - - diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m index ca91adc..2e9da8c 100644 --- a/matlab/identifyFxFyStage.m +++ b/matlab/identifyFxFyStage.m @@ -31,11 +31,12 @@ function motCell=identifyFxFyStage(mode) % loadData reads members currstep,w,mag,phase,meas % % mode bits: - % 0 1 : add ss-models and do checks for motor 1 fy - % 1 2 : add ss-models and do checks for setup motor 2 fx - % 2 4 : identify_currstep - % 3 8 : identify_tf2 - % The default value for mode is 3 + % 0 1 : select motor 1 fy + % 1 2 : select motor 2 fx + % 2 4 : add ss-models and do obser/contr checks + % 3 8 : identify_currstep + % 4 16 : identify_tf (TODO!) + % The default value for mode is 7 %References: @@ -67,6 +68,7 @@ function motCell=identifyFxFyStage(mode) opt=tfestOptions; opt.Display='off'; tfc = tfest(obj.currstep, 2, 0,opt); + s=splitlines(string(evalc('tfc')));disp(join(s(5:7),newline)); s=str2ndOrd(tfc); t=(0:199)*50E-6; [y,t]=step(tfc,t); @@ -85,7 +87,7 @@ function motCell=identifyFxFyStage(mode) print(f,sprintf('figures/currstep_%d',obj.id),'-depsc'); end - function tf2=identify_tf2(obj) + function tf2=identify_tf(obj) opt=tfestOptions; opt.Display='off'; opt.initializeMethod='iv'; @@ -351,19 +353,22 @@ function motCell=identifyFxFyStage(mode) for motid= 1:2 mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/19_01_29/',motid); mot.id=motid; - if bitand(mode,4) - %identification of second order transfer function out of the current step recorded data. - identify_currstep(mot{motid}); - end - if bitand(mode,8) - %identification of second order transfer function out of the position recorded data. - identify_tf2(mot); - end - if motid==1 && bitand(mode,1) - mot=fyStage(mot); - end - if motid==2 && bitand(mode,2) - mot=fxStage(mot); + if bitand(mode,motid) + if bitand(mode,4) + if motid==1 + mot=fyStage(mot); + else + mot=fxStage(mot); + end + end + if bitand(mode,8) + %identification of second order transfer function out of the current step recorded data. + identify_currstep(mot); + end + if bitand(mode,16) + %identification of second order transfer function out of the position recorded data. + identify_tf(mot); + end end motCell{motid}=mot; end diff --git a/matlab/observer.slx b/matlab/observer.slx index c0b0c01..d998914 100644 Binary files a/matlab/observer.slx and b/matlab/observer.slx differ diff --git a/matlab/simFxFyStage.m b/matlab/simFxFyStage.m index f268b5c..0684aa5 100644 --- a/matlab/simFxFyStage.m +++ b/matlab/simFxFyStage.m @@ -13,25 +13,18 @@ function [pb]=simFxFyStage(mot) %!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=mot.tf_mdl.Numerator; - mot_den=mot.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; Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=3500;MaxInt=1000; - mot_num=mot.tf_mdl.Numerator; - mot_den=mot.tf_mdl.Denominator; end - - mdlName='stage_closed_loop'; - %open(mdlName) - %ServoDeltaTau_z(motid) - [A,B,C,D]=tf2ss(mot_num,mot_den); - - + ss_plt=mot.ss_plt; pb=struct(); - for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","mot_num","mot_den","Ts","MaxDac","MaxPosErr","A","B","C","D"] + for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","Ts","MaxDac","MaxPosErr","ss_plt"] pb=setfield(pb,k,eval(k)); end + %mdlName='stage_closed_loop'; + %open(mdlName) + %sim(mdlName) end diff --git a/matlab/stage_closed_loop.slx b/matlab/stage_closed_loop.slx index 9f72091..cc5a8b4 100644 Binary files a/matlab/stage_closed_loop.slx and b/matlab/stage_closed_loop.slx differ