clc
clear all
close all

% Load data
plantData = load('PlantFreqResp.txt');
w = plantData(:,1)*2*pi;            % rad/s
magPlant = 10.^(plantData(:,2)/20); % abs
phasePlant = plantData(:,3);        % deg

% Plant Bode plot
figure(1)
subplot(211)
loglog(w,magPlant);
xlabel('freq (rad/s)');
ylabel('magnitude (abs)');
title('Plant');
grid on

subplot(212)
semilogx(w,phasePlant);
xlabel('freq (rad/s)');
ylabel('phase (deg)');
grid on

%% P controller
Kp = 40;
magLoop = Kp*magPlant;
phaseLoop = phasePlant;

figure(2)
subplot(211)
loglog(w,magLoop);
xlabel('freq (rad/s)');
ylabel('magnitude (abs)');
title('Loop Return Ratio (P control)');
grid on

subplot(212)
semilogx(w,phaseLoop);
xlabel('freq (rad/s)');
ylabel('phase (deg)');
grid on

%% PI controller

% Lag compensator
Ti = 10;
Lag = tf([Ti 1],[Ti 0]);

% Proportional gain
Kp = 40;

% Controller
K = Kp*Lag;

[mag,phase] = bode(K,w);
magController = squeeze(mag);
phaseController = squeeze(phase);

magLoop = magController.*magPlant;
phaseLoop = phaseController+phasePlant;

figure(3)
subplot(211)
loglog(w,magLoop);
xlabel('freq (rad/s)');
ylabel('magnitude (abs)');
title('Loop Return Ratio (PI control)');
grid on

subplot(212)
semilogx(w,phaseLoop);
xlabel('freq (rad/s)');
ylabel('phase (deg)');
grid on

%% PID controller

% Lead compensator
tau = 0.1;
alpha = 10;
Lead = tf([alpha*tau 1],[tau 1]);

% Lag compensator
Ti = 10;
Lag = tf([Ti 1],[Ti 0]);

% P gain
Kp = 50;

% Controller
K = Kp*Lag*Lead;

[mag,phase] = bode(K,w);
magController = squeeze(mag);
phaseController = squeeze(phase);

magLoop = magController.*magPlant;
phaseLoop = phaseController+phasePlant;

figure(4)
subplot(211)
loglog(w,magLoop);
xlabel('freq (rad/s)');
ylabel('magnitude (abs)');
title('Loop Return Ratio (PID control)');
grid on

subplot(212)
semilogx(w,phaseLoop);
xlabel('freq (rad/s)');
ylabel('phase (deg)');
grid on