150 lines
3.7 KiB
Matlab
150 lines
3.7 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;9 3];
|
|
%SIM1=[9 1; 9 2; 9 3; 9 4];
|
|
%SIM1=[9 1; 9 2; 9 3; 9 4; 9 5; 9 6];
|
|
SIM1=[9 1; 9 2; 9 5; 9 6];
|
|
|
|
%SIM2=[1 1; 1 2; 8 2; 9 2];
|
|
%SIM2=[9 1; 9 2; 9 3; 9 4];
|
|
SIM2=[9 1; 9 2];
|
|
|
|
if isempty(simData1)
|
|
close all;
|
|
simData1=ExecSim(mot{1},SIM1);
|
|
end
|
|
if isempty(simData2)
|
|
close all;
|
|
simData2=ExecSim(mot{2},SIM2);
|
|
end
|
|
close all;
|
|
%test1();return
|
|
%test2();return
|
|
bodeSim(simData1);
|
|
bodeSim(simData2);
|
|
for i =1:2
|
|
set(i,'OuterPosition',[80 400 1000 700]);
|
|
print(i,sprintf('figures/sim_optimize%d',i),'-depsc');
|
|
end
|
|
end
|
|
|
|
function test1()
|
|
global pb mot simData1;
|
|
%pb=DeltaTauParam(mot{1},8,0);
|
|
pb=DeltaTauParam(mot{1},9,0);
|
|
sim('DeltaTauSim');
|
|
i=7;
|
|
simData1(i).pb=pb;
|
|
simData1(i).desPos_actPos=desPos_actPos;
|
|
simData1=bodeSim(simData1);
|
|
end
|
|
|
|
function test2()
|
|
global pb mot simData2;
|
|
%pb=DeltaTauParam(mot{2},8,0); % ss_cq no resonance
|
|
pb=DeltaTauParam(mot{2},9,0); % ss_cqr with resonance
|
|
sim('DeltaTauSim');
|
|
i=3;
|
|
simData2(i).pb=pb;
|
|
simData2(i).desPos_actPos=desPos_actPos;
|
|
simData2=bodeSim(simData2);
|
|
return
|
|
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');
|
|
linkaxes([ax1,ax2],'x')
|
|
legend('Location','best');
|
|
end
|
|
|
|
function SCRATCH()
|
|
%simData2(i).mot_mdl_param=SIM(i,:);
|
|
%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
|
|
Ts=1/5000;
|
|
frq0=55;d0=.5;
|
|
frq1=85;d1=.5;
|
|
w0=frq0*2*pi;
|
|
w1=frq1*2*pi;
|
|
tf0=tf([w0^2],[1 2*d0*w0 w0^2 ])
|
|
tf1=tf([1 2*d1*w1 w1^2 ],[w1^2])
|
|
tfs=tf0*tf1
|
|
tfz=c2d(tfs,Ts)
|
|
%h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
%pb.C=tfz.Numerator{1};
|
|
%pb.D=tfz.Denominator{1};
|
|
|
|
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');
|
|
% controlSystemDesigner(tfa)
|
|
end
|