void model()
{
    //Uchyby (aktualny i poprzedni)
    uchyb_poprzedni = UchybWartosc;
    UchybWartosc = TempZadanaWartosc - temp_pokoju_wyjscie_2iner;

    // Sterowanie
    // Ca³kowanie
    czesc_calkujaca += (1 / Ti) * UchybWartosc;

    //Ro¿niczkowanie
    if (LicznikTd >= 10)
    {
        LicznikTd = 500; //wylaczyc licznik juz
        czesc_rozniczkujaca = (UchybWartosc - uchyb_poprzedni) * Td;
    }

    //Sygna³ steruj¹cy
    if (SygnalSterujacyWartosc < 100 && SygnalSterujacyWartosc > -100)
        SygnalSterujacyWartosc = Kp
                * (UchybWartosc + czesc_calkujaca + czesc_rozniczkujaca);
    else if (SygnalSterujacyWartosc >= 100)
        SygnalSterujacyWartosc = 100;
    else if (SygnalSterujacyWartosc <= -100)
        SygnalSterujacyWartosc = -100;

    //Obiekt klimazytatora
    ////0.607=T=2 // 0.90483=T=10 // 0.936=T=15 // 0.97183=T=35 // 0.9753=T=50
    //wyjscie_z_obiektu_1 = wyjscie_z_obiektu_1 * 0.936 + 2 * (1 - 0.936) * SygnalSterujacyWartosc; //aktualnie dane T=15; 0.607 = T=2
    wyjscie_z_obiektu_1 = wyjscie_z_obiektu_1 * 0.936 + 2 * (1 - 0.936) * TempZadanaWartosc;
    //Zaklocenie
    //zaklocenie = ((TempNaDworzeWartosc - temp_pokoju_wyjscie_2iner)) * OtwarcieOknaWartosc;
    zaklocenie = TempNaDworzeWartosc * OtwarcieOknaWartosc;

    //Pomiedzy pierwsza a druga inercja wchodzi zaklocenie
    wejscie_do_drugiego_obiektu = wyjscie_z_obiektu_1 + zaklocenie;

    //Obiekt pokoju
    temp_pokoju_wyjscie_2iner = temp_pokoju_wyjscie_2iner * 0.9753
            + (1 - 0.9753) * wejscie_do_drugiego_obiektu; //aktualnie (chyba) dane T=50; 0.936 = T=15
}


 while (1)
        {



            EnableRefresh = 1;
            LCD.Synchronize();
            EnableRefresh = 0;

            Key = KEYBOARD.GetKey();

            if(LicznikModel >= 100)
            {
                model();
                //for (int i = 498; i > -1; i--)
                UchybKolejka[499] = UchybWartosc;
                ZaklocenieKolejka[499] = zaklocenie;
                SygnalSterujacyKolejka[499] = SygnalSterujacyWartoscRounded;
                TempZadanaKolejka[499] = TempZadanaWartosc;
                temp_pokoju_wyjscie_2iner_Kolejka[499] = temp_pokoju_wyjscie_2iner;
                for (int i = 0; i < 499; i++)
                                {
                                    UchybKolejka[i] = UchybKolejka[i + 1];
                                    ZaklocenieKolejka[i] = ZaklocenieKolejka[i + 1];
                                    SygnalSterujacyKolejka[i] = SygnalSterujacyKolejka[i + 1];
                                    TempZadanaKolejka[i] = TempZadanaKolejka[i + 1];
                                    temp_pokoju_wyjscie_2iner_Kolejka[i] =
                                            temp_pokoju_wyjscie_2iner_Kolejka[i + 1];
                                }

                                /*UchybKolejka[499] = UchybWartosc;
                                ZaklocenieKolejka[499] = zaklocenie;
                                SygnalSterujacyKolejka[499] = SygnalSterujacyWartoscRounded;
                                TempZadanaKolejka[499] = TempZadanaWartosc;
                                temp_pokoju_wyjscie_2iner_Kolejka[499] = temp_pokoju_wyjscie_2iner;*/
                LicznikModel = 0;
            }



void Timer2Isr()
    {

#ifdef BUFFERSYNC
        if (EnableRefresh)
        LCD.PartialRefresh();
#else
        LCD.PartialRefresh();
#endif

        KEYBOARD.PartialRefresh();

        if (++preScale == 167)
        {
            preScale = 0;
            Tim++;

            if (LicznikModel < 100)
            {
                LicznikModel++;
            }
        }
    }