close all

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

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

CLinearize = 4.986;

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

pumpDisplacement = 1.56*10^-7;
pumpEff = 0.6;

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

s = tf('s');
VelocityPlantTF = a / (s * ((m * s) + b));
VelocityPlantTF2 = (a/m) / ((s^2 + (b/m)*s));
aOverm = a/m
bOverm = b/m

poles = [0 -b/m]
figure
rlocus(VelocityPlantTF)
title('Velocity Plant TF');

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

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

VelocityPID_TF = PID_TF * VelocityPlantTF;
figure
rlocus(VelocityPID_TF)
title('Velocity PID TF');

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

zeta = 0.7;
Wn = 0.002;

sgrid(zeta, Wn)

