cp4_1.m

来自「离散控制系统设计的MATLAB 代码」· M 代码 · 共 93 行

%%%%%%%%%%% Comprehensive Problem 4.1 %%%%%%%%%%%
%   Discrete-Time Control Problems using        %
%       MATLAB and the Control System Toolbox   %
%   by J.H. Chow, D.K. Frederick, & N.W. Chbat  %
%         Brooks/Cole Publishing Company        %
%                September 2002                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Ball and Beam System              
% System has 4 states, 1 input, and 2 outputs
% 4 states : ball's position and velocity, and
%            wheel's angular position and velocity
% 1 input  : motor voltage (u)
% 2 output : ball's position (xi), and 
%            wheel's angular position (theta)

disp('CP4.1: Ball and Beam')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part A : Load model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear; close all

% Load continuous and discrete-time systems
if ~exist('bbeam.mat'), 
    dbbeam
elseif exist('bbeam.mat') == 2,
    load bbeam
end

Ts = 0.02;  % in dbbeam already

disp('*******>');pause; disp(' ')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part B : Compute Zeros and Poles
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp(['The state matrix A of the state-space model ',...
      'Gz(z) is :'])
Gz.a
disp('*******>');pause
[zz,pp,kk] = zpkdata(Gz,'v'); % compute zeros & poles of G(z)
disp('Zeros of Gz(1,1) are :'); zz{1}
disp('Poles of Gz(1,1) are :'); pp{1}
disp('*******>');pause
disp(['Zero of Gz(2,1) is : ', num2str(zz{2})])
disp('Poles of Gz(2,1) are : '); pp{2}
disp('*******>');pause

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part C : Use P control for Km(z) and 
%          PD control for Kb(z) - Plot Response
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Km(z) prop. control - Kb(z) prop. control'); disp(' ')
disp('Response to a step command of 0.1 m in ball position')
disp('*******>');pause

t  = [0:1:1250]*Ts;
% controllers from CP 3.1
disp('P controller')
Km = 3
G_Kmcl = feedback(Gz*Km,1,1,2,-1) % servo CL
disp('*******>');pause
Kp2 = 1;
Kd  = 1;
disp('PD controller')
KbPD = tf(Kp2*[(1+Kd/Ts) -Kd/Ts],[1 0],Ts) % PD controller
G_PDcl = feedback(G_Kmcl*KbPD,1,1,1) % b&b CL
disp('Response to 0.1 m position command')
disp('*******>');pause
y_PDcl = step(G_PDcl,t);
figure
plot(t,0.1*y_PDcl,'o'); grid on
title(['Step Response to 0.1 position command: ball (blue) & wheel (green)']);
ylabel('Ball position (m) and wheel angle (rad)')
xlabel('Time (s)')
disp('The PD controller improves the response time and damping.')
disp('*******>');pause

disp('Response to 0.1 m in initial condition')
u = zeros(size(t));
disp('*******>');pause
y = lsim(G_PDcl,u,t,[0.1 0 0 0 0]');
figure
plot(t,y,'o'); grid on
title(['Initial Condition Response: ball (blue) & wheel (green)']);
ylabel('Ball position (m) and wheel angle (rad)')
xlabel('Time (s)')
disp('The controller returns the ball to the origin.')
disp('*******>');pause

disp('End of Comprehensive Problem CP4.1')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%