dt = 0.01;
t = 0:dt:25;
w_zadana = 26;

H = 6;
Y = 0;
STAN = 0;

tab_y = zeros(length(t)-1, 1);
tab_y(1) = 0;
tab_q = zeros(length(t), 1);

u=5;

for i=1:length(t)-1
    tab_y(i+1) = tab_y(i)+dt*(u-3*tab_y(i));
    Y = tab_y(i+1);

    sum = 0;
    for j=1:length(tab_y)
        sum = sum + ((w_zadana - tab_y(i))^2)*dt;
    end
    tab_q(i) = sum;

    if (Y < (w_zadana-(H/2)))
        u = 5.1*w_zadana;
        STAN = 1
    end
    if (Y > (w_zadana+(H/2)))
        u = -0.1*w_zadana;
        STAN = 2
    end
    if ((Y > w_zadana && STAN == 1) || (Y < w_zadana && STAN == 2))
        u = 90;
        STAN = 0
    end

end

plot(t, tab_q)