clear;
close all;

K = 10000000;
b = 1;
Td = 100;
Ti = 100;
Tt = sqrt(Ti * Td);
N = 5;
h = .01;

Tf = Td / N;
p1 = K * b;
p2 = K + K * Td / (Tf + h);
p3 = Tf / (Tf + h);
p4 = K * Td * h / ((Tf + h) * (Tf + h));
p5 = K * h / Ti;
p6 = h / Tt;

Bi = K * h / Ti;
Ad = (2 * Td - N * h) / (2 * Td + N * h);
Bd = 2 * K * N * Td / (2 * Td + N * h);
A0 = h / Tt;

uLow = -2;
uHigh = 2;

GearRatio = 100.0;
LSPitch = 0.1; %inches/ rev
Kv = 42 * 60.0 / 1000.0;    %V/RPS

Ysp = 0;
x = 0;
I = 0;
I2 = 0;
D = 0;
y(1) = 0;
yold = 0;

for i=1:1000000
    if(i == 10)
        Ysp = 1.0;
    end
    time(i) = i * h;
    
    v(i) = (p1 * Ysp) - (p2 * y(i)) + x + I;
    if v(i) > uHigh
        u(i) = uHigh;
    elseif v(i) < uLow
        u(i) = uLow;
    else
        u(i) = v(i);
    end
    
    x = (p3 * x) + (p4 * y(i));
    I = I + p5*(Ysp - y(i)) + p6*(u(i) - v(i));
    
    P = K * (b * Ysp - y(i));
    D = Ad * D - Bd.*(y(i) - yold);
    
    v2(i) = P + I2 + D;
    
    if v2(i) > uHigh
        u2(i) = uHigh;
    elseif v2(i) < uLow
        u2(i) = uLow;
    else
        u2(i) = v2(i);
    end
    I2 = I2 + Bi*(Ysp - y(i)) + A0*(u2(i) - v2(i));
    yold = y(i);   
    y(i+1) = y(i) + (u(i) * (1/Kv) * h * LSPitch * (1/GearRatio));
    
end
plot(time, u, time, u2)
figure
plot(y)

        
    