Code covered by the BSD License

# Motion Control Demo

### Paul Lambrechts (view profile)

30 Oct 2007 (Updated )

Model Based Design Demonstration Based on a Motion Control Case Study

Calc_Hplant.m
```%% 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);

%% 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
pause
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```