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

int temperatura;
unsigned char Pwm_Set = 0;

int kp = 0.6;
int uchyb = 0;
int temperatura_zadana = 60;
int zaklocenie;


void Timer3ISR() {
  static boolean output = HIGH;

  if (temperatura_zadana == 0) digitalWrite(LED1, 0);
  else digitalWrite(LED1, 1);
  if (!digitalRead(SW1) & temperatura_zadana >= 5) temperatura_zadana -= 5;  // zmniejsz temp.

  if (temperatura_zadana == 65) digitalWrite(LED2, 0);
  else digitalWrite(LED2, 1);
  if (!digitalRead(SW2) & temperatura_zadana <= 60) temperatura_zadana += 5;  // zmniejsz temp.

  if (digitalRead(SW3)) digitalWrite(LED3, 1);
  else { // dodaj zakłócenie
    digitalWrite(LED3, 0);
    zaklocenie = rand() % 10;
    Serial.print("Zaklocenie ma wartosc: ");
    Serial.println(zaklocenie);
  }

  uchyb = temperatura_zadana - temperatura;
  Pwm_Set += uchyb;
  analogWrite(M1_EN, Pwm_Set);
  analogWrite(M2_EN, - Pwm_Set);
  analogWrite(M3_EN, Pwm_Set);
  analogWrite(M4_EN, -Pwm_Set);
}

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();
  float tempC = sensors.getTempC(sensor);
  Serial.println(String(temperatura) + String(" ; ") + String(tempC));
  Serial.print("Temperatura zadana: ");
  Serial.println(temperatura_zadana);


}