% =========================================================================
% AUTHOR       : Brian Ha  
% MENTOR       : Gene Massion
% DATE CREATED : 2018-07-11
% LAST REVISED : 2018-07-23
% MBARI GROUP  : Chemical Sensors Lab
% PROJECT      : Coastal Profiling Float (CPF) Depth Control
% =========================================================================

% Clean Up
close all
clear all
clc


% Float Parameters
% =========================================================================
floatLength = 1.016;        % [m] This is equal to 40 in.
floatDiameter = 0.2032;     % [m] This is equal to 8 in.
PlatformArea = (pi/4) * floatDiameter^2; % [m^2]. This the frontal area of a cylinder.

PlatformMass = 38;          % [kg]
VirtualMassFactor = 1.25;
PlatformVirtualMass = PlatformMass * VirtualMassFactor;  % [kg]

pumpDisplacement = 1.56e-7; % [m^3/rev] Per the Oildyne datasheet
pumpEff = 0.6;


% Constraints
% =========================================================================
upperMotorSpeedLimit = 50;   % [revs/sec] This is equal to 3000 rpm.
lowerMotorSpeedLimit = -50;  % [revs/sec]


% Assumptions
% =========================================================================
densitySW = 1025;      % [kg/m^3] This is for seawater. Ignores compressibility.
g = 9.807;             % [m/s^2] Gravitational acceleration.
velFreeStream = 0.1;   % [m/s] This is equal to 10 cm/s.
kinematicViscosity = 13.60e-7; % [m^2/s]. Taken from http://web.mit.edu/seawater/2017_MIT_Seawater_Property_Tables_r2a.pdf

CLinearize = 0.1;


% Estimation of drag coefficient range
% =========================================================================
Re = velFreeStream*floatDiameter/kinematicViscosity; % Just checking Reynolds Number...

% Cd min is interpolated below from the values (based on frontal area) given for a
% flat-faced cylinder on Table 7.3 (page 491) of Fluid Mechanics, 7th Ed.
% by Frank M. White. Copyright 2011. Available at
% https://hellcareers.files.wordpress.com/2016/01/fluid-mechanics-seventh-edition-by-frank-m-white.pdf.

CdMin = (0.99 - 0.87)/(8 - 4) * (floatLength/floatDiameter - 4) + 0.87;

% Cd max was assumed from Laughlin Barker's paper (2014 MBARI intern).
CdMax = 1.5;

Cd = CdMax;  % CdMax or CdMin


% Plant
% =========================================================================
s = tf('s');
a = pumpDisplacement * pumpEff * densitySW * g;
b = 0.5 * densitySW * Cd * PlatformArea * CLinearize;
m = PlatformVirtualMass;

VelocityPlantTF = a / (s * ((m * s) + b));
DepthPlantTF = VelocityPlantTF * (1/s);

x_eq = [0;0.1;0];
u_eq = 0;

[Aeq, Beq] = symLin(x_eq, u_eq, densitySW, Cd, PlatformArea, g, m, pumpDisplacement, pumpEff, CLinearize);
C = [1 0 0];
D = 0;

% Plant Linearized at Equilibrium Point
P = ss(Aeq, Beq, C, D);


% Kalman Controllability Test
% =========================================================================
disp("Kalman Controllability Test")
disp(rank(ctrb(P)))


% Kalman Observability Test
% =========================================================================
disp("Kalman Observability Test")
disp(rank(obsv(P)))


% Compensator
% =========================================================================

%{
Q = [1 0 0; 0 1 0; 0 0 1];
R = 1;

[K, ~, ~] = lqr(Aeq, Beq, Q, R);

Asf = Aeq-Beq*K;
Bsf = Beq;

Psf = ss(Asf, Bsf, C, D);
%}
s = tf('s');
C = 67.669*(s+0.01024)*(s+0.007218)/((s+0.09556)*(s+0.1144));
%C2 = 72.299*(s+0.01384)*(s+0.001968)/((s+0.09556)*(s+0.1144));
C2 = 65.143*(s+0.014)*(s+0.002)*(s+0.0015)/((s+0.114)*(s+0.096)*(s));
     
% 2018-07-25
%C = 55*(s+0.015)*(s+0.02)/((s+0.1)*(s+0.15));
G1 = 0.01;  % Reference Feedforward Gain

% Transfer Functions
% =========================================================================

% Dual Lead
L = tf(P)*C;            % Open-Loop Transfer Function
S = minreal(1/(1 + L)); % Sensitivity Function
T = minreal(1 - S);     % Complementary Sensitivity Function

