//PID - Base

const int encoderPin1 = 25; //Encoder
int cont = 0; //pulsos del encoder
const int pwmPin = 14; //control PWM del motor
int vueltas = 0;
int PWMVal = 0;
//tiempo
unsigned long previo = 0;
int intervalo = 1000;
//revoluciones
volatile float rpm = 0;
//Referencia
float setpoint = 0; 

//Param PID
float proporcional = 1.5; //k_p = 0.5
float integral = 2.5; //k_i = 3
float derivada = 0.01; //k_d = 1
float controlSignal = 0; 

float tiempoPrev = 0; 
float errorPrev = 0; 
float errorIntegral = 0; 
float tiempoAct = 0; 
float deltaT = 0;
float errorVal = 0; 
float derivE = 0; 
void setup()
{
  Serial.begin(115200);
  
  pinMode(encoderPin1, INPUT); 
  attachInterrupt(digitalPinToInterrupt(encoderPin1), Encoder, RISING);
}

void loop()
{

  velocidad();

  PID();

  Motor();
  
}

void Encoder()
{
  cont++;
}

void velocidad(){
   unsigned long actual = millis();
  if ((actual - previo) >= intervalo) {
    rpm = (cont * 60) / 234;
    cont = 0;
    previo = actual;
  }
}
void PID()
{
  
  tiempoAct = micros(); //tiempo actual
  deltaT = (tiempoAct - tiempoPrev) / 1000000.0; //Diferencia de tiempo en s
  tiempoPrev = tiempoAct; //Guarda tiempo prev
  errorVal = rpm - setpoint; //comparador para error

  derivE = (errorVal - errorPrev) / deltaT; //derivada del error

  errorIntegral = errorIntegral + (errorVal * deltaT); //integral del error
  controlSignal = (proporcional * errorVal) + (derivada * derivE) + (integral * errorIntegral); //PID
  errorPrev = errorVal; //Guardar error previo para la derivada
}

void Motor()
{
  
  PWMVal = (int)fabs(controlSignal); //floating point absolute value (positivo y entero)
  if (PWMVal > 255) 
  {
    PWMVal = 255; 
  }

  if (PWMVal < 60 && errorVal != 0)
  {
    PWMVal = 60; //Señal mínima de respuesta
  }
 
  analogWrite(pwmPin, PWMVal); // Escribir al motor

//Mostrar en pantalla  
    Serial.print(errorVal);
    Serial.print(" ");
    Serial.print(PWMVal);
    Serial.print(" ");
    Serial.print(setpoint);
    Serial.print(" ");
    Serial.print(rpm);
    Serial.println();
  
}