clc
clear

%---------- CONTINUOUS TIME TRANSFER FUNCTION ----------%
numerator = 250;
denominator = [1 30 400];
Gs = tf(numerator, denominator);
kp_system = numerator/denominator(3);

%---------- BODE PLOT ----------%
% You will place a Data Cursor over the "starting point" of the plot,
% write down the Magnitude (dB) and then place another Data Cursor
% 10 dB after the first Magnitude. Write down the Frequency of the second
% Data Cursor.
%bode(Gs);

%--------- FC, FS, T ----------%
% You are now ready to obtain the sampling time based on the cutting
% frequency from the previous step
fc = 34; % You should change this field according to G(s) Bode Plot
fs = fc*7;
T = 2*pi/fs;
Ts = round(T,2); % The sampling time must be rounded up (2 decimal places)

%---------- DISCRETE TIME TRANSFER FUNCTION ----------%
Gz = c2d(Gs, Ts);

%---------- SISOTOOL G(Z) ----------%
% Please, refer to [1] for a complete guide on how this section works
% [1] https://dnplas.gitbooks.io/computarized-control-a-matlab-guide/content/compensators/1-sisotool/1-sisotool.html
%sisotool(Gz);

%---------- >>LEAD COMPENSATOR<< ----------%
%---------- ZPK ----------%
zero = 0.6;
pole = -0.3;
gain = 9;
Cz = zpk(zero, pole, gain, T);
kp_lead = gain*((1-zero)/(1-pole));

% After simulating on Simulink, you'll get an error. Let's say you want to
% get that number down to 5%, for instance. That error can only be achieved
% adding a Lag Compensator.

%---------- >>LAG COMPENSATOR<< ----------%
% Calculate the gain using the steady state error expression
ess = 0.05;
syms k;
eqn = ess == 1/(1+(kp_system*kp_lead*k));
kp_lag = vpasolve(eqn, k);
kp_lag = double(kp_lag);
kp_lag = round(kp_lag,1);

% Calculate the compensator expression
syms a_2;
eqn = (1-a_2)/(1-0.99) == kp_lag;
a_2 = vpasolve(eqn, a_2);
a_2 = double(a_2);

%---------- ZPK ----------%
zeros = [zero, a_2];
poles = [pole, 0.99];
C2z = zpk(zeros, poles, gain, T);