K = 1;
b = 1;
Td = .1;
Ti = .1;
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 = -100;
uHigh = 100;

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

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

for i=1:100
    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 + I + D;
    
    if v2(i) > uHigh
        u2(i) = uHigh;
    elseif v2(i) < uLow
        u2(i) = uLow;
    else
        u2(i) = v2(i);
    end
    I = I + Bi*(Ysp - y(i)) + A0*(u2(i) - v2(i));
    yold = y;   
    y(i+1) = u(i) * (1/Kv) * h * LSPitch * (1/GearRatio);
    
end
plot(time, u, time, u2)
figure
plot(time, y)