#include "labduino.h"
#include <TimerThree.h>

#define ON 1
#define OFF 0

//Zmienne do temperatury:
int temperatura; //odczyt z analogowego
float tempC; //odczyt z cyfrowego
int temp_zadana = 40; //temperatura zadana

//Zmienne do regulatora PI
float wyjscie_regP; //wynik regulatora P
float wyjscie_regI; //wynik regulatora I
float suma_uchybow = 0;
int kp = 2; //nastawa wzmocnienia regulatora P
int ki = 1; //nastawa wzmocnienia regulatora I
float uchyb; //roznica temp_zadana - tempC
float sygnal_sterujacy;
int podstawa_calkowania = 0.1;
int wypelnienie_zaklocenia = 0;

//Inne zmienne
int licznik_na_pol_sek = 0; //do odliczania 0.5 sek w przerwaniu Timer3ISR
unsigned char Pwm_Set = 0; //do reprezantacyjnego programu do mrugania diodami

void Timer3ISR() {
  //static boolean output = HIGH; //do mrugania zielona dioda w przycisku trzecim

  //INFORMOWANIE O UCHYBIE
  if (uchyb < 0.1*temp_zadana)
  {
    digitalWrite(LED3, ON);
    digitalWrite(LED1, OFF);
    digitalWrite(LED2, OFF);
  }
  if (uchyb > 0.1*temp_zadana)
  {
    digitalWrite(LED3, OFF);
    digitalWrite(LED1, ON);
    digitalWrite(LED2, ON);
  }

  //REGULATOR PI
  uchyb = temp_zadana - tempC;
  suma_uchybow+=uchyb;
  wyjscie_regI = ki*suma_uchybow;
  if(wyjscie_regI > 255)
  {
    wyjscie_regI = 255;
  }
  //REGULATOR P

   wyjscie_regI = kp * uchyb;
   if(wyjscie_regI > 255)
   {
    wyjscie_regI = 255;
   }


  //STEROWANIE
   sygnal_sterujacy = wyjscie_regP + wyjscie_regI;
   if(sygnal_sterujacy > 255)
   {
    sygnal_sterujacy = 255;
   }
   else if (sygnal_sterujacy < 0)
   {
    sygnal_sterujacy = 0;
   }


  if(uchyb > 0)
  {
      analogWrite(M1_EN, sygnal_sterujacy);
      analogWrite(M2_EN, 0);
  }
  else if(uchyb < 0)
  {
      analogWrite(M1_EN, 0);
      analogWrite(M2_EN, sygnal_sterujacy);
  }

  //    ZAKLOCENIE
  if (!digitalRead(SW3))
  {
      wypelnienie_zaklocenia = 255;
      analogWrite(M3_EN, wypelnienie_zaklocenia);
  }
  else
  {
      analogWrite(M3_EN, 0);
  }

  //ZMIANA TEMPERATURY ZADANEJ - LEWY PRZYCISK ZWIEKSZANIE / PRAWY PRZYCISK ZMNIEJSZANIE
  if (!digitalRead(SW1)) //przycisk pierwszy - zwiekszanie temperatury zadanej
  {
    licznik_na_pol_sek++; //inkrementuj licznik
    if (licznik_na_pol_sek == 5) //jezeli odliczymy 0.5 sek
    {
      temp_zadana++; //zwieksz o jeden
      licznik_na_pol_sek = 0; //zeruj licznik
    }
  }
  else
  {
    //do nothing
  }

  if (!digitalRead(SW2)) //przycisk drugi - zmniejszanie temperatury zadanej
  {
    licznik_na_pol_sek++; //inkrementuj licznik
    if (licznik_na_pol_sek == 5) //jezeli odliczymy 0.5 sek
    {
      temp_zadana--; //zwieksz o jeden
      licznik_na_pol_sek = 0; //zeruj licznik
    }
  }
  else
  {
    //do nothing
  }

  //do mrugania zielona dioda w przycisku trzecim:
  //digitalWrite(LED3, output); //miganie zielonym przyciskiem
  //output = !output;



}

void setup() {
  labduinoSetup();
  sensors.begin();
  Timer3.initialize(100000);
  Timer3.attachInterrupt(Timer3ISR);

  Serial.begin(9600);
  if (!sensors.getAddress(sensor, 0))   Serial.println("Nie znaleziono chipu DS18B20");
}

void loop() {
  temperatura = analogRead(LM35);
  sensors.requestTemperatures();
  tempC = sensors.getTempC(sensor);
  Serial.println(String(temperatura) + String(" ; ") + String(tempC) + String(" ; ") +  String(temp_zadana));

}