L2 = tf(P)*C2;
S2 = minreal(1/(1 + L2));
T2 = minreal(1 - S2);

% Dual Lead with Reference Feedforward
Tyr_ff = S*tf(P)*(G1+C);
Tur_ff = S*(G1+C);

% Dual Lead with Pre-Compensation and Reference Feedforward
G2 = inv(dcgain(Tyr_ff));  % Pre-compensator
Tyr_pre_ff = S*tf(P)*(G1+C*G2);
Tur_pre_ff = S*(G1+C*G2);


% Plots
% =========================================================================
figure(1)

stepSize = 20;  % [m]
opt = stepDataOptions('StepAmplitude', stepSize');

subplot(2,2,1)
step(T, opt)
[y, t] = step(T, opt);
hold on
%step(Tyr_ff, opt, 'g')
%step(Tyr_pre_ff, opt, 'y')
step(T2, opt)
%plot(t, 1.25*ones(1, length(t)), 'r--')
plot([100:5:t(end)], 1.05*stepSize*ones(1, length([100:5:t(end)])), 'r--')
plot([100:5:t(end)], 0.95*stepSize*ones(1, length([100:5:t(end)])), 'r--')
%legend('Dual Lead', 'Dual Lead + FF', 'Dual Lead + FF + Pre', 'Optimized', 'location', 'southeast');
legend('Dual Lead', 'Dual Lead + Lag', 'location', 'southeast')
grid on
title('Command and Noise Response')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)

subplot(2,2,2)
step(S, opt)
hold on
step(S2, opt)
legend('Dual Lead', 'Dual Lead + Lag')
grid on
title('Output Disturbance Rejection')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)


subplot(2,2,3)
step(C*S, opt)
[y, t] = step(C*S);
hold on
%step(Tur_ff, opt, 'g')
%step(Tur_pre_ff, opt, 'y')
step(C2*S2, opt);
plot(t, upperMotorSpeedLimit*ones(1, length(t)), 'r--')
plot(t, lowerMotorSpeedLimit*ones(1, length(t)), 'r--')
%legend('Dual Lead', 'Dual Lead + FF', 'Dual Lead + FF + Pre', 'Optimized');
legend('Dual Lead', 'Dual Lead + Lag')
grid on
title('Control Signal Response')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)


subplot(2,2,4)
step(S*P, opt)
hold on
step(S2*P, opt)
grid on
legend('Dual Lead', 'Dual Lead + Lag')
title('Input Disturbance Rejection')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)


figure(2)
options = bodeoptions('cstprefs');
options.FreqUnits = 'Hz';
bode(L, options)
hold on
bode(L2)
grid on
title('Open Loop TF Bode Plot')
legend('Dual Lead', 'Dual Lead + Lag')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)

%{
figure(4)
nyquist(L)

% Draw a black unit circle
circle = -1+ (-s+1)/(s+1);
omegac = logspace(-3,3,400);
[reC,imC] = nyquist(circle,omegac);
reC = squeeze(reC);
imC = squeeze(imC);
hold on
plot(reC,imC,'k-',reC,-imC,'k-')

axis([-3 3 -3 3])
axis('equal')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
title('Nyquist Plot')


figure(5)
bode(S)
hold on
bode(T)
grid on
title('Sensitivity and Comp. Sensitivity Functions')
legend('S', 'T')
set(findall(gcf, 'type', 'line'), 'linewidth', 2)
%}


% Simulink Simulations

tFinal = 4000;
sim('CPF_PlatformDepthModelLinear')

%%
function [A, B] = symLin(x_eq, u_eq, densitySW, Cd, area, grav, mass, pumpDisplacement, pumpEff, c_lin)
% Calculates the Jacobian A and B matrices about x_eq and u_eq

% Part I of II: Symbolic Calculation of General Jacobian

% Symbolic State Vector
position = sym('position');
velocity = sym('velocity');
delta_volume = sym('volume');

x = [position; velocity; delta_volume];

% Symbolic Input
motor_speed = sym('motor_speed'); % [rev/s]

u = [motor_speed];

% Non-Linear ODEs
x_dot(1,1) = velocity;
x_dot(2,1) = (-0.5*densitySW*area*Cd*velocity*c_lin + densitySW*grav*delta_volume)/mass;
x_dot(3,1) = motor_speed*pumpDisplacement*pumpEff;

A = jacobian(x_dot, x);
B = jacobian(x_dot, u);

% Part II of II: Calculation of Jacobian at x_eq and u_eq
position = x_eq(1);
velocity = x_eq(2);
delta_volume = x_eq(3);

motor_speed = u_eq;

A = eval(subs(A));
B = eval(subs(B));

end