close all
clear all

Diameter = 0.2032;
densitySW = 1025;
Cd = 1.5;
Cd = 0.1;
G = 9.8;

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

CLinearize = 4.9;

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');

VelocityPlantTF = a / (s * ((mv * s) + b));

figure
rlocus(VelocityPlantTF)
title('Velocity Plant TF');

VelocityPlantPoles = pole(VelocityPlantTF)
VelocityPlantZeroes = zero(VelocityPlantTF)

m = 0;
n = 3;

l = n - m;

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

alpha = (sum(real(VelocityPlantPoles)) - sum(real(VelocityPlantZeroes))) / 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 * VelocityPlantTF;
% figure
% rlocus(DepthPID_TF)
% title('Depth PID TF');


