clear all
close all

mass = 40;      % kg
area = .1018;   % m^2
Cd = 1.5;       % drag coef
cLin = .1;      % const for linearizing velocity
g = 9.8;        % m/s^2
swDensity = 1027;   %kg/m^3

motorVol = .862;    % cm^3/rev
motorSpeed = 1000;  % counts/sec
q = motorVol * motorSpeed * 1/2000 * (.01^3);

A1 = mass * g;
B1 = .5 * swDensity * Cd * area * cLin;

s = tf('s');
plantTF = A1 / ((mass*s^2) + (B1*s) + 0)
poles = pole(plantTF)

num = A1;
den = [mass B1 0];

plantTF2 = tf(num, den);

step(plantTF*q)
title('Plant Step Response');

figure
impulse(plantTF)
title('Plant TF');

figure 
pzmap(plantTF)
title('Plant Poles');

figure
bode(plantTF, {10^-3 10^2})
title('Plant Bode Plot');

figure
margin(plantTF)
[gainMargin,phaseMargin,wGainCross,wPhaseCross] = margin(plantTF)
title('Plant Gain and Phase Margin');

Kp = .4;
Kd = 1;
Ki = 0;
Tf = 1;
matlabPID_TF = pid(Kp, Ki, Kd, Tf)

K = .3;
Td = 3;
Tf = 1;
Ti = 0;
ahTF = K*(1 + (1/s*Ti) + (s*Td)) * (1 / (1 + (s*Tf) + (((s*Tf)^2)/2) ))

systemTFMatlabPID = feedback(matlabPID_TF*plantTF,.01);

t = 0:.01:100;
figure
step(systemTFMatlabPID,t)
title('Controller Step Response');
hold on

systemTF_AH = feedback(ahTF * plantTF, .01);
t = 0:.01:100;
step(systemTF_AH,t)
legend('Matlab PID', 'AH PID');

figure
rlocus(systemTFMatlabPID)
title('System Root Locus, Matlab PID');

figure
rlocus(systemTF_AH)
title('System Root Locus, AH PID');



