close all
clear all

Diameter = 0.2032;
densitySW = 1025;
Cd = 1.5;
%Cd = 0.01;
G = 9.807;

PlatformArea = (pi/4) * Diameter^2;

%CLinearize = 0.0;
CLinearize = 0.1;

PlatformMass = 38;
VirtualMassFactor = 1.25;
PlatformVirtualMass = 38 * VirtualMassFactor;

pumpDisplacement = 0.156 * 10^-7;
%pumpDisplacement = 0.156 * 10^-5;
pumpEff = 0.6;

a = pumpDisplacement * pumpEff * densitySW * G;
b = 0.5 * densitySW * Cd * PlatformArea * CLinearize;
mv = PlatformVirtualMass;

a = a
b = b
AoverM =a/mv
BoverM = b/mv

s = tf('s');

DepthPlantTF = a / ((mv*s^3) + (b*s^2));
figure
rlocus(DepthPlantTF)
title('Depth Plant TF');

depthPlantPoles = pole(DepthPlantTF)
depthPlantZeroes = zero(DepthPlantTF)

m = 0;
n = 3;

l = n - m;

for i=1:l
    asymAngles(i) = (180 + (360*(i-1))) / l
end

alpha = (sum(real(depthPlantPoles)) - sum(real(depthPlantZeroes))) / l

Ti = 10000000;
K = 50;
Td = 10;
PID_TF = K * (1 + (1/(s*Ti)) + (s*Td));
%PID_TF = K * (1 + (s*Td));

DepthPID_TF = PID_TF * DepthPlantTF;
% figure
% rlocus(DepthPID_TF)
% title('Depth PID TF');


