136 lines
3.4 KiB
Matlab
136 lines
3.4 KiB
Matlab
%before calling that put following line in the command window:
|
|
%
|
|
% global mot pb;
|
|
% or even:
|
|
% clear global all;clear all;global mot pb;
|
|
|
|
function DeltaTauOptimizer()
|
|
global mot simData1 simData2;
|
|
if isempty(mot)
|
|
mot=identifyFxFyStage(7);
|
|
end
|
|
%SIM1=[1 1; 1 2; 8 2; 9 2];
|
|
%SIM2=[1 1; 1 2; 8 2; 9 2];
|
|
SIM1=[10 0;7 3];
|
|
SIM1=[10 0];
|
|
%SIM2=[9 0];
|
|
if 1 || isempty(simData1)
|
|
close all;
|
|
simData1=ExecSim(mot{1},SIM1);
|
|
end
|
|
%if isempty(simData2)
|
|
% close all;
|
|
% simData2=ExecSim(mot{2},SIM2);
|
|
%end
|
|
close all;
|
|
%test()
|
|
bodeSim(simData1);
|
|
%bodeSim(simData2);
|
|
end
|
|
|
|
function test()
|
|
global pb mot simData1 simData2;
|
|
%pb=DeltaTauParam(mot{2},8,0);
|
|
pb=DeltaTauParam(mot{2},9,0);
|
|
%pb.C=[0.04877];
|
|
%pb.D=[1 -0.9512];
|
|
%pb.C=[1 -1.3236 6.2472 -11.8555 11.3067 -5.4188 1.0440];
|
|
%pb.D=[1.0000 -6.6330 17.6945 -24.5314 18.7409 -7.5020 1.2309];
|
|
global tfs
|
|
tfz=c2d(tf(tfs),1/5000)
|
|
h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
pb.C=tfz.Numerator{1};
|
|
pb.D=tfz.Denominator{1};
|
|
sim('DeltaTauSim');
|
|
i=2;
|
|
%simData2(i).mot_mdl_param=SIM(i,:);
|
|
simData2(i).pb=pb;
|
|
simData2(i).desPos_actPos=desPos_actPos;
|
|
simData2=bodeSim(simData2);
|
|
|
|
opt=tfestOptions;
|
|
opt.Display='off';
|
|
% %tfa=tfest(simData2(i).tfEst, 6, 5,opt);
|
|
tfa=tfest(simData2(i).tfEst, 6, 6,opt);
|
|
% tfb=1/tfa
|
|
% tfc=c2d(tfb,1/5000)
|
|
%
|
|
% tfs=tf([1],[.001 1])
|
|
% tfz=c2d(tfs,1/5000)
|
|
% h=bodeplot(tfs,tfz)
|
|
% setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
|
|
|
|
%C=num=0 -1.3236 6.2472 -11.8555 11.3067 -5.4188 1.0440
|
|
%D=den=1.0000 -6.6330 17.6945 -24.5314 18.7409 -7.5020 1.2309
|
|
% controlSystemDesigner('bode',1,tf(1,1))
|
|
% controlSystemDesigner( 1/simData2(2).tfEst2)
|
|
% controlSystemDesigner(tfa)
|
|
|
|
%C =
|
|
% 1035.3 (s+217.8)
|
|
% -------------------
|
|
% (s+845.3) (s+266.8)
|
|
|
|
% 0.18956 (z-0.9574)
|
|
% --------------------
|
|
% (z-0.948) (z-0.8444)
|
|
|
|
% 0.1896 z - 0.1815
|
|
% ----------------------
|
|
% z^2 - 1.792 z + 0.8006
|
|
end
|
|
|
|
|
|
function simData=ExecSim(mot,SIM)
|
|
global pb;
|
|
% mot mdl param
|
|
simData=struct;
|
|
for i =1:size(SIM,1)
|
|
[mdl,param]=feval(@(x) x{:}, num2cell(SIM(i,:)));
|
|
pb=DeltaTauParam(mot,mdl,param);
|
|
sim('DeltaTauSim');
|
|
simData(i).mot_mdl_param=SIM(i,:);
|
|
simData(i).pb=pb;
|
|
simData(i).desPos_actPos=desPos_actPos;
|
|
|
|
fig=bodeSamples(desPos_actPos);
|
|
title(fig.Children(2),pb.desc);
|
|
end
|
|
end
|
|
|
|
function simData=bodeSim(simData)
|
|
fig=figure();
|
|
ax1=subplot(2,1,1); hold(ax1,'on')
|
|
ax2=subplot(2,1,2); hold(ax2,'on')
|
|
Ts=1/5000; % sampling period
|
|
|
|
for i =1:length(simData)
|
|
sd=simData(i).desPos_actPos;
|
|
t=0:Ts:sd.Time(end);
|
|
posI=interp1(sd.Time,sd.Data(:,1),t);
|
|
posO=interp1(sd.Time,sd.Data(:,2),t);
|
|
ftI=fft(posI);
|
|
ftO=fft(posO);
|
|
tf=ftO./ftI;
|
|
|
|
L=length(t);
|
|
k=(L-1)*Ts; % fmax =1/Ts at sample (L-1)/2 (index0-base)
|
|
N=[10 220]*k; % number of relevant indexes (index0-base)
|
|
frq=(N(1):N(2))/k;
|
|
N=N+1;%index0-base -> index1-base
|
|
tfn=tf(N(1):N(2));
|
|
|
|
%fig=figure(); h=plot(abs(ftI));
|
|
%fig=figure(); h=plot(abs(ftO));
|
|
%fig=figure(); h=plot(abs(ftI(N(1):N(2))));
|
|
h=plot(ax1,frq,abs(tfn), 'DisplayName',simData(i).pb.desc);
|
|
p=unwrap(phase(tfn))/(2*pi)*360;
|
|
h=plot(ax2,frq,p, 'DisplayName',simData(i).pb.desc);
|
|
simData(i).tfEst=idfrd(tfn,frq*2*pi,0);
|
|
end
|
|
grid(ax1,'on');grid(ax2,'on');
|
|
set(ax1, 'XScale', 'log');
|
|
set(ax2, 'XScale', 'log');
|
|
legend('Location','best');
|
|
end |