/********************************************************
   Drehzahlregelung und Gasbetätigung über Seilwindenfunk
   Fendt Farmer 309 C
   Jonas Geier
   01.2024

   Arduino Micro

 ********************************************************/

#include <PID_v1.h>
#include <Servo.h>
Servo myservo;

#define PIN_INPUT 2   //Drehzahlsensor
#define PIN_OUTPUT 9  //Servo

int analogPin = A0;

//Definition der Konstanten
const int Servo_max = 80;        // [steps] maximaler Servowert
const int Servo_min = 180;       // [steps] minimaler Servowert
const int Todweg = 157;          // [steps] Servowert, bis erste Reaktion auf Drehzahl erfolgt
const int Threshold_cons = 15;  // [RPM]   Schwellwert ab wann konservativer Regler übernimmt (Differenz zwischen Soll und Ist-Drehzahl)
const int Threshold_off = 50;   // [RPM]   Schwellwert ab wann Regelung aktiviert werden kann (Setpoint - Threshold_off) bzw. schaltet ab, wenn Drehzahl um diesen Wert vom Sollwert entfernt ist
const int Threshold_off2 = 10;  // [RPM]   Schwellwert ab wann Regelung deaktiviert wird, wenn Zapfwelle während des Betriebs ausgekuppelt wird

//Reglervariablen
double Setpoint, Input;
double Output = Servo_min;
double Output_1 = Servo_min;

//Define the aggressive and conservative Tuning Parameters
double aggKp = 0.3, aggKi = 0.35, aggKd = 0;      //Wenn weit weg vom Sollwert
double consKp = 0.3, consKi = 2.3, consKd = 0;  //Wenn nahe am Sollwert

PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, P_ON_M, DIRECT);

// Seilwindenfunk
int Gas = Servo_min;
bool halten = false;


// Drehzahlsensor
int period = 0;
int lastInt = 0;
int Drehzahl = 0;


int rpm_backwheel() {          /* this code will be executed every time the interrupt 0 (pin2) gets low.*/
  period = millis() - lastInt;  //Periodendauer zu letztem Impuls
  lastInt = millis();
  return period;
}


void setup() {
  Serial.begin(9600);
  myservo.attach(9);                          // Servosignal an Pin 9
  pinMode(2, INPUT);                          // Drehzahlsensor-Signal an Pin 2
  attachInterrupt(0, rpm_backwheel, RISING);  //interrupt 0 is on pin 2 Zeitmessung //muss mit RISING ersetzt werden

  myPID.SetOutputLimits(Servo_max, Servo_min);  //Limit alter Servo: 40, 180    //neuer Servo Limit max: 80 min 180
  myPID.SetControllerDirection(REVERSE);

  Setpoint = 70;  //Standard Setpoint

  // Taster
  pinMode(13, INPUT_PULLUP);
  pinMode(12, INPUT_PULLUP);

  // Relais
  pinMode(4, INPUT_PULLUP);  // Relais Drehzahl Hoch (rot)
  pinMode(5, INPUT_PULLUP);  // Relais Drehzahl runter, Standgas (schwarz)

}


void loop() {
  
  Drehzahl = 60000 / (period * 6);  //Umrechung von millis in Umdrehungen pro Minute
  int Reglerabweichung = abs(Setpoint - Drehzahl);
  //Serial.println(Drehzahl);
  
  // Aktivieren der Regelung mit dem "orangen" Schalter und langsames hochfahren der Drehzahl
  if (digitalRead(13) == LOW) {
    // Verhindert das der Regler aktiviert werden kann, wenn die Zapfwelle nicht eingekuppelt ist
    while (Drehzahl < (Setpoint - Threshold_off)) {    //if würde zur not auch gehen, aber nicht ganz sauber, Servo könnte zappeln!
      myPID.SetMode(MANUAL);
      myservo.write(Servo_min);
      Output_1 = Servo_min;
      Drehzahl = 60000 / (period * 6);
      Serial.println(Drehzahl);
    }

    while (Output_1 >= Todweg) {    //zusätzliche Hilfsvariable erstellen, da Output im Regler immer wieder verändert wird --> Springt immer wieder in while-Schleife rein
      Output_1--;                   //Überbrücken des Todwegs
      myservo.write(Output_1);
      //Serial.print(Output_1);
    }


    if (Reglerabweichung <= Threshold_cons) {
      myPID.SetTunings(consKp, consKi, consKd);
      Serial.println("Kons");
    } else {
      myPID.SetTunings(aggKp, aggKi, aggKd);
      Serial.println("Agg");
    }

    /*while (Drehzahl < (Setpoint - Threshold_off2)) {    // Abschalten, wenn Zapfwelle ausgeschaltet wird (knapper wählen wie Treshold_off)
      myPID.SetMode(MANUAL);
      myservo.write(Servo_min);
      Output_1 = Servo_min;
      Drehzahl = 60000 / (period * 6);
      Serial.println(Drehzahl);
    }*/
    myPID.SetMode(AUTOMATIC);
  }

  //Regelung ausschalten
  if (digitalRead(13) == HIGH) {
    myPID.SetMode(MANUAL);
    Output = Servo_min;
    Output_1 = Servo_min;
    myservo.write(Output);
    Serial.println("Aus");
  }

  // Abspeichern der aktuellen Drehzahl mit dem gelben Taster
  if (digitalRead(12) == LOW) {
    Setpoint = Drehzahl;
  }

  Input = Drehzahl;
  myPID.Compute();
  myservo.write(Output);

//--------------------------------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------//
  // Aktivierung des Servos über die Funkfernbedienung
  while (digitalRead(4) == LOW) {
    Gas--;
    if (Gas > Servo_min) {
      Gas = Servo_min;
    }
    if (Gas < Servo_max) {
      Gas = Servo_max;
    }
    Output = Gas;
    myservo.write(Gas);
    halten = true;
    myPID.SetMode(MANUAL);
    delay(100);
  }

  // Gas wird gehalten nachdem Schalter losgelassen wurde
  while (halten == true && digitalRead(4) == HIGH && digitalRead(5) == HIGH) {
    Output = Gas;
    myservo.write(Gas);
    myPID.SetMode(MANUAL);
  }
  // Gas wird weggenommen wenn "Schildkröten" Taste betätigt wird
  if (digitalRead(5) == LOW) {
    myservo.write(Servo_min);
    Gas = Servo_min;
    Output = Servo_min;
    halten = false;
    myPID.SetMode(MANUAL);
  }

}