% losn7_oppg1.m
% Purpose: Solution of Task 1 in Exercise 7 in SCE1106 Control Theory
% Contents:
% Calculating the phase crossover frequency, omega_180.
% Calculating the Gain Margin, GM.
% Calculating the gain crossover frequency, omega_c.
% Calculating the Phase Margin, PM.
% and PI-controller tuning.
% DDiR, 22. oktober 2002. Rvised 2011

clear all

k=0.5; T1=1; T2=0.5; tau=2;

%% Chosing Ti=1;
% Calculating, omega_180,
% using fix point iterations.
omega=1;
for i=1:100
   omega=(pi/2-atan(T2*omega))/tau;
end
omega_180=omega                              % Phase crossover frequency
%Check, wether angle_h0=-pi.
vinkel_h0=-tau*omega-pi/2-atan(T2*omega)

% Calculating  K_p such that GM=2
Kp=T1*omega_180*sqrt(1+(T2*omega_180)^2)/(2*k)

% Calculating , omega_c,
% using fix point iterations.
omega=1, k0=Kp*k/T1;
for i=1:100
   omega=k0/sqrt(1+(T2*omega)^2);
end
omega_c=omega                                % Gain crossover frequency

% Phase Margin, PM
vinkel_h0=-tau*omega_c-pi/2-atan(T2*omega_c);
PM=(vinkel_h0+pi)*180/pi



%% Check same calculations using the control system toolbox functions
Ti=T1;
num1=[0,0,1];
den1=[1,3,2];
[numd,dend]=pade(tau,10);
[num_hp,den_hp]=series(num1,den1,numd,dend);

num_hc=Kp*[Ti,1];
den_hc=[Ti,0.e-9];

[num_h0,den_h0]=series(num_hc,den_hc,num_hp,den_hp);
[Gm,Pm,W180,Wc] = margin(num_h0,den_h0)


