int kot = 0;
float kot_v_radianih;
float referencna_hitrost = cos(kot_v_radianih) * 100;

float pwm = 0;
int cas_med_pulzoma = 0.01;
int prejsnji_cas = 0;
float obrati_na_min = 0.01;
float napaka_prej = 0;

float Kp = 1.5;
float Ki = 0.002;
float Kd = 2;
float napaka = 0;

void setup() {
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  attachInterrupt(0, encoderISR, RISING);
  Serial.begin(9600);
}

void loop() {

  kot_v_radianih = (360/300) * kot * (3.14/360);

  obrati_na_min = 60. * 1000000./(cas_med_pulzoma*300.);

  int napaka_zdej = referencna_hitrost - obrati_na_min;
  float parameter_Kp = napaka_zdej * Kp;

  napaka = napaka + napaka_zdej;
  napaka = constrain(napaka, -100000, 100000);
  float parameter_Ki = Ki * (napaka);

  float parameter_Kd = napaka_zdej - napaka_prej;
  napaka_prej = napaka_zdej;
  pwm = parameter_Kp + parameter_Ki + parameter_Kd;
  pwm = constrain(pwm, 0, 200);

  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
  analogWrite(5, pwm);
  Serial.println(obrati_na_min);
}

void encoderISR () {
  kot++;
  int trenutni_cas = micros();
  cas_med_pulzoma = trenutni_cas - prejsnji_cas;
  prejsnji_cas = trenutni_cas;
  }