// Definición de pines de los motores
#define ENA 10//PWM Motor A
#define IN1 9
#define IN2 8
#define ENB 11//PWM Motor B
#define IN3 12
#define IN4 13

// Constantes del controlador PID
//#define Kp 1.0          // Constante proporcional
//#define Kv 0.1          // Constante de velocidad
//#define CONSTANT_V 200  // Velocidad máxima del motor

// Variables para motor A
//float error_posicion_1 = 0;
//float error_velocidad_1 = 0;
float velA;
float tauA = 0;

// Variables para motor B
//float error_posicion_2 = 0;
//float error_velocidad_2 = 0;
float velB;
float tauB = 0;

//Otras constantes
String aux, valStrA, valStrB;
int indx;
bool ledState = LOW;

void setup() {
  Serial.begin(9600);
  Serial.println("Control de motores en lazo abierto");
  //TCCR1B=TCCR1B&0xf8|0x01; // Pin9,Pin10 PWM 31250Hz
  //TCCR2B=TCCR2B&0xf8|0x01; // Pin3,Pin11 PWM 31250Hz
  //pinMode(IN1, OUTPUT);
  //pinMode(IN2, OUTPUT);
  //pinMode(IN3, OUTPUT);
  //pinMode(IN4, OUTPUT);
}

void loop() {

  // Retornar y actualizar al recibir datos
  if (Serial.available() > 0) {
    // Leer hasta salto de linea
    aux = Serial.readStringUntil('\n');//Serial.read()
    indx = aux.indexOf(',');
    // Dividir el string en dos partes
    valStrA = aux.substring(0, indx);
    valStrB = aux.substring(indx + 1);

    tauA = valStrA.toFloat();
    tauB = valStrB.toFloat();
    // Regresa los valores para verificación
    Serial.print("I received: ");
    Serial.print(tauA);
    Serial.print(", ");
    Serial.println(tauB);
  }

  // Limitar la velocidad entre -255 y 255 para motor A
  //if (tauA > 255)     tauA = 255;
  //else if (tauA < -255)  tauA = -255;
  tauA = constrain(tauA, -255, 255);

  // Controlar la dirección del motor A según la velocidad calculada
  if (tauA > 0) {
    digitalWrite (IN1, HIGH);
    digitalWrite (IN2, LOW);
    //Serial.println("Giro positivo");
  } 
  else if (tauA < 0) {
    digitalWrite (IN1, LOW);
    digitalWrite (IN2, HIGH);
    //Serial.println("Giro negativo");
  }
  else {
    digitalWrite (IN1, LOW);
    digitalWrite (IN2, LOW);
  }
  analogWrite (ENA, abs(tauA)); //Velocidad motor A

  // Limitar la velocidad entre -255 y 255 para motor B
  //if (tauB > 255)     tauB = 255;
  //else if (tauB < -255)  tauB = -255;
  tauB = constrain(tauB, -255, 255);

  // Controlar la dirección del motor 1 según la velocidad calculada
  if (tauB > 0) {
    digitalWrite (IN3, LOW);
    digitalWrite (IN4, HIGH);
  } 
  else if (tauB < 0) {
    digitalWrite (IN3, HIGH);
    digitalWrite (IN4, LOW);
  }
  else {
    digitalWrite (IN3, LOW);
    digitalWrite (IN4, LOW);
  }
  analogWrite (ENB, abs(tauB)); //Velocidad motor A

  // LED indicador
  ledState = !ledState;        // Alterna entre HIGH y LOW
  digitalWrite(LED_BUILTIN, ledState); // Actualiza el estado del LED

  // Imprimir información de depuración
  /*Serial.print("Motor A: Posición: ");
  Serial.print(posicion_actual_1);*/

  delay(1000);  // Esperar un tiempo antes de la próxima iteración
}