% Matlab Tutorial
% CDS 101/110a
% Luis Soto

% This tutorial will introduce you to useful Matlab commands that are used
% to analyze control systems. Most of the control theory topics in this tutorial
% will be covered later in the course. An objective of this tutorial is to give you a glimpse of 
% what you will be asked to do later in the course, and to show how using Matlab 
% can make the task easier. Do not worry if you are not
% yet familiar with the Matlab commands and concepts covered in this
% tutorial. However, we do expect you to ask questions when something is not
% clear or if a concept seems confusing. 

% Example 1 (done by the TA)
% Process or system to be controlled represented by the "transfer function"
% P=1/[(s+3)(s+5)]

% Define numerator and denominator of plant transfer function
numP = 1; % Typing a semicolon at the end suppresses the output
denP = conv([1 3],[1 5]);

% Use command "tf" to obtain the transfer function representation
P = tf(numP,denP)

% To determine the response of the plant (process) to a constant (unit) step input
% use the "step" command
figure(1); step(P)

% One way to determine the stability of the plant is to determine the
% poles of its transfer function using either the "pole" or "pzmap"
% command

Poles = pole(P)      % Gives the numerical values of the poles of the tf
figure(2); pzmap(P)  % Poles and zeros are plotted in the complex plane

% Note that the response of the plant to a unit step input is terrible
% (desired value is 1, but the actual steady-state value of the plant is
% 0.0667
% A controller can be designed and implemented to improve system
% performance. You will learn how to do this later in the course. For now
% we will use the controller represented as C = r*(s+6)/(s+10)

% Define numerator and denominator of the controller transfer function
r=100;
numC = r*[1 6];   
denC = [1 10];

% Use command "tf" to obtain the transfer function representation
C = tf(numC,denC)

% If the controller and plant are to be connected in series, we can use the
% command "series" to connect these two "subsystems"
% Here's the open loop tf (negative feedback not yet implemented) L = C*P.
L = series(C,P)

% Let's see the response to a step input after implementing C & P
% in open loop 
figure(3); step(L)

% Now the problem is that the actual steady-state value is too large (3.98)
% Let's determine the closed-loop transfer function of the entire system when 
% negative feedback is used. Negative or positive Feedback is implemented using the command
% "feedback" command. Here's the closed-loop tf of the feedback system
H = feedback(L,1)

% Step response using negative feedback
figure(4); step(H,3)   % use step(H,t) to specify time interval

% Response has improved considerably, although a steady-state error of 0.2
% remains. Later in the course you will learn how to improve closed-loop peformance using
% techniques such as PID control.

% Now assume that the input is a sinusoidal driving force  u = A*sin(omega*t). 
% Let's see the steady-state response of the closed-loop system to a sinusoidal input
% for different frequencies omega. We call this the "frequency response" of the system. 
% The frequency response can be represented via a Bode plot
figure(5); bode(H)

% Notice that the gain = Ay/Au decreases as angular frequency omega
% increases, and the output tends to be increasingly out of phase as the angular frequency 
% increases.

% We are often interested in the bandwidth of the closed-loop system.
% Bandwidth is defined as the first frequency (in rads/sec) where the gain drops below 
% 70.79 percent (gain is -3 dB) of its zero-frequency (DC) value. The command
% "bandwidth" is used to obtain an accurate value, although it can also be
% estimated fom the bode plot of the closed-loop system using the trace tool. Methods for
% increasing bandwidth to a satisfactory value will be presented later in
% the course.
bw = bandwidth(H)

% Two important concepts that tell "how" stable is the closed-loop system  are "phase margin" and "gain
% margin". These are obtained using the "margin" command on the OPEN-loop
% system L = C*P. This command gives the bode plot of L along with the
% phase and gain margins.

figure(6); margin(L)

% Another way to represent the same information given by a bode plot is by
% plotting a system's tf in the complex plane. This plot is called the
% Nyquist Plot, and it also plots the gain and phase but in a different way
figure(7); nyquist(L)

% At this point, the TA should point out the effect of increasing the
% parameter r of the controller to r=1000. This increase in the gain provided
% by the controller leads to:
%   1. Decrease in steady-state error for a step input (good)
%   2. Faster system response (good)
%   3. Larger overshoot and more oscillatory behavior (bad)
%   4. Decrease in phase margin (bad)


% We can represet any system in state-space form (time domain) given its transfer 
% function by using the command "tf2ss". We obtain the A, B, C, and D matrices corresponding
% to the system. The "ss" command creates the corresponding state-space
% model.

% State-space representation of the Plant
[A, B, C, D] = tf2ss(numP,denP)
Plant = ss(A,B,C,D);
figure(8); step(Plant)

% The real part of the eigenvalues of the dynamics matrix A determine the stability of the
% state-space system. The eigenvalues of A correspond to the poles of the
% tf of the plant (which earlier were found to be -3 and -5).
eigP = eig(A)

% If you know the eigenvalues that you would like the closed-loop system to have,
% the command "place" determines the matrix K for the control law u = -K*x,
% which will place the eigenvalues in the desired location.

% If the feedback control law is u = -K*x, then the closed-loop
% system is obtained by substituting u into xdot = A*x + B*u, which leads
% to the closed-loop system xdotc = A*x + B*(-K*x) = (A - B*K)*x,
% where Ac = A - B*K is the dynamics matrix of the closed-loop system having the
% desired eigenvalues.

K = place(A,B,[-0.3+i -0.3-i])  %maybe steady-state error is more important than overshoot 
Ac = A-B*K
eigAc = eig(Ac)
H2 = ss(Ac,B,C,D);
figure(9);step(H2)
figure(10);bode(H2); %The Bode command also works when system is represented in ss form


% You can go back to trasfer function representation via the "tf2ss" command
[num den] = ss2tf(Ac,B,C,D)
H3 = tf(num,den)  % the 2.22e-16*s term that appears in the numerator can be set to 0.
pole(H3)          % As expected, the poles equal the complex eigenvalues we placed earlier


% Finally, a reminder that when you use the "plot" command you must label the axes, give 
% a title to the plot, as Matlab will not do this for you. Include a legend if applicable. 
% Always submit all code and plots as part of your homework.

t = [0:0.01:2*pi]; %Define time vector
y = sin(t);         
figure(11); plot(t,y); xlabel('time (sec)'); ylabel('Position (meters)'); title('Sinusoidal Motion')