#include <PID_v1.h>
#include <prg_342.h>

#define PIN_INPUT A0
#define PIN_OUTPUT DAC0
#define K_FAKTOR 1.0995380532183
#define ALPHA 1

PRG_342 prg = PRG_342(K_FAKTOR);

//Define Variables we'll be connecting to
double Setpoint, Input, Output;
int angle;
int lastAngle;
unsigned long timer1;

//Specify the links and initial tuning parameters
double Kp = 0, Ki = 0.04, Kd = 0;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup() {prg.setOutput(4095*0.5, false);
  delay(3000);
  prg.setOutput(0, false);
  delay(2000);
  Serial.begin(115200);

  angle = prg.getAngle();
  Input = prg.getTorque(true, ALPHA);
  prg.updateActOut(Input);
  Setpoint = 15000;

  //turn the PID on
  myPID.SetOutputLimits(0, 4095);
  myPID.SetMode(AUTOMATIC);
}

void loop() {
  angle = prg.getAngle();
  Input = abs(prg.getTorque(true, ALPHA));
  prg.updateActOut(Input);

  if (SPEEDCHECK() >= 400) {
    myPID.Compute();
  }

  prg.setOutput(Output, false);
}

float SPEEDCHECK() {
  // Überprüfen der Rotationsgeschwindigkeit
  if (millis() - timer1 >= 200) {
    timer1 = millis();
    int deltaAngle = angle - lastAngle;
    lastAngle = angle;
    return abs(deltaAngle * 5);  // Umrechnen in Winkel pro Sekunde
  }
}