387 lines
14 KiB
Matlab
387 lines
14 KiB
Matlab
|
|
function motCell=identifyFxFyStage(mode)
|
|
%loads recorded data of the current step and bode diagrams of the stages then plots the bode diagrams and identifies
|
|
%the current step transfer function
|
|
%
|
|
%finally it builds a ss-Model of the stage with:
|
|
%
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> iqMeas
|
|
% | 2|-------> actVel
|
|
% | 3|-------> actPos
|
|
% +-----------+
|
|
%
|
|
% the returned motor objects mot1 and mot2 contains:
|
|
%
|
|
% currstep : gathered data with Python: file 'curr_step%d.mat'
|
|
% w,mag,phase : gathered data with Python: file 'full_bode_mot%d.mat'
|
|
% meas : a MATLAB idfrd model with data w,mag,phase
|
|
% mdl : a structure with the python numerators and denominators for the transfer functions
|
|
% tfc,tf_mdl : various transfer functions
|
|
% ssPlt : the final continous state space model of the plant (not observable, not controlable)
|
|
% ssMdl : the simplified continous state space model for the observer (observable, controlable)
|
|
% ssMdlNC : model without resonance and current loop
|
|
%
|
|
% The used data files (generated from Python) are:
|
|
% (located for now in: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/<date>/ )
|
|
% - curr_step[1|2].mat
|
|
% - full_bode_mot[1|2].mat
|
|
% - model[1|2].mat
|
|
%
|
|
% loadData reads members currstep,w,mag,phase,meas
|
|
%
|
|
% mode bits:
|
|
% 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:
|
|
%create ss from tf MIMO:
|
|
%https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
|
|
%http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
|
|
%https://ch.mathworks.com/help/control/ref/append.html
|
|
|
|
function obj=loadData(path,motid)
|
|
obj=struct();
|
|
f=load(strcat(path,sprintf('curr_step%d.mat',motid)));
|
|
obj.currstep=f;
|
|
%prepend sone zeros to stable system identification
|
|
obj.currstep=iddata([zeros(10,1); obj.currstep.data(:,2)],[zeros(10,1); obj.currstep.data(:,3)],50E-6);
|
|
|
|
f=load(strcat(path,sprintf('full_bode_mot%d.mat',motid)));
|
|
obj.w=f.frq*2*pi; %convert from Hz to rad/s
|
|
obj.mag=10.^(f.db_mag/20); %mag not in dB
|
|
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=identify_currstep(obj)
|
|
%identification of second order transfer function out of the current step recorded data.
|
|
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);
|
|
f=figure();f.Position=[200,100,900,500];
|
|
subplot(1,2,1);
|
|
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');
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
%print(sprintf('figures/currstep_%d',obj.id),'-dpng','-r0');
|
|
print(f,sprintf('figures/currstep_%d',obj.id),'-depsc');
|
|
end
|
|
|
|
function tf2=identify_tf(obj)
|
|
opt=tfestOptions;
|
|
opt.Display='off';
|
|
opt.initializeMethod='iv';
|
|
%opt.WeightingFilter=[1,5;30,670]*(2*pi); % Hz->rad/s conversion
|
|
opt.WeightingFilter=[1,2;10,100]*(2*pi); % Hz->rad/s conversion
|
|
|
|
figure();
|
|
tf2 = tfest(obj.meas, 2, 0, opt);disp(str2ndOrd(tf2));
|
|
subplot(1,1,1);
|
|
h=bodeplot(tf2,'r',obj.meas,'b',obj.w);
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
p=getoptions(h);p.YLim{2}=[-360 90];p.YLimMode='manual';setoptions(h,p);
|
|
ax=h.getaxes();
|
|
legend(ax(1),'Location','sw',{'real','tf2'});
|
|
|
|
frq=obj.w/(2*pi)
|
|
[m1,p1,w1]=bode(obj.meas,obj.w);
|
|
[m2,p2,w2]=bode(tf2,obj.w);
|
|
m1=20*log10(reshape(m1,[],1));p1=reshape(p1,[],1);
|
|
m2=20*log10(reshape(m2,[],1));p2=reshape(p2,[],1);
|
|
me=m1-m2;
|
|
pe=p1-p2;
|
|
figure();
|
|
ax1=subplot(2,1,1);
|
|
title('remaining mag (dB) and phase error')
|
|
semilogx(frq,me,'r');
|
|
ax2=subplot(2,1,2);
|
|
semilogx(frq,pe,'r');
|
|
linkaxes([ax1,ax2],'x')
|
|
ax2.YLim=[-90 90];ax2.YLimMode='manual';
|
|
ax2.XLim=[frq(1), frq(1000)];ax2.XLimMode='manual';
|
|
grid(ax1,'on');grid(ax2,'on');
|
|
end
|
|
|
|
|
|
function s=str2ndOrd(tf)
|
|
den=tf.Denominator;
|
|
num=tf.Numerator;
|
|
k=num(1)/den(3);
|
|
w0=sqrt(den(3));
|
|
damp=den(2)/2/w0;
|
|
s=sprintf('k:%g w0:%g damp:%g',k,w0,damp);
|
|
end
|
|
|
|
function chkCtrlObsv(ss,s)
|
|
P=ctrb(ss.A,ss.B);
|
|
if rank(ss.A)==rank(P)
|
|
ct='';%controlable
|
|
else
|
|
ct='not ';%not controlable
|
|
end
|
|
|
|
Q=obsv(ss.A,ss.C);
|
|
if rank(ss.A)==rank(Q)
|
|
ob='';%sys observable
|
|
else
|
|
ob='not ';%not observable
|
|
end
|
|
disp([s,' is ',ct,'controlable and ',ob,'observable.']);
|
|
|
|
%tf(ss) % display all transfer functions
|
|
|
|
end
|
|
|
|
function y=myNorm(y)
|
|
%normalizes num and den by factor 1000
|
|
%y.*10.^(3*(length(y):-1:1))
|
|
end
|
|
|
|
function plotBode(mot)
|
|
figure()
|
|
h=bodeplot(mot.meas,'r',mot.ss_plt(3,1),'g',mot.ss_c1(3,1),'b',mot.ss_d1(3,1),'m',mot.ss_1(2,1),'c',mot.ss_0(2,1),'k',mot.w);
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
p=getoptions(h);p.YLim{2}=[-360 90];p.YLimMode='manual';setoptions(h,p);
|
|
ax=h.getaxes();
|
|
legend(ax(1),'Location','sw',{'real','plant','c1','d1','1','0'});
|
|
print(gcf,sprintf('figures/plotBode_%d',mot.id),'-depsc');
|
|
end
|
|
|
|
function mot=fyStage(mot)
|
|
|
|
%current loop iqCmd->iqMeas
|
|
tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas');
|
|
|
|
%simplified current loop iqCmd->iqMeas (first order tf)
|
|
tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas');
|
|
|
|
%resonance iqMeas->iqForce
|
|
tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqMeas','OutputName','iqForce');
|
|
|
|
%current to position iqForce->actPos
|
|
tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
|
|
|
|
%force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
|
|
tf1=tf({[mot.mdl.num1 0];mot.mdl.num1},mot.mdl.den1,'InputName','iqForce','OutputName',{'actVel','actPos'});
|
|
|
|
%simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
|
|
tf0=tf({[mot.mdl.num0 0];mot.mdl.num0},mot.mdl.den0,'InputName','iqForce','OutputName',{'actVel','actPos'});
|
|
|
|
%check observable/controlable of transfer functions
|
|
ssLst=["tfc","tfd","tf0","tf1","tf2","tfc*tf1*tf2","tfc*tf1","tfd*tf1","tf1*tf2"];
|
|
sys=[];
|
|
for s = ssLst
|
|
eval('sys=ss('+s+');')
|
|
chkCtrlObsv(sys,char(s));
|
|
end
|
|
|
|
% sample code:
|
|
%tfc iqCmd-> iqMeas
|
|
%tf2 resonance iqMeas->iqForce
|
|
%tf1 iqForce->(actVel,actPos)
|
|
%connect(tfc,tf2,'iqCmd','iqForce');
|
|
%connect(tfc,tf2,'iqCmd',{'iqMeas','iqForce'});
|
|
%connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','iqForce','actPos'});
|
|
%connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','actPos'});
|
|
|
|
|
|
% best plant approximation
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> iqMeas
|
|
% | 2|-------> actVel
|
|
% | 3|-------> actPos
|
|
% +-----------+
|
|
mot.ss_plt=connect(tfc,tf1,tf2,'iqCmd',{'iqMeas','actVel','actPos'});
|
|
mot.ss_plt.Name='best plant approximation';
|
|
chkCtrlObsv(mot.ss_plt,'ss_plt fyStage');
|
|
|
|
%without resonance
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> iqMeas
|
|
% | 2|-------> actVel
|
|
% | 3|-------> actPos
|
|
% +-----------+
|
|
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
|
|
mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
|
|
mot.ss_c1.Name='without resonance';
|
|
chkCtrlObsv(mot.ss_c1,'ss_c1 fyStage');
|
|
tf1.InputName{1}=s;%restore
|
|
|
|
|
|
%simplified current, without resonance
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> iqMeas
|
|
% | 2|-------> actVel
|
|
% | 3|-------> actPos
|
|
% +-----------+
|
|
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
|
|
mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
|
|
mot.ss_d1.Name='simplified current, without resonance';
|
|
chkCtrlObsv(mot.ss_d1,'ss_d1 fyStage');
|
|
tf1.InputName{1}=s;%restore
|
|
|
|
|
|
%no current loop, no resonance
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> actVel
|
|
% | 2|-------> actPos
|
|
% +-----------+
|
|
mot.ss_1=ss(tf1);
|
|
mot.ss_1.Name='no current loop, no resonance';
|
|
chkCtrlObsv(mot.ss_1,'ss_1 fyStage');
|
|
|
|
|
|
%simplified mechanics, no current loop, no resonance
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> actVel
|
|
% | 2|-------> actPos
|
|
% +-----------+
|
|
mot.ss_0=ss(tf0);
|
|
mot.ss_0.Name='simplified mechanics, no current loop, no resonance';
|
|
chkCtrlObsv(mot.ss_0,'ss_0 fyStage');
|
|
|
|
%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);
|
|
plotBode(mot)
|
|
end
|
|
|
|
function mot=fxStage(mot)
|
|
%current loop iqCmd->iqMeas
|
|
tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas');
|
|
|
|
%simplified current loop iqCmd->iqMeas (first order tf)
|
|
tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas');
|
|
|
|
%resonance iqMeas->iqForce
|
|
tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqMeas','OutputName','iqF1');
|
|
%resonance iqMeas->iqForce
|
|
tf3=tf(mot.mdl.num3,mot.mdl.den3,'InputName','iqF1','OutputName','iqF2');
|
|
%resonance iqMeas->iqForce
|
|
tf4=tf(mot.mdl.num4,mot.mdl.den4,'InputName','iqF2','OutputName','iqF3');
|
|
%resonance iqMeas->iqForce
|
|
tf5=tf(mot.mdl.num5,mot.mdl.den5,'InputName','iqF3','OutputName','iqForce');
|
|
|
|
%current to position iqForce->actPos
|
|
tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
|
|
|
|
%force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
|
|
tf1=tf({[mot.mdl.num1 0];mot.mdl.num1},mot.mdl.den1,'InputName','iqForce','OutputName',{'actVel','actPos'});
|
|
|
|
%simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
|
|
tf0=tf({[mot.mdl.num0 0];mot.mdl.num0},mot.mdl.den0,'InputName','iqForce','OutputName',{'actVel','actPos'});
|
|
|
|
%check observable/controlable of transfer functions
|
|
ssLst=["tfc","tfd","tf0","tf1","tf2","tf3","tf4","tf5",...
|
|
"tfc*tf1*tf2","tfc*tf1","tfd*tf1","tf1*tf2","tf1*tf2*tf3"];
|
|
sys=[];
|
|
for s = ssLst
|
|
eval('sys=ss('+s+');')
|
|
chkCtrlObsv(sys,char(s));
|
|
end
|
|
|
|
|
|
% best plant approximation
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> iqMeas
|
|
% | 2|-------> actVel
|
|
% | 3|-------> actPos
|
|
% +-----------+
|
|
mot.ss_plt=connect(tfc,tf1,tf2,tf3,tf4,tf5,'iqCmd',{'iqMeas','actVel','actPos'});
|
|
chkCtrlObsv(mot.ss_plt,'ss_plt fxStage');
|
|
|
|
%without resonance
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> iqMeas
|
|
% | 2|-------> actVel
|
|
% | 3|-------> actPos
|
|
% +-----------+
|
|
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
|
|
mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
|
|
chkCtrlObsv(mot.ss_c1,'ss_c1 fxStage');
|
|
tf1.InputName{1}=s;%restore
|
|
|
|
|
|
%simplified current, without resonance
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> iqMeas
|
|
% | 2|-------> actVel
|
|
% | 3|-------> actPos
|
|
% +-----------+
|
|
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
|
|
mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
|
|
chkCtrlObsv(mot.ss_d1,'ss_d1 fxStage');
|
|
tf1.InputName{1}=s;%restore
|
|
|
|
|
|
%no current loop, no resonance
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> actVel
|
|
% | 2|-------> actPos
|
|
% +-----------+
|
|
mot.ss_1=ss(tf1);
|
|
chkCtrlObsv(mot.ss_1,'ss_1 fxStage');
|
|
|
|
|
|
%simplified mechanics, no current loop, no resonance
|
|
% u +-----------+ y
|
|
%iqCmd------->|1 1|-------> actVel
|
|
% | 2|-------> actPos
|
|
% +-----------+
|
|
mot.ss_0=ss(tf0);
|
|
chkCtrlObsv(mot.ss_0,'ss_0 fxStage');
|
|
|
|
plotBode(mot)
|
|
end
|
|
close all
|
|
|
|
motCell=cell(2,1);
|
|
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,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
|
|
|
|
|
|
|
|
%controlSystemDesigner('bode',1,mot1.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited
|
|
|
|
|
|
end
|