int motorPWM = 14; // Pin del PWM para controlar la velocidad del motor
int motorIN1 = 12; // Pin de dirección 1 del motor
int motorIN2 = 13; // Pin de dirección 2 del motor
int potDeseadaPin = 34; // Pin del potenciómetro para la posición deseada
int potActualPin = 35; // Pin del potenciómetro para la posición actual
double kp = 1.0; // Ganancia proporcional del controlador PID
double ki = 0.1; // Ganancia integral del controlador PID
double kd = 0.02; // Ganancia derivativa del controlador PID
unsigned long lastUpdateTime = 0; // Variable para el tiempo de la última actualización
int lastError = 0; // Variable para almacenar el error en el ciclo anterior
double integral = 0; // Variable para la componente integral del controlador PID
int minValue = 0; //valor minimo del potenciometro calibrado 
int maxValue = 4095; //valor maximo del potenciometro calibrado 
bool calibrated = false; //flag para saber si el potenciometro ha sido calibrado 



void setup() {
  pinMode(motorPWM, OUTPUT);
  pinMode(motorIN1, OUTPUT);
  pinMode(motorIN2, OUTPUT);
  pinMode(potDeseadaPin, INPUT);
  pinMode(potActualPin, INPUT);
  Serial.begin(9600);
  calibrarPotenciometro();
}
 void calibrarPotenciometro() {
  int minVal = analogRead(potActualPin);// Inicializar minVal con un valor de potActualPin
  delay(500); 
  int maxVal = 0; // Inicializar maxVal con un valor bajo
  digitalWrite(motorIN1, HIGH); // MotorIN1 = 0
  digitalWrite(motorIN2, LOW); // MotorIN2 = 0
  analogWrite(motorPWM, 160); // MotorPWM = 0
  delay(500); // Esperar un segundo para que el potenciómetro se estabilice
  for (int i = 0; i < 100; i++) {
    int val = analogRead(potActualPin);
    if (val > maxVal) {
      maxVal = val;
    }
    delay(10);
  }
  minValue = minVal;
  // Set minValue as the minimum value obtained
  maxValue = maxVal;
  // Set maxValue as the maximum value obtained
  calibrated = true;
}

void loop(){
  if (!calibrated){
    calibrarPotenciometro();
  }
  if (millis() - lastUpdateTime >= 50) {
    lastUpdateTime = millis();
    int posicionDeseada = map(analogRead(potDeseadaPin), 0, 4095, 0, 255);
    int posicionActual = map(analogRead(potActualPin), minValue, maxValue, 0, 255);
    
    double error = posicionDeseada - posicionActual;
    integral = integral + error;
    double derivativa = error - lastError;
    lastError = error;
    
    double correccion = (kp * error) + (ki * integral) + (kd * derivativa);
    int pwmValue = abs(correccion);
    
    if (correccion > 0) {
      analogWrite(motorPWM, pwmValue);
      digitalWrite(motorIN1, HIGH);
      digitalWrite(motorIN2, LOW);
    } else if (correccion < 0) {
      analogWrite(motorPWM, pwmValue);
      digitalWrite(motorIN1, LOW);
      digitalWrite(motorIN2, HIGH);
    } else {
      analogWrite(motorPWM, 0);
      digitalWrite(motorIN1, LOW);
      digitalWrite(motorIN2, LOW);
    }
    Serial.print("Potenciómetro actual: ");
    Serial.print(posicionActual);
    Serial.print(" | Valor PWM: ");
    Serial.println(pwmValue);
  }
}