K = 1;
Ti = 0;
Td = 0;
Tt = sqrt(Ti * Td);
h = .01;
N = 8;
b = 1;
outputLoLimit = -10.0;
outputHiLimit = 10.0;

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;

lastDterm = 0;
Iterm = 0;
ItermOld = 0;

setPoint = 0;

lastY = 0;
Y = 0; 
for i = 1:100
    Pterm = K * (b * setPoint - Y);
    DTerm = ad *lastDTerm - bd * (Y - lastY);
    
    output1(i) = P * I * D;
    if ( output(i) > outputHiLimit )
        output2(i) = outputHiLimit;
    elseif (output < outputLoLimit )
        output2(i) = outputLoLimit;
    else
        output2(i) = output1;
    end
    I = I + bi*(setPoint - y) + a0 * (output2 - output1);\
    lastDTerm = DTerm;
    lastITerm = ITerm;
end

time = 0:h:99*h;
plot(time, output1, time, output2);