float wartosc_zadana=20;
float sterowanie=0;
#include "math.h"
float Kp=2;
float Ti=40;
float Td=0.15;
float uchyb_poprzedni = 0;
float uchyb = 0;
float temp_pokoju = 18;
float temp_zewnetrzna = 16;
float czesc_calkujaca = 0;
float czesc_rozniczkujaca = 0;
float sygnal_sterujacy = 0;
float zaklocenie = 0;
float otwarcie_okna = 1;
int licznik = 0;
float wyjscie_z_obiektu_1 = 0;
float wejscie_do_drugiego_obiektu = 0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

}

void loop() {
  // liczymy uchyb i  zapisujemy poprzedni uchyb
  uchyb_poprzedni = uchyb;
  uchyb = wartosc_zadana - temp_pokoju;
  // Sterowanie
  // Całkowanie
  if (czesc_calkujaca <= 70)
  {
    czesc_calkujaca += (1/Ti) * uchyb;
  }
  else
  {
    czesc_calkujaca = 70;
  }
  // Rożniczkowanie
  czesc_rozniczkujaca = (uchyb_poprzedni - uchyb)* Td;
  //Sygnał sterujący
  if (sygnal_sterujacy < 100)
  {
    sygnal_sterujacy = Kp * (uchyb + czesc_calkujaca + czesc_rozniczkujaca);
  }
  else
  {
    sygnal_sterujacy = 100;
  }

  //Obiekt klimazytatora
  wyjscie_z_obiektu_1 = wyjscie_z_obiektu_1 * 0.607 + 2* (1 - 0.607) * sygnal_sterujacy;

  // zaklocenie
  zaklocenie = ((temp_zewnetrzna - temp_pokoju)* 0.5) * otwarcie_okna;

  wejscie_do_drugiego_obiektu = wyjscie_z_obiektu_1 + zaklocenie;
  //Obiekt pokoju

  temp_pokoju = temp_pokoju * 0.936 + (1 - 0.936) * wejscie_do_drugiego_obiektu;

  if(licznik <= 100)//Wykonywane co 1 sek
  {
    Serial.println(String(temp_pokoju) + String(" ; ") + String(sygnal_sterujacy) + String(" ; ") +  String(zaklocenie));
    licznik = 0;
    delay(10);
  }
  licznik++;
}