#define Pmax 1023.0
#define Kp 2.0
#define Kd 0.016


float Ki = 0.03;
float dt = 100;
float delta = 0.0;
float A = 1.0;
float t = 1.0;
float zadnjeA = 0.0;
float MjStara = 0.0;
float Spoint = 0.0;
float crv = 0.0;
float plv = 0.0;
unsigned long proslemili = 0;
const long interval = 1000;


void setup() {
  Serial.begin(9600);
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
}


void loop() {
  unsigned long mili = millis();


  if (mili - proslemili >= interval) {
    mili = proslemili;


    float Mjerena;
    Mjerena = analogRead(0);
    Spoint = analogRead(1);
    float dy = MjStara - Mjerena;
    delta = ((Spoint - Mjerena) / Pmax) * 100;
    float KP = Kp * delta;
    float KI = Ki * delta * t;
    float KD = (dy / dt) * Kd;
    MjStara = Mjerena;


    A = KP + KI + KD;
    t++;


    if (zadnjeA * A < 0) {
      t = 0;
    }
    zadnjeA = A;


    if (A > 100) {
      A = 100.0;
    } else {
      if (A < -100) {
        A = -100.0;
      }
    }


    plv = map(A, -100, 0, 2000, 50);
    if (A < 0) {
      digitalWrite(12, HIGH);
      delay(plv);
      digitalWrite(12, LOW);
      delay(2000 - plv);
    } else {
      digitalWrite(12, LOW);
    }
    crv = map(A, 0, 100, 50, 2000);
    if (A > 0) {
      digitalWrite(13, HIGH);
      delay(crv);
      digitalWrite(13, LOW);
      delay(2000 - crv);
    } else {
      digitalWrite(13, LOW);
    }


    Serial.println("t:" + String(t) + " MV:" + String(Mjerena) + " SP:" + String(Spoint) + " A:" + String(A) + "%" + " KP:" + String(KP) + " KI:" + String(KI) + " KD:" + String(KD));
  }
}