s = tf('s');

% Bilmodell (kraft till position)
G = 0.5/(2*s+1)/s;

%%%%%%%%
% Fall 1
%%%%%%%%

% P-regulator 1
K1 = 1;

% Bodediagram öppet system med skärfrekvens och fasmarginal
Go1 = K1*G;
%figure(1)
%margin(Go1)

% Stegsvar för slutet system (referens till position)
Gc1 = feedback(Go1,1);
%figure(2)
%step(Gc1)
%stepinfo(Gc1)

%%%%%%%%
% Fall 2
%%%%%%%%

% P-regulator 2
K2 = 1/0.34;

% Bodediagram öppet system med skärfrekvens och fasmarginal
Go2 = K2*G;
%figure(3)
%margin(Go2)

% Stegsvar för slutet system (referens till position)
Gc2 = feedback(Go2,1);
%figure(4)
%step(Gc2)
%stepinfo(Gc2)

%%%%%%%%%%%%%%%%%%%%
% Fall 3 (lead-länk)
%%%%%%%%%%%%%%%%%%%%

% Öka fasmarginal med 20 grader
beta = 0.49; % fig. 5.13

% Placera de 20 graderna vid önskad skärfrekvens 0.79 rad/s
tauD = 1/0.79/sqrt(beta); % ekv. (5.5)

% Se till att kompenserade öppna systemet har skärfrekvens 0.79 rad/s
K3 = sqrt(beta)/0.34; % ekv. (5.6) och |G(i0.79)|=0.34

% Lead-länk
Flead = K3*(tauD*s+1)/(beta*tauD*s+1);

% Bodediagram öppet system med skärfrekvens och fasmarginal
Go3 = Flead*G;
figure(1)
margin(Go3)

% Stegsvar för slutet system (referens till position)
Gc3 = feedback(Go3,1);
figure(2)
step(Gc3)
stepinfo(Gc3)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Jämförelse Fall 1, 2 och 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Bodediagram öppet system 
figure(3)
bode(Go1,Go2,Go3)
legend('Fall 1','Fall 2','Fall 3')

% Stegsvar för slutet system (referens till position)
figure(4)
step(Gc1,Gc2,Gc3)
legend('Fall 1','Fall 2','Fall 3')
