%% Calculate plant frequency response from measured data in simout
%---------------------------------------------
%| The Digital Motion Control Demo |
%---------------------------------------------
%| A practical lesson in Model-Based Design. |
%| Prepared by: |
%| Paul Lambrechts, March, 2007 |
%| Copyright 2007, The MathWorks, Inc. |
%---------------------------------------------
%| Note: This demo is prepared with R2006b |
%---------------------------------------------
% close all figures and set parameters
close all
nfft=512;
lw=2;
if exist('Ts','var')~=1
Ts=0.002;
end
if exist('n','var')~=1
Retrieve_Data
close all
end
%% Assign measured results to signal variables
% n=simout.signals.values(:,1);
% u1=simout.signals.values(:,2);
% u2=simout.signals.values(:,3);
%% Estimate transfer functions and coherence functions
% (actually you only need the first transfer)
[Tnu1,Fnu1] = tfestimate(n,u1,[],0,nfft,1/Ts);
[Cnu1] = mscohere(n,u1,[],0,nfft,1/Ts);
[Tnu2,Fnu2] = tfestimate(n,u2,[],0,nfft,1/Ts);
[Cnu2] = mscohere(n,u2,[],0,nfft,1/Ts);
F=Fnu1*2*pi; %frequency vector in rad/s
%% Plot the estimated transfer functions and coherence functions
figure
subplot(2,1,1);semilogx(Fnu1,20*log10(abs([ Tnu1 Tnu2 ])),'LineWidth',lw)
%subplot(2,1,1);semilogx(Fnu1,20*log10(abs([ Tnu1 ])),'LineWidth',lw)
grid
title('Estimated Transfer Functions')
%legend('Tnu1','Tnu2')
xlim([1 200])
ylabel('amplitude [dB]')
%subplot(2,1,2);semilogx(Fnu1,[ Cnu1 ],'LineWidth',lw)
subplot(2,1,2);semilogx(Fnu1,[ Cnu1 Cnu2 ],'LineWidth',lw)
grid
title('Coherence Functions')
xlim([1 200])
ylabel('coherence')
xlabel('frequency [Hz]')
%legend('Cnu1','Cnu2')
%pause
%% Calculate open loop response
Hol1=1./Tnu1-1;
%Hol1=-Tnu2./Tnu1; % This is a second way to obtain the open loop response
% Display the results
%figure
%semilogx(Fnu1,20*log10(abs([ Hol1 Hol2 ])))
%grid
%pause
%% Get controller transfer function (used to obtain measurements).
% You can also do this with the linearization tool
% Calculate preliminary controller
num =1e-4*[1/(2*pi* 5) 1];
den = [1/(2*pi*70) 1];
Cprel = c2d(tf(num,den),Ts);
%Cf=squeeze(freqresp(C,F));
Cf=squeeze(freqresp(Cprel,F));
%
%figure
%semilogx(Fnu1,20*log10(abs([ Hol1 Cf ])))
%grid
%pause
%% The open loop plant response can now be calculated
Hpm=Hol1./Cf;
%% For comparing: calculate state-space plant model from parameter data
% use defaults if parameters not defined
if exist('J1','var')~=1
DMC_parameters
end
%
Ap = [ 0 1 0 0 % x1
-k/J1 -(b1+b12)/J1 k/J1 b12/J1 % x1dot
0 0 0 1 % x2
k/J2 b12/J2 -k/J2 -(b2+b12)/J2 ]; % x2dot
Bp = [ 0
1/J1 % F
0
0 ];
%Cp = [ 1 0 0 0 % x1
% 0 0 1 0 ]; % x2
Cp = [ 0 0 1 0 ]; % x2
%Dp = [ 0
% 0 ];
Dp=0;
%
sysp=ss(Ap,Bp,Cp,Dp);
Hpf=[0 ; squeeze(freqresp(sysp,F(2:end)))];
%% You can divide the open loop plant response by a double integral
% to remove the -2 slope
sysint=tf(1/(J1+J2),[1 0 0]);
sysintf=squeeze(freqresp(sysint,F+eps)); % the eps is to prevent 'response is infinite' warning
sysint=c2d(sysint,Ts);
Hpmint=Hpm./sysintf;
%% Plot figures
figure
%subplot(211);
%semilogx(Fnu1,20*log10(abs([ Hol1 Cf Hpm])),'LineWidth',lw)
%semilogx(Fnu1,20*log10(abs([ Hpm Hpmint])),'k','LineWidth',lw)
semilogx(Fnu1,20*log10(abs( Hpm )),'k','LineWidth',lw)
title('Estimated Plant Transfer Function')
%legend('Open Loop','Controller','Estimated Plant')
grid on
xlim([1 200])
ylabel('amplitude [dB]')
xlabel('frequency [Hz]')
hold on
texthandle=text(1.2,-15,'Press key to add model','FontSize',16);
disp('Press key to add model')
pause
disp('Model added')
set(texthandle,'Visible','off')
semilogx(Fnu1,20*log10(abs( Hpf )),'r','LineWidth',lw)
%legend('Open Loop','Controller','Estimated Plant','Model')
legend('Estimated Plant','Model')
grid on
xlim([1 200])
zoom on
% ax=subplot(212);
% semilogx(Fnu1,unwrap(angle([ Hol1 Cf Hpm Hpf ]))*180/pi,'LineWidth',lw)
% grid
% set(ax,'YTick',[-360 -180 0 180])
% axis([1 200 -400 200])
%% Now use the tuned controller
% Cf=squeeze(freqresp(C,Fnu1*2*pi));
% Hfinal=Cf.*Hpm;
% figure
% semilogx(Fnu1,20*log10(abs([ Hpm Cf Hfinal ])))
% title('Final Open Loop')
% grid
